R_e=6371000;       % Radius of the earth [meters]
g=9.8;             % Gravity [m/s^2]
Pt=100*eye(7,7);
Delta_t=5;
% record the time his variences in PA
PA=[];
% record time in TA
TA=[];
% time between measurements
Delta_mt=60;
% total number of measurements
Num_Meas = 50;
% time vector is T
T=[0:Delta_t:Delta_mt]';
% Sensitivity matrix H
H = [eye(2) zeros(2,5)];
% Measurement noise covarience R
R = 1*[eye(2)];

figure;

% for each measurement
for i = 1:1:Num_Meas  
   
   TA=[TA;T];
   % let time pass until the next measurement
	 ans = propagate_P(T,Pt,R_e,g);
	 Pt = ans(1:7,1:7);
   PA = [PA; ans(:,8:max(size(ans)))'];
      
   % find K since we have a measurement
   K = Pt*H'*inv(H*Pt*H' + R);
   % knowing K recalculate P to reflect the measurement
   Pt = (eye(7) - K*H)*Pt;
   T = [T(length(T))+Delta_t:Delta_t:T(length(T))+Delta_t+Delta_mt]';
   
end;

% Plot the results
titles = {'North Postion Standard Deviation'; 
  'East Position Standard Deviation';
  'North Velocity Standard Deviation';
  'East Velocity Standard Deviation';
  'Platform Tilt X Standard Deviation';
  'Platform Tilt Y Standard Deviation';
  'Platform Azimuth Standard Deviation'};

for i=1:1:7
  subplot(4,2,i);
  semilogy(TA,sqrt(PA(:,i)));
  grid on;
  title(titles(i));
  ylabel('\sigma');
  xlabel('Time [sec]');
end;