kf_loop.m

kalman滤波

%KF_LOOP  Performs the prediction and update steps of the Kalman filter
%         for a set of measurements.   
%
% Syntax:
%   [MM,PP] = KF_LOOP(X,P,H,R,Y,A,Q)
% 
% In:
%   X - Nx1 initial estimate for the state mean 
%   P - NxN initial estimate for the state covariance
%   H - DxN measurement matrix
%   R - DxD measurement noise covariance
%   Y - DxM matrix containing all the measurements.
%   A - Transition matrix of the discrete model (optional, default identity)
%   Q - Process noise of the discrete model     (optional, default zero)
%   
% Out:
%   MM - Filtered state mean sequence
%   PP - Filtered state covariance sequence
%  
%  Description:
%    Calculates state estimates for a set measurements using the
%    Kalman filter. This function is for convience, as it basically consists
%    only of a space reservation for the estimates and of a for-loop which
%    calls the predict and update steps of the KF for each time step in
%    the measurements.  
%  
%  See also:
%    KF_PREDICT, KF_UPDATE

%  History:
%   
%    12.2.2007 JH Initial version.  
%
% Copyright (C) 2007 Jouni Hartikainen
%
% This software is distributed under the GNU General Public 
% Licence (version 2 or later); please refer to the file 
% Licence.txt, included with the software, for details.

function [MM,PP] = kf_loop(X,P,H,R,Y,A,Q)

  % Check the input parameters.  
  if nargin < 5
     error('Too few arguments');
  end
  if nargin < 6
     A = [];
  end
  if nargin < 7
     Q = [];
  end

  % Apply the defaults
  if isempty(A)
    A = eye(size(X,1));
  end
  if isempty(Q)
    Q = zeros(size(X,1));
  end

  % Space for the estimates.
  MM = zeros(size(X,1), size(Y,2));
  PP = zeros(size(X,1), size(X,1), size(Y,2));

  % Filtering steps.
  for i = 1:size(Y,2)
     [X,P] = kf_predict(X,P,A,Q);
     [X,P] = kf_update(X,P,Y(:,i),H,R);
     MM(:,i) = X;
     PP(:,:,i) = P;
  end