clc
close all
clear all
% Set true trajectory
Nsamples=60;
a=2; % acceleration of ship
v0=6; %initial velocity of ship
dt = 0.1; % sampling time
t=0:dt:dt*Nsamples;
Vtrue = v0 + a*t; %m/s (true velocity of ship)
Xinitial = 1000; % initial position of ship
Xtrue = Xinitial -(v0*t + (a/2) * t.^2);% Xtrue is a vector of true positions of the ship
%%% Motion equations
% Previous state (initial guess): Our guess is that the ship starts at 1000 m with velocity
% that equals to 70% of the real velocity
Xk_prev = [1000;
0.7*v0;
a];
% Current state estimate
Xk=[];
% Motion equation: Xk = Phi*Xk_prev + Noise, that is Xk(n) = Xk(n-1) + Vk(n-1) * dt
% here, velocity and acceleration is not measured, but it is estimated
% Phi represents the dynamics of the system: it is the motion equation
Phi = [1 -dt -dt*dt/2; 0 1 dt; 0 0 1];
% The error matrix (or the confidence matrix): P states whether we should
% give more weight to the new measurement or to the model estimate
sigma_model = 1;
% P = sigma^2*G*G';
P = [sigma_model^2 0 0;
0 sigma_model^2 0;
0 0 sigma_model^2];
% Q is the process noise covariance
Q = [0 0 0;
0 0 0;
0 0 0];
% M is the measurement matrix.
% We measure X, so M(1) = 1
% We do not measure V and a, so M(2)= M(3)= 0
M = [1 0 0];
% R is the measurement noise covariance. Generally R and sigma_meas can
% vary between samples.
sigma_meas = 3; % 1 m/sec
R = sigma_meas^2;
%%% Kalman iteration
% Buffers for later display
Xk_buffer = zeros(3,Nsamples+1);
Xk_buffer(:,1) = Xk_prev;
Z_buffer = zeros(1,Nsamples+1);
Z_buffer(1,1)=1000;
for k=1:Nsamples
% Z is the measurement vector. In our
% case, Z = TrueData + RandomGaussianNoise
Z = Xtrue(k+1)+sigma_meas*randn;
Z_buffer(k+1) = Z;
% Kalman iteration
P1 = Phi*P*Phi' + Q;
S = M*P1*M' + R;
% K is Kalman gain. If K is large, more weight goes to the measurement.
% If K is low, more weight goes to the model prediction.
K = P1*M'*inv(S)
P = P1 - K*M*P1
Xk = Phi*Xk_prev + K*(Z-M*Phi*Xk_prev);
Xk_buffer(:,k+1) = Xk;
% For the next iteration
Xk_prev = Xk;
end;
%%% Plot resulting graphs
figure;
plot(t,Xtrue-Z_buffer,'g','Linewidth',2);
hold on;
% plot(t,Z_buffer,'c');
plot(t,Xtrue-Xk_buffer(1,:),'--m','Linewidth',2);
title('Position estimation results');
xlabel('Time (s)');
ylabel('Error (m)');
% legend('True position','Measurement','Kalman estimated');
legend('Possition Error in case of Without Filtering','Error in case of Kalman Estimation');
grid on;
figure;
plot(t,Xtrue,'--g','Linewidth',2);
hold on;
plot(t,Z_buffer,'c','Linewidth',2);
plot(t,Xk_buffer(1,:),'.m','Linewidth',2);
title('Position estimation results');
xlabel('Time (s)');
ylabel('Position (m)');
legend('True position','Without Filtering','Kalman estimated');
% legend('Error of Measurement','Error of estimation');
grid on;
%%% Velocity analysis
% The instantaneous velocity as derived from 2 consecutive position
% measurements
InstantaneousVelocity = [-9 (Z_buffer(2:Nsamples+1)-Z_buffer(1:Nsamples))/dt];
figure;
plot(t,Vtrue,'m','Linewidth',2);
hold on;
plot(t,-InstantaneousVelocity,'--g','Linewidth',2);
plot(t,Xk_buffer(2,:),'.k','Linewidth',2);
title('Velocity estimation results');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
legend('True velocity','Estimated velocity by raw consecutive samples','Estimated velocity by Kalman filter');
%%% Position analysis
%
% figure;
% plot(t,Xtrue,'g');
% hold on;
% plot(t,Z_buffer,'c');
%
% plot(t,Xk_buffer(1,:),'m');
% title('Position estimation results');
% xlabel('Time (s)');
% ylabel('Position (m)');
% legend('True position','Measurement','Kalman estimated');
%
% % %%% Velocity analysis
% %
% % % The instantaneous velocity as derived from 2 consecutive position
% % % measurements
% InstantaneousVelocity = [-9 (Z_buffer(2:Nsamples+1)-Z_buffer(1:Nsamples))/dt];
%
% figure;
% plot(t,Vtrue,'m');
% hold on;
% plot(t,-InstantaneousVelocity,'g');
% plot(t,Xk_buffer(2,:),'k');
% title('Velocity estimation results');
% xlabel('Time (s)');
% ylabel('Velocity (m/s)');
% legend('True velocity','Estimated velocity by raw consecutive samples','Estimated velocity by Kalman filter');
