% ### Run some initialization initialization steps common to all filters
localize_init;

% ## Evaluate EKF without measurements: 
%    Estimates are based on control input alone.
curr_mean = [0; 0; 0];
curr_cov = diag([0, 0, 0]);

nomeas_mean = curr_mean;
nomeas_cov = reshape(curr_cov, 9, 1);
for i = 1:size(u,2)
    % Update mean and Jacobian G:
    [curr_mean G] = motion_diff(curr_mean, u(:,i));
    
    % Update covariance matrix
    curr_cov = G*curr_cov*(G') + R;
    
    % Store result for plotting
    nomeas_mean = [nomeas_mean, curr_mean];
    nomeas_cov = [nomeas_cov, reshape(curr_cov, 9, 1)];
end % for each u
plot_trajectory_cov(x_true, nomeas_mean, nomeas_cov, dt);


% ## Full EKF: Estimates based on control input and measurements.
curr_mean = [0; 0; 0];
curr_cov = diag([0. 0, 0]);

ekf_mean = curr_mean;
ekf_cov = reshape(curr_cov, 9, 1);
for i = 1:size(u,2)
    % Prediction step:
    [curr_mean G] = motion_diff(curr_mean, u(:,i));
    curr_cov = G*curr_cov*(G') + R;
    
    % Correction step:
    % Expected measurement:
    [z_exp, H] = measurement(curr_mean);
   
    % Kalman gain:
    K = curr_cov * (H') * inv(H*curr_cov*(H') + Q);
    curr_mean = curr_mean + K*(z_meas(:,i) - z_exp);
    curr_cov = (eye(3) - K*H)*curr_cov;
    
    % Store result for plotting
    ekf_mean = [ekf_mean, curr_mean];
    ekf_cov = [ekf_cov, reshape(curr_cov, 9, 1)];
end % for each u

plot_trajectory_cov(x_true, ekf_mean, ekf_cov, dt);

disp('final result WITH measurements')
final_mean = curr_mean
final_cov = curr_cov
final_err = final_mean - x_true(:,end);
err_pos = norm(final_err(1:2))
err_yaw = abs(final_err(3))