Kalman-examples/ExtendedBody.m at master · br5555/Kalman-examples · GitHub

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
function [AltErr, VelErr, BallErr] = ExtendedBody

% Continuous time etended Kalman filter example.
% Track a body falling through the atmosphere.
% Outputs are:
%   AltErr = RMS altitude estimation error
%   VelErr = RMS velocity estimation error
%   BallErr = RMS ballistic coefficient estimation error

rho0 = 0.0034; % lb-sec^2/ft^4
g = 32.2; % ft/sec^2
k = 22000; % ft
R = 100; % measurement variance (ft^2)

x = [100000; -6000; 2000]; % initial state
xhat = [100010; -6100; 2500]; % initial state estimate
H = [1 0 0]; % measurement matrix

P = [500 0 0; 0 20000 0; 0 0 250000]; % initial estimation error covariance

tf = 16; % simulation length
dt = tf / 40000; % simulation step size
PlotStep = 200; % how often to plot data points
i = 0;
xArray = x;
xhatArray = xhat;
for t = dt : dt : tf
   % Simulate the system (rectangular integration).
   xdot(1,1) = x(2);
   xdot(2,1) = rho0 * exp(-x(1)/k) * x(2)^2 / 2 / x(3) - g;
   xdot(3,1) = 0;
   x = x + xdot * dt;
   % Simulate the measurement.
   z = H * x + sqrt(R) * randn;
   % Simulate the filter.
   temp = rho0 * exp(-xhat(1)/k) * xhat(2) / xhat(3);
   F = [0 1 0; -temp * xhat(2) / 2 / k temp ...
      -temp * xhat(2) / 2 / xhat(3); ...
         0 0 0];
   Pdot = F * P + P * F' - P * H' * inv(R) * H * P;
   P = P + Pdot * dt;
   K = P * H' * inv(R);
   xhatdot(1,1) = xhat(2);
   xhatdot(2,1) = temp * xhat(2) / 2 - g;
   xhatdot(3,1) = 0;
   xhatdot = xhatdot + K * (z - H * xhat);
   xhat = xhat + xhatdot * dt;
   % Save data for plotting.
   i = i + 1;
   if i == PlotStep
      xArray = [xArray x];
      xhatArray = [xhatArray xhat];
      i = 0;
   end
end

% Plot data.
close all;
t = 0 : PlotStep*dt : tf;

figure;
plot(t, xArray(1,:) - xhatArray(1,:));
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds'); ylabel('Position Estimation Error (feet)');

AltErr = std(xArray(1,:) - xhatArray(1,:));
disp(['Continuous EKF RMS altitude estimation error = ', num2str(AltErr)]);

figure;
plot(t, xArray(2,:) - xhatArray(2,:));
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds'); ylabel('Velocity Estimation Error (feet/sec)');

VelErr = std(xArray(2,:) - xhatArray(2,:));
disp(['Continuous EKF RMS velocity estimation error = ', num2str(VelErr)]);

figure;
plot(t, xArray(3,:) - xhatArray(3,:));
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds'); ylabel('Ballistic Coefficient Estimation Error');

BallErr = std(xArray(3,:) - xhatArray(3,:));
disp(['Continuous EKF RMS ballistic coefficient estimation error = ', num2str(BallErr)]);

figure;
plot(t, xArray(1,:));
title('Falling Body Simulation', 'FontSize', 12);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds'); ylabel('True Position');

figure;
plot(t, xArray(2,:));
title('Falling Body Simulation', 'FontSize', 12);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Seconds'); ylabel('True Velocity');