%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Add your code between the comments in this file.
%
% Author: IO
% Date: 07.10.2022
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear all;
figure(1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Parameters for adjusting the visualization %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
plotRatio = 1; % Plot: 0 = none, 1 = every, 2 = every other...
ptime = 0.2; % pause time after each plot in seconds
focus = 1; % 0 = whole area, 1 = focuses into current point
ymin = 3; % Parameters for adjusting the size of focus window
ymax = 3;
xmin = 15;
xmax = 5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate the data and measurements %
load("measurement.mat");
load("RealData.mat");
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Allocating memory for estimated state and covariance matrix %
n = length(Y);
Xm = zeros(2, n); % variable for saving the estimated state
SP = zeros(2, 2, n); % variable for saving the estimated state covariance
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Implementation of Kalman filter !!!
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% INITIALIZE KALMAN FILTER PARAMETERS HERE !!!
%
dt = 1/100;
F = [1 dt; 0 1];
H = [1 0];
L = [dt*dt/2; dt];
Q = 0.3;
R = 0.5;
X0 = [0; 0];
P0 = [1 0; 0 100];
Xm(:, 1) = X0;
SP(:,:, 1) = P0;
%
% MODEL INITIALIZATION END HERE !!!
%
for i = 2:n
%
% IMPLEMENTATION OF KALMAN FILTER START HERE !!!
%
x_p = F*Xm(:, i-1);
P_p = F*SP(:,:, i-1)*F' + L*Q*L';
v = Y(i) - H*x_p;
S = H*P_p*H' + R;
W = P_p * H'*inv(S);
Xm(:,i) = x_p + W*v;
SP(:,:,i) = P_p - W*S*W';
% Store latest estimate and covariance
%Xm(:, i) =
%SP(:, :, i) =
%
% YOUR CODE ENDS HERE !!!
%
%%%%%%%%%%%%%%%%%%
% Visualize data %
%%%%%%%%%%%%%%%%%%
if(~mod(i,plotRatio))
clf(1)
if(focus)
axis([i-xmin i+xmax X_r(1, i)-ymin X_r(1,i)+ymax])
else
axis([0 n min(Y(1:i)) max(Y(1:i))])
end
hold on
plot(i,X_r(1,i),'ko','MarkerFaceColor','r','MarkerSize',5)
plot(i,Y(i),'ko','MarkerFaceColor','k','MarkerSize',6)
plot(i,Xm(1, i),'ko','MarkerFaceColor','b','MarkerSize',5)
legend('Real','Meas','Est','Location','Best')
plot(X_r(1,1:i),'r.-')
plot(Y(1:i-1),'k.')
plot(Xm(1,1:i),'b.-')
pause(ptime)
end
end
%%%%%%%%%%%%%%%%%
% Plot all data %
%%%%%%%%%%%%%%%%%
clf(1)
plot(X_r(1,:),'r-', 'LineWidth',2)
hold on;
plot(Y(:),'k.')
plot(Xm(1,:),'b-', 'LineWidth',2)
legend('Real', 'Meas', 'Est')
title('Position Estimate')
figure(2)
clf(2)
plot(X_r(2,:),'r-', 'LineWidth',2)
hold on;
plot(Xm(2,:),'b-', 'LineWidth',2)
legend('Real', 'Est')
title('Velocity Estimate')
figure(3)
SP_position = squeeze(SP(1,1,:));
plot(SP_position,'b-', 'LineWidth',2)
title('Evolution of the estimated position variance')