clc
clear all
close all

% SIMULATION (*_sim):
% -------------------------------------------------------
t_sim = [0:0.2:15];

% true initial state...
g_sim=9.8;  % m/s^2
v0_sim=150; % m/s
alfa_sim = 30*pi/180; % rad

% Eq. of motion (solved analyticallly): assume t0=0, z(t0)=0; x(t0)=0
%
% dz(t)/dt = w(t); dw(t)/dt = -g; 
% dx(t)/dt = u(t); du(t)/dt = 0; 
% given z(0) = 0; w(0)=v0*sin(alfa);
% given x(0) = 0; u(0)=v0*cos(alfa);
%
w0_sim = v0_sim*sin(alfa_sim);
u0_sim = v0_sim*cos(alfa_sim);
z_sim = w0_sim*t_sim -0.5*g_sim*t_sim.^2;
x_sim = u0_sim*t_sim;


% Observed observable (range)
d_sim = 500; % m (observer x position, z_observer=0)
obs_id=[6:1+0*20:46]; % index of observatin time (1s, 5s, 9s)
t_obs = t_sim(obs_id);
rho_sim = ((x_sim(obs_id) - d_sim).^2 + z_sim(obs_id).^2).^0.5;

% ... add noise
noise = 1*randn(size(t_obs)); % normal, zero-mean, unit-variance
noise = noise - mean(noise); % make it closer to zero mean
noise = noise/std(noise);   % make it closer to iunit variance
rho_obs = rho_sim'+ 1*noise'; % make it a column vector

% END SIMULATION!
% -------------------------------------------------------




% ESTIMATION:
% -------------------------------------------------------
t = t_sim; % just to compute the trajectory on the same interval
alfa = alfa_sim;  % assume alfa is known 
d = d_sim; % assume d is known 


% first guess (% ALL QUANTITIES ENDING WITH _star REFER TO THE NOMINAL TRAJECTORY!
v0_star= 250; 
g_star = 16;


% iterate:
maxiter = 10; % 5 iterations
for iter=1:maxiter        
    %display(['iter ', num2str(iter)])

    % reference traj.
    w0_star = v0_star*sin(alfa);
    u0_star = v0_star*cos(alfa);
    z_star = w0_star.*t -0.5*g_star*t.^2;
    x_star=u0_star.*t;
    
    % save traj
    x_save(:, iter) = x_star'; % save as a column
    z_save(:, iter) = z_star'; % save as a column
    
    % computed observables (as column vector)
    rho_cmp = ((x_star(obs_id) - d).^2 + z_star(obs_id).^2).^0.5';  
    
    % Residuals (as column vector)
    y = rho_obs - rho_cmp;
    
    % save residuals (prefit)
    eps_save(:, iter) = y;
    
    % State transition matrix: 
    phi=eye(2,2);  % NOTE: A(t) = 0 we have ony cont. params. in state vector;
    
    % Mapping Matrix:
    for j = 1:length(t_obs)
        tj = t_obs(j); % shortcut for t(j)  
        xj = x_star(obs_id(j)); % shortcut for x at t(j)  
        zj = z_star(obs_id(j)); % shortcut for z at t(j)  
        h_tilde = [cos(alfa)*tj*(xj-d)+sin(alfa)*tj*zj, -0.5*(tj^2)*zj].*(1/rho_cmp(j)); % [drho(tj)/dv0,  drho(tj)/dg]
        H(j,:) = h_tilde*phi;
    end
    
    % solve
    delta = inv(H'*H)*H'*y;
    delta_save(:,iter) = delta;
    
    % update first guess
    v0_star = v0_star + delta(1);
    g_star = g_star + delta(2);  
end

% get uncertainty:
P = inv(H'*H);
v0_std = sqrt(P(1,1));
g_std = sqrt(P(2,2));

display('Best estimate:')
display(['v0 =', num2str(v0_star), ' +/- ', num2str(v0_std)])
display(['g =', num2str(g_star), ' +/- ', num2str(g_std)])

% save residual (predicted post-fit, last iter)
eps_save(:, iter+1) = y-H*delta;


figure(1);
plot(x_sim, z_sim, 'b-')
hold on 
plot (x_sim(obs_id), z_sim(obs_id), 'b*') 
hold off
hold on 
plot (x_save(:,1), z_save(:,1), 'r--') 
hold off
hold on 
plot (x_save(:,end), z_save(:,end), 'r-') 
hold off
ylabel('z (m)')
xlabel('x (m)')
grid on
legend('True (sim.)', 'obs. location', 'first guess (iter 0)', 'estim (iter 4)' ) 
title('Trajectory')


figure(2);
plot(t_obs, rho_obs, '*')
grid on
ylabel('time (s)')
xlabel('range (m)')
title('observables')

figure(3);
subplot(2,1,1), plot([1:maxiter], delta_save(1,:))
grid on
ylabel('delta v0')
subplot(2,1,2), plot([1:maxiter], delta_save(2,:))
grid on
ylabel('delta g')
xlabel('iteration')
title('diff. correction')


figure(4);
plot(t_obs, eps_save(:,1), 'b*')
hold on
plot(t_obs, eps_save(:,end), 'ro')
hold off
grid on
xlabel('time (s)')
ylabel('residuals (m)')
grid on
legend('residual (prefit)', 'residuals (postfit)') 
title('residuals')

