ASSIGNMENT NO: 4
MOBILE ROBOTICS
Submitted By: SHAHZEB AWAN
ROLL NO. 362585
SUBMITTED TO: DR. FAHAD MUMTAZ
Date: 11-Feb-22
I have implemented Linear Kalman filter in this assignment for the
Motion model, in which I have selected delta=0.12sec, Q has normal
distribution (0, 0.1I), and R=0.05; I have taken 10 discrete points for
this verification of estimate.
Code:-
X0_hat=[0;20];
y1=103;
y2=90;
y3=60;
y4=40;
y5=30;
y6=20;
y7=10;
y8=5;
y9=3;
y10=1;
H=[1 0];
d_t=0.12;
F=[1 d_t;0 1];
G=[0;d_t];
u=-1;
Q=[0.1 0;0 0.1];
R=0.05;
p0_hat=[0.01 0;0 1];
%%%%%%%%%%%
X1_hat_invert=F*X0_hat+G*u
p1_hat_invert=F*p0_hat*F'+Q
K1=p1_hat_invert*H'*(inv(H*p1_hat_invert*H'+R))
X1_hat=X1_hat_invert+K1*(y1-H*X1_hat_invert)
p1_hat=(1-K1*H)*p1_hat_invert;
%%%%%%%%%%%%%%
X2_hat_invert=F*X1_hat+G*u;
p2_hat_invert=F*p1_hat*F'+Q;
K2=p2_hat_invert*H'*(inv(H*p2_hat_invert*H'+R));
X2_hat=X2_hat_invert+K2*(y2-H*X2_hat_invert)
p2_hat=(1-K2*H)*p2_hat_invert;
%%%%%%%%%%%%%
X3_hat_invert=F*X2_hat+G*u;
p3_hat_invert=F*p2_hat*F'+Q;
K3=p3_hat_invert*H'*(inv(H*p3_hat_invert*H'+R));
X3_hat=X3_hat_invert+K3*(y3-H*X3_hat_invert)
p3_hat=(1-K3*H)*p3_hat_invert;
%%%%%%%%%%%%%
X4_hat_invert=F*X3_hat+G*u;
p4_hat_invert=F*p3_hat*F'+Q;
K4=p4_hat_invert*H'*(inv(H*p4_hat_invert*H'+R));
X4_hat=X4_hat_invert+K4*(y4-H*X4_hat_invert)
p4_hat=(1-K4*H)*p4_hat_invert;
%%%%%%%%%%%%%
X5_hat_invert=F*X4_hat+G*u;
p5_hat_invert=F*p4_hat*F'+Q;
K5=p5_hat_invert*H'*(inv(H*p5_hat_invert*H'+R));
X5_hat=X5_hat_invert+K5*(y5-H*X5_hat_invert)
p5_hat=(1-K5*H)*p5_hat_invert;
%%%%%%%%%%%%%
X6_hat_invert=F*X5_hat+G*u;
p6_hat_invert=F*p5_hat*F'+Q;
K6=p6_hat_invert*H'*(inv(H*p6_hat_invert*H'+R));
X6_hat=X6_hat_invert+K3*(y6-H*X6_hat_invert)
p6_hat=(1-K6*H)*p6_hat_invert;
%%%%%%%%%%%%%
X7_hat_invert=F*X6_hat+G*u;
p7_hat_invert=F*p6_hat*F'+Q;
K7=p7_hat_invert*H'*(inv(H*p7_hat_invert*H'+R));
X7_hat=X7_hat_invert+K7*(y7-H*X7_hat_invert)
p7_hat=(1-K7*H)*p7_hat_invert;
%%%%%%%%%%%%%
X8_hat_invert=F*X7_hat+G*u;
p8_hat_invert=F*p7_hat*F'+Q;
K8=p8_hat_invert*H'*(inv(H*p8_hat_invert*H'+R));
X8_hat=X8_hat_invert+K8*(y8-H*X8_hat_invert)
p8_hat=(1-K8*H)*p8_hat_invert;
%%%%%%%%%%%%%
X9_hat_invert=F*X8_hat+G*u;
p9_hat_invert=F*p8_hat*F'+Q;
K9=p9_hat_invert*H'*(inv(H*p9_hat_invert*H'+R));
X9_hat=X9_hat_invert+K9*(y9-H*X9_hat_invert)
p9_hat=(1-K9*H)*p9_hat_invert;
%%%%%%%%%%%%%
X10_hat_invert=F*X9_hat+G*u;
p10_hat_invert=F*p9_hat*F'+Q;
K10=p10_hat_invert*H'*(inv(H*p10_hat_invert*H'+R));
X10_hat=X10_hat_invert+K10*(y10-H*X10_hat_invert)
p10_hat=(1-K10*H)*p10_hat_invert;
%%%%%%%%%%%%%
P_hat=[p1_hat p2_hat p3_hat p4_hat p5_hat p6_hat p7_hat p8_hat p9_hat
p10_hat]
y=[y1 y2 y3 y4 y5 y6 y7 y8 y9 y10]
X_=[X1_hat X2_hat X3_hat X4_hat X5_hat X6_hat X7_hat X8_hat X9_hat X10_hat]
X_hat=[X_hat(1) X_hat(3) X_hat(5) X_hat(7) X_hat(9) X_hat(11) X_hat(13)
X_hat(15) X_hat(17) X_hat(19)]
plot(X_hat,y)
xlabel('X_Hat')
ylabel('Y_Hat')
Result:
y=
103 90 60 40 30 20 10 5 3 1
X_hat =
74.1583 89.4605 62.7591 41.2161 30.4686 20.8564 10.2069 5.0633 3.0110 1.0072
Following is the plot of x_hat Vs y:-
Conclusion:-
By completing this assignment I conclude that my Kalman Filter’s Estimate
is very close to the measured output, I have implemented Linear-Kalman Filter for
the motion model.
