Escolar Documentos
Profissional Documentos
Cultura Documentos
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)
(5)
(6)
(7)
(8)
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
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);
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');
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
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
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 =
1.0e-004 *
0.4162
0.8419
0.8329
0.2564
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.