%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