%EKF_PREDICT2 2nd order Extended Kalman Filter prediction step
%
% Syntax:
% [M,P] = EKF_PREDICT2(M,P,[A,F,Q,a,W,param])
%
% In:
% M - Nx1 mean state estimate of previous step
% P - NxN state covariance of previous step
% A - Derivative of a() with respect to state as
% matrix, inline function, function handle or
% name of function in form A(x,param) (optional, default identity)
% F - NxNxN Hessian matrix of the state transition function
% w.r.t. state variables as matrix, inline
% function, function handle or name of function
% in form F(x,param) (optional, default identity)
% Q - Process noise of discrete model (optional, default zero)
% a - Mean prediction E[a(x[k-1],q=0)] as vector,
% inline function, function handle or name
% of function in form a(x,param) (optional, default A(x)*X)
% W - Derivative of a() with respect to noise q
% as matrix, inline function, function handle
% or name of function in form W(x,k-1,param) (optional, default identity)
% param - Parameters of a (optional, default empty)
%
%
%
% Out:
% M - Updated state mean
% P - Updated state covariance
%
% Description:
% Perform Extended Kalman Filter prediction step.
%
% See also:
% EKF_PREDICT1, EKF_UPDATE1, EKF_UPDATE2, DER_CHECK, LTI_DISC,
% KF_PREDICT, KF_UPDATE
% History:
% 22.5.07 JH Initial version. Modified from ekf_predict1.m
% originally created by SS.
%
% Copyright (C) 2007 Jouni Hartikainen, Simo S�rkk�
%
% $Id: ekf_predict2.m 467 2010-10-12 09:30:14Z jmjharti $
%
% 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 [M,P] = ekf_predict2(M,P,A,F,Q,a,W,param)
%
% Check arguments
%
if nargin < 3
A = [];
end
if nargin < 4
F = []
end
if nargin < 5
Q = [];
end
if nargin < 6
a = [];
end
if nargin < 7
W = [];
end
if nargin < 8
param = [];
end
% Apply defaults
if isempty(A)
A = eye(size(M,1));
end
if isempty(F)
F = eye(size(M,1));
end
if isempty(Q)
Q = zeros(size(M,1));
end
if isempty(W)
W = eye(size(M,1),size(Q,2));
end
if isnumeric(A)
% nop
elseif isstr(A) | strcmp(class(A),'function_handle')
A = feval(A,M,param);
else
A = A(M,param);
end
if isnumeric(F)
% nop
elseif isstr(F) | strcmp(class(F),'function_handle')
F = feval(F,M,param);
else
F = F(M,param);
end
% Perform prediction
if isempty(a)
M = A*M;
elseif isnumeric(a)
M = a;
elseif isstr(a) | strcmp(class(a),'function_handle')
M = feval(a,M,param);
else
M = a(M,param);
end
for i=1:size(F,1)
F_i = squeeze(F(i,:,:));
M(i) = M(i) + 0.5*trace(F_i*P);
end
if isnumeric(W)
% nop
elseif isstr(W) | strcmp(class(W),'function_handle')
W = feval(W,M,param);
else
W = W(M,param);
end
P_new = A * P * A' + W * Q * W';
for i = 1:size(F,1)
for j = 1:size(F,1)
F_i = squeeze(F(i,:,:));
F_j = squeeze(F(j,:,:));
P_new(i,j) = P_new(i,j)+0.5*trace(F_i*P*F_j*P);
end
end
P = P_new;