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

% ## Evaluate UKF 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)
    [curr_mean, curr_cov] = ukf_prediction(curr_mean, curr_cov, u(:,i), R, @motion_diff);
  
    % Store result for later 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);

% ## Evaluate full UKF with measurements: 
curr_mean = [0; 0; 0];
curr_cov = diag([0, 0, 0]);
ukf_mean = curr_mean;
ukf_cov = reshape(curr_cov, 9, 1);
for i = 1:size(u,2)
    [curr_mean, curr_cov] = ukf_prediction(curr_mean, curr_cov, u(:,i), R, @motion_diff);
    [curr_mean, curr_cov] = ukf_correction(curr_mean, curr_cov, z_meas(:,i), Q, @measurement);
    
    % Store result for later plotting
    ukf_mean = [ukf_mean, curr_mean];
    ukf_cov  = [ukf_cov, reshape(curr_cov, 9, 1)];
end % for each u

plot_trajectory_cov(x_true, ukf_mean, ukf_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))