
t=[1,5,9]
n0=0       % SET THE NOISE STANDARD DEVIATION
noise=n0*randn(1,3)

% BEGIN COMPUTATION OF OBSERVED OBSERVABLES
v0=150;    % true state
g=20;

d=500;

z=v0*t-0.5*g*t.^2;
v=v0-g*t;

figure (1);
plot (t,z)

rho=sqrt(d^2+z.^2);
rhodot=z./rho;
rhodot=rhodot.*v + noise;  % ADD NOISE IF n0!=0

figure (2);
plot (t,rho)


% END SIMULATION

v0star=100;     % PUTATIVE VALUES (NOMINAL SOLUTION) 
gstar=15;       % ALL QUANTITIES ENDING WITH "star" REFER TO THE NOMINAL TRAJECTORY

for ii=1:5      % 5 ITERATIONS  -  JUST 3 ARE NEEDED TO CONVERGE TO THE CORRECT VALUES
    
    ii
zstar=v0star*t-0.5*gstar*t.^2;  
vstar=v0star-gstar*t;

rhostar=sqrt(d^2+zstar.^2);       % COMPUTED OBSERVABLES
rhodotstar=zstar./rhostar.*vstar
y=rhodot-rhodotstar;              % RESIDUALS = (OBSERVED OBSERVABLES) - (COMPUTED OBSERVABLES)

a1 = (-(zstar(1)/rhostar(1))^2*vstar(1)*t(1)+vstar(1)*t(1)+zstar(1))/rhostar(1);
b1 = (0.5*t(1)/rhostar(1))*((zstar(1)/rhostar(1))^2*vstar(1)*t(1)-t(1)*vstar(1)-2*zstar(1));
H1 = [a1 b1];                     % MATRIX H AT t=1s - DIMENSION 1x2 

a2 = (-(zstar(2)/rhostar(2))^2*vstar(2)*t(2)+vstar(2)*t(2)+zstar(2))/rhostar(2);
b2 = (0.5*t(2)/rhostar(2))*((zstar(2)/rhostar(2))^2*vstar(2)*t(2)-t(2)*vstar(2)-2*zstar(2));
H2 = [a2 b2];                     % MATRIX H AT t=5s - DIMENSION 1x2 

a3 = (-(zstar(3)/rhostar(3))^2*vstar(3)*t(3)+vstar(3)*t(3)+zstar(3))/rhostar(3);
b3 = (0.5*t(3)/rhostar(3))*((zstar(3)/rhostar(3))^2*vstar(3)*t(3)-t(3)*vstar(3)-2*zstar(3));
H3 = [a3 b3];                     % MATRIX H AT t=9s - DIMENSION 1x2 

H  = [H1; H2; H3];                % THE COMPLETE H MATRIX (3x2)
                    % THE PARTIAL DERIVATIVES OF THE OBSERVABLES WRT THE STATE CAN BE COMPUTED EASILY,
                    % WITHOUT THE NEED OF THE TRANSITON MATRIX (REQUIRED IN THE TEST)  
                    % THESE PARTIAL DERIVATIVES ARE SLIGHTLY MORE COMPLEX THAN THOSE FOR RANGE

x = inv(H'*H)*H'*y' % SOLUTION FOR THE STATE DEVIATION VECTOR

v0star=v0star+x(1)  % UPDATE STATE VECTOR
gstar=gstar+x(2)    % REMEMBER: x = STATE DEVIATION VECTOR = (UPDATED STATE VECTOR) - (NOMINAL STATE VECTOR)
                    %           NOMINAL STATE VECTOR = STATE VECTOR USED TO OBTAIN THE COMPUTED OBSERVABLES

end