simple_kalman_filter.m

来自「IMM PDAF跟踪滤波的程序」· M 代码 · 共 44 行

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