1.

THEORY
Kalman 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
equation models the noisy and distorted observation signal. For a signal x(k) and
noisy observation y(k), equations describing the state process model and the
observation model are defined as
x(k)=Ax(k-1)+w(k-1)
y(k)=Hx(k)+n(k)

(1)
(2)

where
x(k) is the P-dimensional signal vector, or the state parameter, at time k,
A is a P P dimensional state transition matrix that relates the states of the
process at times k 1 and k,
w(k) (process noise) is the P-dimensional uncorrelated input excitation
vector of the state equation. w(k) is assumed to be a normal (Gaussian)
process p(w(k))~N(0, Q),
Q being the P P covariance matrix of w(k) or process noise covariance.
y(k) is the M dimensional noisy observation vector,
H is a M P dimensional matrix
n(k) is the M-dimensional noise vector, also known as measurement
noise, n(k) is assumed to have a normal distribution p(n(k))~N(0, R)) and R is
the M covariance matrix of n(k) (measurement noise covariance).
The Kalman Filter algorithm was originally developed for systems
assumed to be represented with a linear state-space model. However, in
many applications the system model is nonlinear. Furthermore the linear
model is just a special case of a nonlinear model. Therefore, I have decided

to present the Kalman Filter for nonlinear models, but comments are given
about the linear case. The Kalman Filter for nonlinear models is denoted
the Extended Kalman Filter because it is an extended use of the original
Kalman Filter.
We define x(k|k-1) to be our a priori estimate (prediction) at step k from the
previous trajectory of x, and x(k|k) to be our a posteriori state estimate at
step k given measurement y(k). Note that x(k|k-1) is a prediction of the value
of x(k) which is based on the previous values and not on the current observation at
time k. x(k|k) on the other hand, uses the information in the current observation
(the notation |k is used to emphasize that this value is an estimation of x(k) based
on the evidence or observation at time k). The a priori and a posteriori estimation
errors are defined as:
e-(k)=x(k)-x(k|k-1)
e(k)=x(k)-x(k|k)

(3)
(4)

The a priori estimate error covariance then is:

P-(k)=E{e-(k)e-T(k)}

(5)

and the a posteriori estimate 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 difference y(k)-Hx(k|k-1) in (7) is called the measurement innovation.

The innovation reflects the discrepancy between the predicted value and the actual
measurement. The PM matrix, 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 for e(k),
substituting that into (6), performing the indicated expectations, taking the
derivative of the trace of the result with respect to K, setting that result equal to
zero, and then solving for K. One form of the resulting K(k) that minimizes (6) is
given by:
K(k)=P-(k)HT(HP-(k)HT +R)-1

(8)

Another way of interpreting weighting by K is 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 twodimensional 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 pendulum

The 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

X =AX + BU
Y =CX
T

Where X =[ xc x c ] , U=V and Y =[ x c ]

0

0
1

Beq ( M p l I )

M p lB p

q
M p lBeq

( M M p ) B p

0
0

1
0

(M pl )

A 0

0 ( M M p ) M p gl

0
0
2
M p l I

q
M pl
q

Using the parameters given below, the following state model of the system is
obtained.
Symbol
l
I
Mp
M

Description
Pendulum length from pivot to centre of mass
Pendulum moment of inertia
Pendulum mass
Cart mass

[][

xc
0
0
1
0
0
= 0
0
1
xc
0 2.2643 15.8866 0.0073
0 27.8203 36.6044 0.0896

[]

xc
y= 1 0 0 0
0 1 0 0 xc

][ ] [ ]

xc
0
+
0
u
x c 2.2772
5.2470

Value/Unit
0.3302 m
7.88x10-3 kgm2
0.23kg
0.94kg

3. EXPERIMENT DESCRIPTION
3.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.
3.4. MATLAB CODE:
% 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);

% 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
title('Error plot of X1');

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

x1

1 4.481e-006

x2

x3

0.004457

x4

0.05547

b=

u1
x1 4.507e-006
x2 1.038e-005
x3

0.004483

x4

0.001969 -1.145e-008

-7.244e-005

0.002

0.9687 -9.889e-006
-0.07205

0.9999

x4

c=

0.01033

x1 x2 x3 x4
y1 1 0 0 0
y2 0 1 0 0

d=

u1
y1 0
y2 0

Sampling time (seconds): 0.002

Discrete-time model.
X=

12.4174
154.5619
19.1662
573.3977

P1 = 1.0e+003 *

Y=

1.0010

0.0010

0.0029

0.0009

0.0010

1.0011 0.0054

0.0585

0.0029

0.0054

0.0009

0.0585 -0.0686

0.9395 -0.0686
1.0090

12.4175
154.5619
19.1663
573.3977

Y1 = 12.4175 154.5619 19.1663 573.3977

Z=

12.4178
154.5623

K=

1.0000 -0.0000

Xk =

-0.0000

1.0000

0.0029

0.0054

0.0008

0.0584

12.4178
154.5623
19.1662
573.3977

X1 =

12.4174 154.5619 19.1662 573.3977

1.0e-004 *

0.4162

0.8419

3.6. OUTPUT WAVEFORMS

0.8329

0.2564

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 X4

3.7. INFERENCE

The 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.