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