%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% 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')