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

function [x, P, K, loglik] = Simple_Kalman_Filter(y, F, H, Q, R, initx, initP) 
% Kalman filter. 
% [x, P, K, loglik] = Simple_Kalman_Filter(y, F, H, Q, R, initx, initP) 
% 
% Inputs: 
% y(:,t)   - the observation at time t 
% F(:,:,m) - the system matrix for model m 
% H(:,:,m) - the observation matrix for model m 
% Q(:,:,m) - the system covariance for model m 
% R(:,:,m) - the observation covariance for model m 
% initx(:,m) - the initial state for model m 
% initP(:,:,m) - the initial covariance for model m 
% 
% Outputs: 
% x(:,t) = E[X_t | t] 
% P(:,:,t) = Cov[X_t | t] 
% K(:,t) = kalman gain 
% loglik = log P(Y_t) 
 
[os T]  = size(y); 
ss      = size(F,1); 
 
x       = zeros(ss, T); 
P       = zeros(ss, ss, T); 
K       = zeros(ss, os, T); 
loglik  = zeros(1, T); 
 
prevx   = initx; 
prevP   = initP; 
 
for t=1:T, 
     
  [x(:,t), P(:,:,t), K(:,:,t), loglik(t)] = Simple_Kalman_Update(F, H, Q, R, y(:,t), prevx, prevP); 
   
  prevx = x(:,t); 
  prevP = P(:,:,t); 
   
end