%
% Propagate the error covarience matrix P
%
% P(k+1) = phi(k)*P(k)*phi(k)' + Q(k)
%
% where P is the error covarience matrix
%       phi is the state transition matrix
%       Q is the random walk noise
%

function ans=propagate_P(T,P0,R,g)

  Delta_t=T(2)-T(1);
  
  % calculate phi=expm(F)
  % 
  % where F is the state transition matrix which is const.

  F = [0  0   1  0  0  0  0;
       0  0   0  1  0  0  0;
       0  0   0  0  0  g  0;
       0  0   0  0 -g  0  0;
       0  0   0 1/R 0  0  0;
       0  0 -1/R 0  0  0  0;
       0  0   0  0  0  0  0];

  phi = expm(F*Delta_t);
  
  % Q*(t-t0) is the covarience of the random walk error w
  
  Q = [0  0  0      0       0        0        0;
       0  0  0      0       0        0        0;
       0  0  2.4e-7 0       0        0        0;
       0  0  0      2.4e-7  0        0        0;
       0  0  0      0       8.4e-10  0        0;
       0  0  0      0       0        8.4e-10  0;
       0  0  0      0       0        0        8.4e-10];
  
  % iterate over the time given to us
  % using the equation:
  % P(k+1) = phi(k)*P(k)*phi(k)' + Q*(tk-t0)

  % initialize the iterator Pt
  Pt=P0;
  
  % initialize PA which will store the time history of diag(Pt)
  PA=diag(Pt)';
  
  for t=T(2):Delta_t:T(length(T))
    % calculate the new value of P
    Pt = phi*Pt*phi' + Q*(t-T(1));
    
    % record the diagonal of Pt in PA
    PA = [PA; diag(Pt)'];
  end;
  
  ans=[Pt PA'];