
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) + noise;  % ADD NOISE IF n0!=0
rhodot=z./rho;
rhodot=rhodot.*v;

figure (2);
plot (t,rho)

figure (3);
plot (t,rhodot)

% 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
y=rho-rhostar;               % RESIDUALS = (OBSERVED OBSERVABLES) - (COMPUTED OBSERVABLES)

H1 = [zstar(1)/rhostar(1)*t(1) -zstar(1)/rhostar(1)*t(1)^2/2];  % MATRIX H AT t=1s - DIMENSION 1x2
H2 = [zstar(2)/rhostar(2)*t(2) -zstar(2)/rhostar(2)*t(2)^2/2];  % MATRIX H AT t=5s - DIMENSION 1x2
H3 = [zstar(3)/rhostar(3)*t(3) -zstar(3)/rhostar(3)*t(3)^2/2];  % 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)  

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