kfcova.m

GPS TOOLBOX包含以下内容： 1、GPS相关常量和转换因子； 2、角度变换； 3、坐标系转换： &#61656 点变换； &#61656 矩阵变换； &#61656 向量变换

%                                  kfcova.m
%  Scope:   This MATLAB macro generates Kalman filter covariance matrix when the
%           parameters are constants; alternate conventional formulation is implemented.
%           This computation is useful for covariance analysis.
%  Usage:   [pafter,pbefore] = kfcova(nstep,n,m,phi,q,h,r,pinitinv) 
%  Description of parameters:
%           nstep    - input, number of steps to be executed
%           n        - input, state vector dimension
%           m        - input, measurement dimension
%           phi      - input, transition matrix,  n by n
%           q        - input, process noise matrix,  n by n
%           h        - input, measurement matrix,  m by n, where m is the 
%                      number of measurements
%           r        - input, measurement noise matrix,  m by m
%           pinvinit - input, initial value of the inverse of the covariance matrix, 
%                      symmetric, n by n matrix
%           pafter   - output, final value of the covariance matrix after measurement 
%                      incorporation (current time cycle)
%           pinvbef  - output, final value of the covariance matrix after time 
%                      propagation and before measurement incorporation (next time cycle) 
%  Remarks: 1) For covariance analysis the matrix pafter should be saved at each step.
%           2) When parameters are time-dependent then this module should be called 
%              for one step only (nstep = 1) with new time dependent values at each call.
%  Last update: 04/03/00
%  Copyright (C) 2000 by LL Consulting. All Rights Reserved.

function    [pafter,pinvbef] = kfcova(nstep,n,m,phi,q,h,r,pinvinit)

pinvbef = pinvinit;
rinv = inv(r);

for  k = 1:nstep
   
%  Compute measurement update of the inverse covariance matrix
   
   pinvaft = h' * rinv * h + pinvbef;   
   
%  Determine the covariance matrix after measurement incorporation
   
   pafter = inv(pinvaft);
   
%  Propagate the covariance matrix

   pbefore = phi * pafter * phi' + q;
   
%  Determine the inverse of the covariance matrix to be used at the next cycle
   
   pinvbef = inv(pbefore);   
   
end