%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Add your code between the comments in this file. There are two places
% where some code is needed to make this script run.
%
% 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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Load the real and measurements data (Y, X_r)%
load("measurement.mat"); % This command loads the measurement: Y
load("RealData.mat"); % This command loads the real data: X_r
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 =
L =
H = %measurment matrix/vector
q = % process covariance
r = % measurement covariance
X0 =
P0 =
Xm(:, 1) = X0;
SP(:,:, 1) = P0;
%
% MODEL INITIALIZATION END HERE !!!
%
for i = 2:n
%
% IMPLEMENTATION OF KALMAN FILTER START HERE !!!
%
% Store latest estimate and covariance
Xm(:, i) =
SP(:, :, i) =
%
% YOUR CODE ENDS HERE !!!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Visualize data (dot not modify the code bellow)%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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-')
hold on;
plot(Y(:),'k.')
plot(Xm(1,:),'b-')
legend('Real', 'Meas', 'Est')
title('Position Estimate')
figure(2)
clf(2)
plot(X_r(2,:),'r-')
hold on;
plot(Xm(2,:),'b-')
legend('Real', 'Est')
title('Velocity Estimate')