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);
