% 5.3 Extended Kalman Filter (stereo system)

clear;
close all;

% Intrinsic parameters for both cameras
f = 1000;
rx = 640;
ry = 480;

% Constant Transformation matrices, from World to Camera 1 and 2
T1 = [eye(3,3), [200; 0; 0]; 0 0 0 1];
T2 = [eye(3,3), [-200; 0; 0]; 0 0 0 1];

% Motion model:
% Perturbed acceleration

% Sample time (we do 100 steps = 10 sec. of simulation)
dt = 0.1;

% Constant (gravity) acceleration term
% a0 = g*dt (acceleration over a dt time interval)
a0 = [0; -981*dt; 0];

% This is the real system state ("ground truth")
% used to do the simulation
% (in real systems, we do not know it!)

% Initial Position (real value)
p0 = [0;0;1200];

% Initial Velocity: forwards (z), and upwards (y)
% (real value)
v0 = [0;300;100];

% This is the motion model
A = [eye(3,3), eye(3,3)*dt; zeros(3,3), eye(3,3)];

B = [0.5*eye(3,3)*dt^2; eye(3,3)*dt];

% Covariance of acceleration noise
lw = 1;

% Covariance matrix of motion process
Qw = B*lw*B';

% Covariance matrix of measurement process
lv = 1;
Qv = lv*eye(4,4);

% Real state (initial state)
p_real = p0;
v_real = v0;

% Initial state estimate and covariance matrix
% state = (position,velocity)
p_est = [0;0;1000];
v_est = [0;0;0];
s_est = [p_est;v_est];
S_est = diag([10 10 100 10 10 10]);

% Variables used to plot the results
xr(1) = p_real(1);
xe(1) = p_est(1);
yr(1) = p_real(2);
ye(1) = p_est(2);
zr(1) = p_real(3);
ze(1) = p_est(3);

p_error = zeros(100,1);
v_error = zeros(100,1);

p_error(1) = norm(p_est-p_real);
v_error(1) = norm(v_est-v_real);

% Main loop
for t=2:100
    
    % Real motion (simulate the random process)
    w = sqrt(lw)*randn(3,1);
    
    p_real = p_real + v_real*dt + 0.5*w*dt^2 + 0.5*a0*dt^2;
    v_real = v_real + w*dt + a0*dt;
    a_real = w + a0;
    
    % Extended Kalman Filter: Prediction Step
    
    % The predicted state (=prior) is obtained applying motion model
    % without noise (expected motion)
    p_pred = p_est + v_est*dt + 0.5*a0*dt^2;
    v_pred = v_est + a0*dt;

    % Covariance matrix of the prediction
    S_pred = A*S_est*A' + Qw;
    
    % Expected measurement h(p_pred,0) and Jacobian matrix in (p_pred)
    [z1exp, z2exp, J1, J2] = Meas_Stereo(p_pred,f,rx,ry,T1,T2);    
    % C is the Jacobian matrix of the full measurement vector (4x4)
    C = [J1;J2];

    % Simulate the real measurement:
    
    % At the real position, compute the expected measurement
    % and add the Gaussian measurement noise
    [z1real, z2real, J1, J2] = Meas_Stereo(p_real,f,rx,ry,T1,T2);    
    z1real = z1real+sqrt(lv)*randn(2,1);
    z2real = z2real+sqrt(lv)*randn(2,1);
        
    % Extended Kalman Filter: Correction step

    % Kalman Gain
    G = S_pred*C'*inv(C*S_pred*C'+Qv);
    
    % Update the state estimate (=posterior)
    s_est = [p_pred; v_pred] + G*([z1real; z2real] - [z1exp; z2exp]);
    p_est = s_est(1:3);
    v_est = s_est(4:6);

    % Update the covariance matrix
    S_est = S_pred-G*C*S_pred;   
    
    % Errors (norm) : estimated position - real position
    p_error(t) = norm(p_est-p_real);
    v_error(t) = norm(v_est-v_real);

    % Store the results for plotting
    xr(t) = p_real(1);
    xe(t) = p_est(1);
    yr(t) = p_real(2);
    ye(t) = p_est(2);
    zr(t) = p_real(3);
    ze(t) = p_est(3);
    
    z1r(:,t-1) = z1real;
    z2r(:,t-1) = z2real;
end

% Plot position and velocity errors
figure;
plot(p_error);
hold;
plot(v_error,'r');
title('Position (blue) and velocity (red) errors');

figure;
plot(z1r(1,:),z1r(2,:));
axis equal;
title('Measurements from camera 1');

figure;
plot(z2r(1,:),z2r(2,:));
axis equal;
title('Measurements from camera 2');

% Plot the real x trajectory, and its estimate
%figure;
%plot(xr);
%hold;
%plot(xe,'r');
%title('Real (blue) and estimated (red) x trajectory');

% Plot the real parabolic trajectory (y,z), and its estimate
figure;
plot(zr,yr);
hold;
plot(ze,ye,'r');
title('Real (blue) and estimated (red) (y,z) trajectory');

% Plot the real parabolic trajectory (y,z), and its estimate
%figure;
%plot3(xr,zr,yr);
%hold;
%plot3(xe,ze,ye,'r');
%title('Real (blue) and estimated (red) (y,z) trajectory');
%axis equal