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