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