kalman_filter.m

来自「Bayes网络工具箱」· M 代码 · 共 88 行

function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, model)
% Kalman filter.
% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, model)
%
% Inputs:
% y(:,t)   - the observation at time t
% A(:,:,m) - the system matrix for model m
% C(:,:,m) - the observation matrix for model m
% Q(:,:,m) - the system covariance for model m
% R(:,:,m) - the observation covariance for model m
% init_x(:,m) - the initial state for model m
% init_V(:,:,m) - the initial covariance for model m
% model(t) - which model to use at time t (defaults to model 1 if not specified)
%
% Outputs:
% x(:,t) = E[X_t | t]
% V(:,:,t) = Cov[X_t | t]
% VV(:,:,t) = Cov[X_t, X_t-1 | t] t >= 2
% loglik = sum_t log P(Y_t)

[os T] = size(y);
ss = size(A,1);

if nargin<8, model = ones(1, T); end

x = zeros(ss, T);
V = zeros(ss, ss, T);
VV = zeros(ss, ss, T);

loglik = 0;
for t=1:T
  m = model(t);
  if t==1
    prevx = init_x(:,m);
    prevV = init_V(:,:,m);
    initial = 1;
  else
    prevx = x(:,t-1);
    prevV = V(:,:,t-1);
    initial = 0;
  end
  [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...
      kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, initial);
  loglik = loglik + LL;
end




%%%%%%%%%%%%%%

function [xnew, Vnew, loglik, VVnew] = kalman_update(F, H, Q, R, y, x, V, initial)
% KALMAN_UPDATE Do a one step update of the Kalman filter
% [xnew, Vnew, loglik] = kalman_update(F, H, Q, R, y, x, V, initial)
%
% Given
%  x(:) =   E[ X | Y(1:t-1) ] and
%  V(:,:) = Var[ X(t-1) | Y(1:t-1) ],
% compute 
%  xnew(:) =   E[ X | Y(1:t-1) ] and
%  Vnew(:,:) = Var[ X(t) | Y(1:t) ],
%  VVnew(:,:) = Cov[ X(t), X(t-1) | Y(1:t) ],
%
% If initial=true, x and V are taken as the initial conditions (and F and Q are ignored).
% If there is no observation vector, set K = zeros(ss).

if nargin < 8, initial = 0; end

if initial
  xpred = x;
  Vpred = V;
else
  xpred = F*x;
  Vpred = F*V*F' + Q;
end
e = y - H*xpred; % error (innovation)
n = length(e);
ss = length(F);
S = H*Vpred*H' + R;
Sinv = inv(S);
ss = length(V);
loglik = gaussian_prob(e, zeros(1,length(e)), S, 1);
K = Vpred*H'*Sinv; % Kalman gain matrix
xnew = xpred + K*e;
Vnew = (eye(ss) - K*H)*Vpred;
VVnew = (eye(ss) - K*H)*F*V;