%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LEAVE THESE LINES UNCHANGED...
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear                   % clear all variables
more off                % prevent pagination of output
format short            % display floats in short form

data = load("-ascii", "sheet8_ekf.log"); % load log file
t = data(:,1)';         % timestamps
z = data(:,2)';         % measurements
truetheta = data(:,3)'; % ground truth for angle theta
d = 3; % distance to the wall
x_history = [];       
pred_z_history = [];

% initial state estimate
mu = [pi/2;
      1;
      1];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FILL IN YOUR CODE FOR NOISE MATRICES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Syntax example: 
% I = [1, 0, 0;
%      0, 1, 0;
%      0, 0, 1];

% initial state covariance
sigma = ?

% state transition noise matrix
R = ?

% measurement noise matrix
Q = ?

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% END OF INITIALIZATION. START OF ITERATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


for i=1:numel(t) % iterating over all time steps
  % zt is the current measurement
  zt = z(i); 
  
  % delta t, needed for the state transition function
  if i==1 
    dt = (t(i+1)-t(i)); % approx. first dt from following time step
  else
    dt = (t(i)-t(i-1));
  end
  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % FILL IN YOUR EKF CODE HERE
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  % prediction of state using g(mu)
  mu(1) = ?   % angle theta
  mu(2) = ?   % translational velocity v
  mu(3) = ?   % radius r
 
  % Jacobian G (3x3 matrix)
  G = ? 
  
  % Jacobian H (1x3 matrix)
  H = ?

  % prediction of state covariance
  sigma = ? 

  % determining Kalman gain
  K = ?

  % predicted measurement using h(mu)
  pred_z = ?
  
  % correction step
  mu = ?
  sigma = ?

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % NO CHANGES NECESSARY BELOW THIS LINE
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  % storing current state and predicted z for visualization
  x_history = [x_history, mu];
  pred_z_history = [pred_z_history, pred_z];

  % plot visualization every 10 time steps
  if mod(i,10)==0
    mu
    sigma
    
    figure(1)
    plot(t(1:i), z(1:i))
    hold on
    plot(t(1:i), pred_z_history, 'r')
    hold off
    title('Distance to the wall');
    legend('Measurement z', 'Predicted measurement');
    xlabel('Time [s]');
    ylabel('Distance [m]');
    
    figure(2)
    plot(t(1:i), truetheta(1:i));
    hold on
    plot(t(1:i), x_history(1,1:i),'r');
    hold off
    title('Angle theta');
    legend('Ground truth', 'Estimated by EKF');
    xlabel('Time [s]');
    ylabel('Angle [rad]');
    
    figure(3)
    plot(t(1:i), x_history(2,1:i));
    hold on
    plot(t(1:i), x_history(3,1:i), 'r');
    hold off
    title('Further estimates by the EKF');
    legend('Translational velocity v', 'Circle radius r');
    xlabel('Time [s]');
    ylabel('Velocity [m/s], Radius [m]');

    sleep(0.4)
  end
end