-
Notifications
You must be signed in to change notification settings - Fork 0
/
Project1_EKF.m
125 lines (102 loc) · 3.36 KB
/
Project1_EKF.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
close all
clear all
% clc
XY = csvread('XYData_cm.csv');
HA = csvread('HeadingAngle_rad.csv');
dT = 0.33;
V0 = sqrt((XY(2,1)-XY(1,1))^2 + (XY(2,2)-XY(1,2))^2)/dT;
X = [XY(1,1); XY(1,2);V0;pi];
dx = XY(2,1)-XY(1,1);
dy = XY(2,2)-XY(1,2);
P = [dx,0,0,0;
0,dy,0,0;
0,0,1.07,0;
0,0,0,0.0304];
sX = 0.2;
sY = 0.2;
gamma = [0,0;0,0;sX*sqrt(dT),0;0,sY*sqrt(dT)];
Q = [1,0;0,1];
R = [0.0588,0;0,0.0588];
H = [1,0,0,0;0,1,0,0];
X_est = [X(1,1)];
Y_est = [X(2,1)];
P11 =[P(1,1)];
P22 =[P(2,2)];
P33 =[P(3,3)];
P44 =[P(4,4)];
P11_hist =[P11];
P22_hist =[P22];
P33_hist =[P33];
P44_hist =[P44];
vel_hist = [0];
Vel_est =[0];
Theta_est =[X(4,1)];
for t = 2:length(XY)
%Prediction Step
x1 = X(1,1) + X(3,1)*dT*cos(X(4,1));
x2 = X(2,1) + X(3,1)*dT*sin(X(4,1));
x3 = X(3,1);
x4 = wrapToPi(X(4,1));
X_Pred = [x1;x2;x3;x4];
Phi = [ 1, 0, dT*cos(x4), -x3*dT*sin(x4);
0, 1, dT*sin(x4), x3*dT*cos(x4);
0, 0, 1, 0;
0, 0, 0, 1];
P_Pred = Phi*P*Phi' + gamma*Q*gamma';
K = P_Pred*H'*inv(H*P_Pred*H'+R);
X_upd = X_Pred + K*(XY(t,:)'-H*X_Pred);
P_upd = (eye(4,4) - K*H)*P_Pred;
X_upd(4,1) = wrapToPi(X_upd(4,1));
X_est = [X_est X_upd(1,1)];
Y_est = [Y_est X_upd(2,1)];
Vel_est = [Vel_est X_upd(3,1)];
Theta_est = [Theta_est X_upd(4,1)];
P11_hist =[P11_hist P_upd(1,1)];
P22_hist =[P22_hist P_upd(2,2)];
P33_hist =[P33_hist P_upd(3,3)];
P44_hist =[P44_hist P_upd(4,4)];
vel = sqrt((XY(t,1)-XY(t-1,1))^2 + (XY(t,2) - XY(t-1,2))^2)/dT;
vel_hist = [vel_hist vel ];
X = X_upd;
P = P_upd;
end
t= 1:length(XY);
%Plots
figure(1);
plot(t,XY(:,1),'r-',t,X_est(1,:),'g-');
ylabel('Magnitude of X Position measurements in cm');
xlabel('Discrete time intervals k in seconds');
title('Plot of X Position measurements and Estimates with time');
legend('measurement', 'Estimate','Location','northeast');
figure(2);
plot(t,XY(:,2),'r-',t,Y_est(1,:),'g-');
ylabel('Magnitude of Y Position measurements in cm');
xlabel('Discrete time intervals k in seconds');
title('Plot of Y Position measurements and Estimates with time');
legend('measurement', 'Estimated value','Location','northeast');
figure(3);
plot(XY(:,1),XY(:,2),'r-','LineWidth',1.5);
hold on;
plot(X_est,Y_est,'g-','LineWidth',1.5);
ylabel('Y Position measurements in cm');
xlabel('X Position measurements in cm');
title('Plot of Position measurements and Estimate with time');
legend('measurement', 'Estimate','Location','northeast');
figure(4);
plot(t,HA(:,1),'r-',t,Theta_est(1,:),'g-','LineWidth',1.5);
ylabel('Magnitude of Heading Angle in radians');
xlabel('Discrete time intervals k in seconds');
title('Plot of Heading Angle and its estimates with time');
legend('measurement', 'Estimate','Location','northeast');
figure(5);
plot(t,vel_hist,'r-',t,Vel_est(1,:),'g-','LineWidth',1.5);
ylabel('Magnitude of X Position measurements in cm/sec');
xlabel('Discrete time intervals k in seconds');
title('Plot of Velocity Estimate with time');
legend('measurement','Estimate', 'Location','southeast');
figure(6);
plot(t,P11_hist,t,P22_hist,t,P33_hist,t,P44_hist);
ylabel('Magnitude of variance');
xlabel('Discrete time intervals k in seconds');
title('Plot of State Variance with time');
legend('P11','P22','P33','P44', 'Location','northeast');