MATLAB Program for Kalman Filter

Q. To estimate the positions and velocity of an object using Kalman Filter in MATLAB when a set of measurements and control inputs are available.

Please read about Kalman Filter and Extended Kalman Filter.

Program

%==============================KALMAN FILTER ==============================
%To estimate position 'X' and velocity 'Xdot' of a moving object when the
%measurement of position 'Y' and measurement of velocity 'Ydot' has some
% error in them.
 
%-------------------------------------------------------------------------
%This section creates an erroneous measurements from actual position and
%velocity values. First it is assumed that actual position of object
%initially was Yinitial=400 meters and initial velocity was Ydotinitial=0
%i.e. the particle is at rest. Let a varying acceleration 'u'  be applied,
%after every time step of deltaT. then for value of u(deltaT), new position
%'Yactual' and vecolity 'Ydotactual' is calculated using equations of
%newton`s second law of motion. These values of position and velocity i.e.
%matrix 'Yactual' and 'Ydotactual' are the actual measurement values. Now
%suppose that these actual values are measured with the help of some
% sensors and these sensors have error in them. Let 'variance_Y' be the
% variance of the measurement from position sensor and 'variance_Ydot' be
%the variance of the measurement from velocity sensor. Thus data from
%sesnsors is erroneous with variances 'variance_Y' and 'variance_Ydot'. In
%a practical system we do not have the actual values, rather we have the
%values from position and velocity sensors which have error in them.
clc                      %clear command window
Yinitial=400;            %actual initial position
Ydotinitial=0;           %actual initial velocity
std_deviation_Y=20;      %standard deviation of measured data from position sensor
std_deviation_Ydot=5;    %standard deviation of measured data from velocity sensor
deltaT=0.4;              %Time step
u= [2.0 -0.62 2.1 8.5... %acceleration values (u is a 1-dimensional vector)
    3.5 3.6 2.1 16.0 2.1...
    2.4 1.2 22.5 8.0 6.0...
    7.2 4.0 4.1 2.4 3.2...
    2.4 2.5 2.1 3.4 2.9...
    5.2 3.6 7.4 1.2 -6.2...
    2.4 5.3 9.9 30 2.3];
num=numel(u);            %no. of acceleration values
 
%calculation will be done for each of these values
for i=1:num;
    Yactual(i)=Yinitial+Ydotinitial*deltaT+0.5*deltaT^2*u(i);  %equation from newtons second law s=s0+vt+(0.5)*a*t^2
    Ydotactual(i)=Ydotinitial+deltaT*u(i);                     %v=v0+a*t
    Yinitial=Yactual(i);                                       %set current position as initial position for next step
    Ydotinitial=Ydotactual(i);                                 %set current velocity as initial velocity for next step
end
 
Y=Yactual+std_deviation_Y*randn(1,num);                        %adding noise of variance 'variance_Y' to actual  
        %position values to generate position sensor data
Ydot=Ydotactual+std_deviation_Ydot*randn(1,num);       %adding noise of variance 'variance_Ydot' to actual 
  %velocity values to generate velocity sensor data
Y=[Y;Ydot];                                            %forming measurement vector 'Y'
%Section Ends ------------------------------------------------------------
%Defining Matrices and Constants, Initializing State variables=============
A=[1 deltaT;0 1];                                      %forming A matrix of equation X(k)=AX(k-1)+BU(k-1)+W(k)
B=[0.5*deltaT^2; deltaT];                              %forming B matrix of equation X(k)=AX(k-1)+BU(k-1)+W(k)
X0=350;                                                %setting a random initial state (position estimate)
Xdot0=20;                                              %setting a random initial state (velocity estimate)
X=[X0;Xdot0];                                          %X the initial state vector (have both position and  velocity)
std_deviation_X=500;                                   %defining initial standard deviation for position estimate
std_deviation_Xdot=80;                                 %defining initial standard deviation for velocity estimate
Q=[10 0; 0 1];                                         %process noise covariance matrix
P=[std_deviation_X^2 0;0 std_deviation_Xdot^2];        %State Error covariance matrix
R=[std_deviation_Y^2 0;0 std_deviation_Ydot^2];        %Measurement Error covariance matrix
H=eye(2);                                              %Scaling matrix - An identity matrix in this case
 
   KG=P*H/(H*P*H'+R);                                     %Kalman Gain - Note that in starting P is chosen with very 
 %high values (i.e. high values of standard deviations of X and   
 %Xdot) since we are not sure whether our assumed initial state 
 %is close to true value or not.This will make KG very close to 1 
 %Hence more weightage will be given to measurements Y in the 
 %beginning. When state estimates comes closer and closer to true 
 %values in later stages, KG becomes lower and lower and more 
 %weightage is given to the estimated states.
                                                           
Precord=[];                                           %just an empty variable - used later
Xrecord=[];                                           %just an empty variable - used later
KGrecord=[];                                          %just an empty variable - used later
 
%KALMAN FILTER=============================================================
for i=1:num
    KGprevious=KG                                     %Setting current Kalman Gain as Previous Kalman Gain for next step
    Pprevious=P;                                      %Setting current State Error Covariance Matrix as Previous State Error 
 %Covariance Matrix for next step
    Xprevious=X;                                      %Setting current State Vector as Previous State Vector for next step
    Yprevious=Y;                                      %Setting current Measurement Vector as Previous Measurement Vector for 
 %next step
    %Prediction------------------------------------------------------------
    X=A*Xprevious+B*u(i);                             %prediction step of kalman filter - Predicting the current state vector 
 %from previous state vector using equations of system dynamics
    P=A*Pprevious*A'+Q;                               %Updating the State Error Covariance Matrix using Kalman equations
    P(1,2)=0;                                         %Making covariances in State Error Covariance Matrix zero (off diagonal  
 %elements) considering that 
    P(2,1)=0;                                         %error in one state does not effect error in another state
    %Correction------------------------------------------------------------
    KG=P*H/(H*P*H'+R);                                %updating the current Kalman Gain to be used in correction step
    X=X+KG*(Y(:,i)-H*X);                              %correction step of kalman filter - correction in state vector based on 
 								 %output, previous state and kalman gain 
    P=(eye(2)-KG*H)*P;                                %updating state error covarinace matrix to be used in next step - it 
 %tells the error in the estimated state    
    Precord=[Precord P];                              %recording all Ps
    Xrecord=[Xrecord X];                              %recording all Xs
    KGrecord=[KGrecord KG];                           %recording all KGs
end
 
t=[1:1:num];                                          %time instants
plot(t,Xrecord(1,:),'x');                             %plotting states(positions) with time
hold on
plot(t,Y(1,:),'o');                                   %plotting measured values(positions) with time
plot(t,Yactual(1,:));
hold off
 
sum1=0;
sum2=0;
%error in states from true values------------------------------------------
for i=1:num
    sum1=sum1+(Xrecord(1,i)-Yactual(i))^2
end
sum1=sum1/num;
sd_X=sqrt(sum1);
%error in measurement values from true values------------------------------
for i=1:num
    sum2=sum2+(Y(1,i)-Yactual(i))^2
end
sum2=sum2/num;
sd_Y=sqrt(sum2);

 

Leave a Comment

This site uses Akismet to reduce spam. Learn how your comment data is processed.

error: Content is protected !!

Adblocker detected! Please consider reading this notice.

We've detected that you are using AdBlock Plus or some other adblocking software which is preventing the page from fully loading.

We don't have any banner, Flash, animation, obnoxious sound, or popup ad. We do not implement these annoying types of ads!

We need fund to operate the site, and almost all of it comes from our online advertising.

Please add electricalvoice.com to your ad blocking whitelist or disable your adblocking software.

×