1. THEORYKalman Filter is a state estimator which is an adaptive least square error filter that provides an efficient computational recursive solution for estimating a signal in presence of Gaussian noises. It is an algorithm which makes optimal use of imprecise data on a linear (or nearly linear) system with Gaussian errors to continuously update the best estimate of the system's current state.Kalman filter theory is based on a state-space approach in which a state equation models the dynamics of the signal generation process and an observation equationmodels the noisy and distorted observation signal. For a signalx(k) and noisy observationy(k), equations describing the state process modeland the observation model are defined as

x(k)=Ax(k-1)+w(k-1)(1)y(k)=Hx(k)+n(k) (2)wherex(k) is theP-dimensional signal vector, or the state parameter, at timek,Ais aPPdimensional state transition matrix that relates the states of the process at timesk 1 andk,w(k) (process noise) is theP-dimensional uncorrelated input excitation vector of the state equation.w(k) is assumed to be a normal (Gaussian) processp(w(k))~N(0,Q),Qbeing thePPcovariance matrix ofw(k) orprocess noise covariance.y(k) is theMdimensional noisy observation vector,His aMPdimensional matrixn(k) is theM-dimensional noise vector, also known as measurement noise,n(k) is assumed to have a normal distributionp(n(k))~N(0,R)) andRis theMcovariance matrix ofn(k) (measurement noise covariance).

The Kalman Filter algorithm was originally developed for systemsassumed to be represented with a linear state-space model. However, inmany applications the system model is nonlinear. Furthermore the linearmodel is just a special case of a nonlinear model. Therefore, I have decidedto present the Kalman Filter for nonlinear models, but comments are givenabout the linear case. The Kalman Filter for nonlinear models is denotedthe Extended Kalman Filter because it is an extended use of the originalKalman Filter.We definex(k|k-1)to be oura prioriestimate (prediction) at stepkfrom the previous trajectory ofx,andx(k|k) to be oura posterioristate estimate at stepkgiven measurementy(k).Note thatx(k|k-1)is a prediction of the value ofx(k)which is based on the previous values and not on the current observation at timek.x(k|k)on the other hand, uses the information in the current observation (the notation |kis used to emphasize that this value is anestimationofx(k) based on the evidence or observation at timek). Thea priorianda posterioriestimation errors are defined as:e-(k)=x(k)-x(k|k-1) (3)e(k)=x(k)-x(k|k) (4)

Thea prioriestimate error covariance then is:P-(k)=E{e-(k)e-T(k)} (5)and thea posterioriestimate error covariance is:P(k)=E{e(k)eT(k)} (6)In deriving Kalman filter formulation, we begin with the goal of finding an equation that computes an a posteriori state estimate as a linear combination of an a priori estimate (prediction) and a weighted difference between an actual measurement and a measurement prediction (innovation). Hence, each estimate consists of a fraction which is predictable from the previous values and does not contain new information and a fraction that contains the new information extracted from the observation.x(k|k)=x(k|k-1)+K(k)(y(k)-Hx(k|k-1)) (7)The differencey(k)-Hx(k|k-1)in (7) is called the measurementinnovation. The innovation reflects the discrepancy between the predicted value and the actual measurement. ThePMmatrix,K(k), in (7) is chosen to be the gain or blending factor that minimizes the a posteriori error covariance (6). This minimization can be accomplished by first substituting (7) into the above definition fore(k), substituting that into (6), performing the indicated expectations, taking the derivative of the trace of the result with respect toK, setting that result equal to zero, and then solving forK. One form of the resultingK(k) that minimizes (6) is given by:K(k)=P-(k)HT(HP-(k)HT+R)-1 (8)Another way of interpreting weighting byKis that as the measurement error covariance approaches zero, the actual measurement is trusted more, while the predicted measurement is trusted less. On the other hand, as the a priori estimate error covariance approaches zero the actual measurement is trusted less, while the predicted measurement is trusted more.The Kalman filter estimates a process by using a form of feedback control: the filter estimates the process state at some time and then obtains feedback in the form of (noisy) measurements. As such, the equations for the Kalman filter fall into two groups: time update equations (prediction) and measurement update equations (correction). The time update equations are responsible for projecting forward (in time) the current state and error covariance estimates to obtain the a priori estimates for the next time step. The measurement update equations are responsible for the feedback i.e. for incorporating a new measurement into the a priori estimate to obtain an improved a posteriori estimate.

2. SYSTEM SPECIFICATIONS

The system here consists of an inverted pendulum mounted to a motorized cart. The inverted pendulum system is an example commonly found in control system textbooks and research literature. Its popularity derives in part from the fact that it is unstable without control, that is, the pendulum will simply fall over if the cart isn't moved to balance it. Additionally, the dynamics of the system are nonlinear. The objective of the control system is to balance the inverted pendulum by applying a force to the cart that the pendulum is attached to. A realworld example that relates directly to this inverted pendulum system is the attitude control of a booster rocket at takeoff. In this case we will consider a two-dimensional problem where the pendulum is constrained to move in the vertical plane shown in the figure below. For this system, the control input is the force that moves the cart horizontally and the outputs are the angular position of the pendulum and the horizontal position of the cart .

Figure 2.1 Schematic of cart-inverted pendulumThe inverted pendulum on a cart system is an under actuated mechanical system with one control input (motor voltage) and two outputs (cart position and pendulum angle), so it a type of single input multiple output (SIMO) system. The block diagram of the overall set up is illustrated in Figure 2.1. By attempting to control the pendulum, we gain insight into controlling other nonlinear systems because the inverted pendulum is such a canonical example of a nonlinear dynamic system. The control objective is to balance the inverted pendulum on the cart even in the presence of disturbances, while the cart tracks the reference trajectory. The state space model of the inverted pendulum is given as follows Where and

