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.