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

error: Content is protected !!