Using the parameters given below, the following state model of the system is obtained. SymbolDescriptionValue/Unit

lPendulum length from pivot to centre of mass0.3302 m

IPendulum moment of inertia7.88x10-3 kgm2

MpPendulum mass0.23kg

MCart mass0.94kg

3. EXPERIMENT DESCRIPTION3.1 AIM: To estimate the states of the considered fourth order system with the help of the Kalman filter. 3.2 TOOLS REQUIRED: Personal Computer with MATLAB.3.3. PROCEDURE: 1) Obtain the mathematical model of inverted pendulum. 2).Compute the predicted state and the Kalman gain.3).Calculate the difference between true and estimated value and get the error. 4) Perform the simulation and obtain the closed loop response from the simulation.

% Implementation of State Estimator clc; clear all; close all;% System Matrices A=[0 0 1 0; 0 0 0 1; 0 2.2643 -15.8866 -0.0073; 0 27.8203 -36.6044 -0.0896]; B=[0; 0; 2.2772; 5.2470]; C=[1 0 0 0; 0 1 0 0]; D=[0; 0];% StateSpace Model sys1=ss(A,B,C,D); % Discretization of Model sys2=c2d(sys1,0.002); [Ad,Bd,Cd,Dd]=ssdata(sys2);3.4. MATLAB CODE:

% Initial State Vector X = [10;100;0;0];% Initial covariance P = eye(4).*(1000); % Process noise matrix Q =1.00000; % Measurement Noise R =0.000001; % Control Input U=10; for i=1:100 % Prediction Equations X = Ad*X+Bd*U; P1 = Ad*P*Ad'+Q; Y=X+0.0001*rand(4,1); Y1=Y'; Z=C*Y+0.0006*rand(1); % Update Equations K = (P1*C')/(C*P1*C'+R); Xk = X+K*(Z-C*X); Pxk = (eye(4)-K*C)*P1; X1=X'; E=Y1-X1; p(i)=i*0.01; true(i,:)=Y1; esti(i,:)=X1; error(i,:)=E;end figure(1);plot(p,true(:,1),'--k',p,esti(:,1),'-k');xlabel('Time in seconds');ylabel('X1');title('True and estimated State of X1');legend('True','Estimated'); figure(2);plot(p,true(:,2),'--k',p,esti(:,2),'-k');xlabel('Time in seconds');ylabel('X2');title('True and estimated State of X2');legend('True','Estimated'); figure(3);plot(p,true(:,3),'--k',p,esti(:,3),'-k');xlabel('Time in seconds');ylabel('X3');title('True and estimated State of X3');legend('True','Estimated');

figure(4);plot(p,true(:,4),'--k',p,esti(:,4),'-k');xlabel('Time in seconds');ylabel('X4');title('True and estimated State of X4');legend('True','Estimated'); figure(5);plot(p,error(:,1),'k');xlabel('Time in seconds');ylabel('Error X1');title('Error plot of X1'); figure(6);plot(p,error(:,2),'k');xlabel('Time in seconds');ylabel('Error X2');title('Error plot of X2'); figure(7);plot(p,error(:,3),'k');xlabel('Time in seconds');ylabel('Error X3');title('Error plot of X3'); figure(8);plot(p,error(:,4),'k');xlabel('Time in seconds');ylabel('Error X4');title('Error plot of X4');

3.5. MATLAB OUTPUT:a = x1 x2 x3 x4 x1 1 4.481e-006 0.001969 -1.145e-008 x2 0 1 -7.244e-005 0.002 x3 0 0.004457 0.9687 -9.889e-006 x4 0 0.05547 -0.07205 0.9999 b = u1 x1 4.507e-006 x2 1.038e-005 x3 0.004483 x4 0.01033 c = x1 x2 x3 x4 y1 1 0 0 0 y2 0 1 0 0 d = u1 y1 0 y2 0 Sampling time (seconds): 0.002Discrete-time model.X = 12.4174 154.5619 19.1662 573.3977P1 = 1.0e+003 * 1.0010 0.0010 0.0029 0.0009 0.0010 1.0011 0.0054 0.0585 0.0029 0.0054 0.9395 -0.0686 0.0009 0.0585 -0.0686 1.0090Y = 12.4175 154.5619 19.1663 573.3977Y1 = 12.4175 154.5619 19.1663 573.3977Z = 12.4178 154.5623K = 1.0000 -0.0000 -0.0000 1.0000 0.0029 0.0054 0.0008 0.0584Xk = 12.4178154.562319.1662573.3977X1 = 12.4174 154.5619 19.1662 573.3977E = 1.0e-004 * 0.4162 0.8419 0.8329 0.25643.6. OUTPUT WAVEFORMS

Figure 3.1: True and estimated state of X1

Figure 3.2: True and estimated state of X2

Figure 3.3: True and estimate state of X3

Figure 3.4: True and estimate state of X4

Figure 3.5: Error plot of X1

Figure 3.6: Error plot of X2

Figure 3.7: Error plot of X3

Figure 3.8: Error plot of X43.7. INFERENCEThe estimated state output designed for the system tracks the actual output of the system with minimum error as it can be justified with error plots of the states above which have error in the order magnitude of 1e-4.3.8. RESULT The state estimation for the given system is implemented using Kalman filter in Matlab.

4. REFERENCES [1]. Mohinder S Grewal, Kalman Filtering: Theory and practice using MATLAB, John Wiley & Sons, Second edition. [2]. http://dea.brunel.ac.uk/cmsp/home_esfandiar/Kalman.html.