Code covered by the BSD License  

Highlights from
UD Factorization & Kalman Filtering

from UD Factorization & Kalman Filtering by Gerard Van Willigenburg
UD and LD factorization of nonnegative matrices and associated Kalman filter implementations.

[P,K,xhat]=kmfmu(c,w,P,xhat,y) 
function [P,K,xhat]=kmfmu(c,w,P,xhat,y)
% KMFMU: Discrete-time Kalman filter measurement update.
%
%        [P,k,xhat]=kmfmu(c,w,P,xhat,y)
%
%        P   : Covariance matrix
%        c   : Output matrix
%        w   : Output covariance matrix
%        xhat: State Estimate
%        y   : Measurement
%        K   : Kalman gain
%
%        L.G. Van Willigenburg, W.L. De Koning, 10-01-2002.
%
if nargin<3; error(' KMFMU requires 3 or 5 Input Argments'); end
[l,n]=size(c); K=P*c'*inv(c*P*c'+w);
ikh=eye(n)-K*c;P=ikh*P*ikh'+K*w*K';
if nargin==5
  xhat=ikh*xhat+K*y;
elseif nargin~=3
  error(' KMFMU requires 3 or 5 Input Argments');
end

Contact us