% We implement a particle filter to estimate the state of the system given
% in problem 1.
clear all

global L
L = 0.15;
load('measurement_data.mat');
load('input_data.mat');
load('True_trajectory.mat');


N_particles = 1200;
X_particles = zeros(4, N_particles); % storage for the particles representing the state.
P_particles = zeros(4,4, N_particles);
weights = zeros(1, N_particles);
X_est = zeros(4, length(Z)); %storage for saving the estimated state.
P_k = zeros(4,4, length(Z));

P0 = eye(4,4)*0.05;
X0 = [0; 1.5; 0; 1.5];
std_meas = 0.5;

P_k(:,:,1) = P0;
X_est(:,1) = X0;
dt = 0.01;

R = [std_meas*std_meas 0; 
     0 std_meas*std_meas];
H = [1, 0, 0, 0;
     0, 1, 0, 0];
Q = eye(4,4)*0.0001;

%Initial particles and weights
for i=1:N_particles
    X_particles(:,i) = X0 + chol(Q)*randn(4,1);
    weights(i) = 1.0/N_particles;
end


for k=2:1:length(Z)
    
    %Propagate the particles
    for i=1:N_particles
        X_particles(:,i) = nominal_state_transition(X_particles(:,i), U(k), dt);
        X_particles(:,i) = X_particles(:,i) + chol(Q)*randn(4,1);
    end

    % The predicted particles can be used to compute the predicted mean and
    % covariance.

    % Compute the weights
    mean_Z = H*X_particles;
    for i=1:N_particles
        weights(i) = exp((-1/2)*(Z(:,k) - mean_Z(:,i))'*inv(R)*(Z(:,k) - mean_Z(:,i)));
    end
    
    weights = weights./sum(weights);

    % Do resampling
    ind = resampstr(weights);
    X_particles = X_particles(:, ind);

    % Compute estimated state
    X_est(:, k) = mean(X_particles, 2);
end

figure
plot(Z(1,:), Z(2,:), 'k.', 'MarkerSize', 6)
hold on
plot(true_states(1,:), true_states(2,:),'b', X_est(1,:), X_est(2,:),'r-.', 'LineWidth', 2.5)
legend("Measurement", "True","Estimated")
axis equal
hold off

figure
plot(1:length(true_states), true_states(4,:),'b', 1:length(X_est(4,:)), X_est(4,:),'r', 'LineWidth', 2.5)
legend("True speed","Estimated speed")

figure
plot(1:length(true_states), true_states(3,:),'b', 1:length(X_est(3,:)), X_est(3,:),'r', 'LineWidth', 2.5)
legend("True heading","Estimated heading")
%%
rmse = 0;
for i=1:length(true_states)
    rmse = rmse + (true_states(:,i) - X_est(:,i)).*(true_states(:,i) - X_est(:,i));
end
rmse = sqrt(rmse./length(Z))

%%
function [state_next] = nominal_state_transition(state, u, dt)
    theta = state(3);
    v = state(4);
    global L
    state_next = [1, 0, 0, dt*v*cos(theta);
                  0, 1, 0, dt*v*sin(theta);
                  0, 0, 1, dt*tan(u)/L;
                  0, 0, 0, 1] * state;
end