www.pudn.com > un_IMM_PDAF.zip > Simple_Kalman_Update.m

function [xnew, Pnew, K, loglik] = Simple_Kalman_Update(F, H, Q, R, y, x, P) 
% KALMAN_UPDATE Do a one step update of the Kalman filter 
% [xnew, Pnew, K, loglik] = Simple_Kalman_Update(F, H, Q, R, y, x, P) 
% 
% Given 
%  x(:) =   E[ X | Y(1:t-1) ] and 
%  P(:,:) = Var[ X(t-1) | Y(1:t-1) ], 
% compute  
%  xnew(:) =   E[ X | Y(1:t-1) ] and 
%  Pnew(:,:) = Var[ X(t) | Y(1:t) ], 
% using 
%  y(:)   - the observation at time t 
%  F(:,:) - the system matrix 
%  H(:,:) - the observation matrix 
%  Q(:,:) - the system covariance 
%  R(:,:) - the observation covariance 
% 
 
% a priori 
xpred   = F*x; 
Ppred   = F*P*F' + Q; 
 
% error (innovation) 
e       = y - H*xpred;  
 
S       = H*Ppred*H' + R; 
Sinv    = inv(S); 
K       = Ppred*H'*Sinv; % Kalman gain matrix 
 
% log likelihood 
loglik  = gaussian_prob(e, zeros(size(e)), S, 0); %0); 
 
% updated 
xnew    = xpred + K*e; 
Pnew    = (eye(size(F)) - K*H)*Ppred; 
 
%K