kfcov.m

GPS software toolbox for GPS receiver development

%                             kfcov.m
%  Scope:   This MATLAB macro generates Kalman filter covariance matrix when the
%           parameters are constants; conventional formulation is implemented.
%           This computation is useful for covariance analysis.
%  Usage:   [pafter,pbefore] = kfcov(nstep,n,m,phi,q,h,r,pinit) 
%  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
%           pinit   - input, initial value of the covariance matrix, symmetric
%                     n by n matrix
%           pafter  - output, final value of the covariance matrix after measurement 
%                     incorporation (current time cycle)
%           pbefore - 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/02/00
%  Copyright (C) 2000 by LL Consulting. All Rights Reserved.

function    [pafter,pbefore] = kfcov(nstep,n,m,phi,q,h,r,pinit)

imatrix = eye(n);
pbefore = pinit;

for  k = 1:nstep

%  Compute Kalman gain matrix

   temp = pbefore * h';
   kmatrix = temp * inv(h*temp + r);

%  Update and symmetrize the covariance matrix

   pafter = (imatrix - kmatrix*h) * pbefore;
   pafter = 0.5 * (pafter + pafter');
   
   %  Propagate the covariance matrix

   pbefore = phi * pafter * phi' + q;
   
end