clear all

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



% Kalman Filter
X_est = zeros(4, length(Z));
P_est = zeros(4,4, length(Z));
Q = eye(4,4)*0.00001;
P0 = eye(4,4)*0.05;
%x0 = zeros(4,1);
x0 = [0; 1.5; 0; 1.5];
std_meas = 0.5;

P_est(:,:,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];


for i=2:1:length(Z)
       
    previous_x_est = X_est(:, i-1);
    previous_P_est = P_est(:,:,i-1);
    x_pred = nominal_state_transition(previous_x_est, U(i), dt);
    F = jacobian_est(x_pred, U(i), dt);
    P_pred = F*previous_P_est*F' + Q;

    %update
    S = H*P_pred*H' + R;
    W = P_pred*H'*inv(S);
    x_up = x_pred + W*(Z(:,i) - H*x_pred);
    P_up = P_pred - W*S*W';

    X_est(:,i) = x_up;
    P_est(:,:,i) = P_up;


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

%%

%%% For Kalman filter
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

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