kalman_smoother.m

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

function [xsmooth, Vsmooth, VVsmooth, loglik] = kalman_smoother(y, A, C, Q, R, init_x, init_V)
% Kalman smoother.
% [xsmooth, Vsmooth, VVsmooth, loglik] = kalman_smoother(y, A, C, Q, R, init_x, init_V)
%
% We compute the smoothed state estimates based on all the observations, Y_1, ..., Y_T:
%   xsmooth(:,t)    = E[x(t) | T]
%   Vsmooth(:,:,t)  = Cov[x(t) | T]
%   VVsmooth(:,:,t) = Cov[x(t), x(t-1) | T] for t>=2

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

xsmooth = zeros(ss, T);
Vsmooth = zeros(ss, ss, T);
VVsmooth = zeros(ss, ss, T);

% Forward pass
[xfilt, Vfilt, VVfilt, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V);

% Backward pass
xsmooth(:,T) = xfilt(:,T);
Vsmooth(:,:,T) = Vfilt(:,:,T);
%VVsmooth(:,:,T) = VVfilt(:,:,T);

for t=T-1:-1:1
  [xsmooth(:,t), Vsmooth(:,:,t), VVsmooth(:,:,t+1)] = ...
      smooth_update(xsmooth(:,t+1), Vsmooth(:,:,t+1), xfilt(:,t), Vfilt(:,:,t), ...
      Vfilt(:,:,t+1), VVfilt(:,:,t+1), A, Q);
end

VVsmooth(:,:,1) = zeros(ss,ss);

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

function [xsmooth, Vsmooth, VVsmooth_future] = smooth_update(xsmooth_future, Vsmooth_future, ...
    xfilt, Vfilt,  Vfilt_future, VVfilt_future, F, Q)
% One step of the backwards RTS smoothing equations.
% [xnew, Vnew, VVnew] = smooth_update(xsmooth, Vsmooth, xfilt, Vfilt, VVfil, F, Q)
%
% Inputs:
% xsmooth_future = E[X_t+1|T]
% Vsmooth_future = Cov[X_t+1|T]
% xfilt = E[X_t|t]
% Vfilt = Cov[X_t|t]
% Vfilt_future = Cov[X_t+1|t+1]
% VVfilt_future = Cov[X_t+1,X_t|t+1]
% F = F(:,:,t+1)
% Q = Q(:,:,t+1)
%
% Outputs:
% xsmooth = E[X_t|T]
% Vsmooth = Cov[X_t|T]
% VVsmooth_future = Cov[X_t+1,X_t|T]
%
% xpred = E[X_t+1 | t]
% Vpred = Cov[X_t+1 | t]

xpred = F*xfilt;
Vpred = F*Vfilt*F' + Q;
J = Vfilt * F' * inv(Vpred); % smoother gain matrix
xsmooth = xfilt + J*(xsmooth_future - xpred);
Vsmooth = Vfilt + J*(Vsmooth_future - Vpred)*J';
VVsmooth_future = VVfilt_future + (Vsmooth_future - Vfilt_future)*inv(Vfilt_future)*VVfilt_future;