ekf.m

Matlab toolbox that contains functions of Kalman filter and random system simulation.

function [xh, Px, pNoise, oNoise, InternalVariablesDS] = ekf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)

% EKF  Extended Kalman Filter
%
%   [xh, Px, pNoise, oNoise, InternalVariablesDS] = ekf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)
%
%   This filter assumes the following standard state-space model:
%
%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
%     y(k) = hfun[x(k),n(k),U2(k)]
%
%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
%   system.
%
%   INPUT
%         state                state mean at time k-1          ( xh(k-1) )
%         Pstate               state covariance at time k-1    ( Px(k-1) )
%         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')
%         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')
%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
%         InferenceDS          inference data structure generated by GENINFDS function.
%
%   OUTPUT
%         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )
%         Px                   state covariance
%         pNoise               process noise data structure     (possibly updated)
%         oNoise               observation noise data structure (possibly updated)
%
%         InternalVariablesDS  (optional) internal variables data structure
%           .xh_                  predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )
%           .Px_                  predicted state covariance
%           .yh_                  predicted observation ( E[y(k)|Y(k-1)] )
%           .inov                 inovation signal
%           .Pinov                inovation covariance
%           .KG                   Kalman gain
%

%   Copyright  (c) Rudolph van der Merwe (2002)
%
%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
%   academic use only (see included license file) and can be obtained by contacting
%   [e-mail].  Businesses wishing to obtain a copy of the software should
%   contact [e-mail] for commercial licensing information.
%
%   See LICENSE (which should be part of the main toolkit distribution) for more
%   detail.

%=============================================================================================

Xdim  = InferenceDS.statedim;                                % state dimension
Odim  = InferenceDS.obsdim;                                  % observation dimension
U1dim = InferenceDS.U1dim;                                   % exogenous input 1 dimension
U2dim = InferenceDS.U2dim;                                   % exogenous input 2 dimension
Vdim  = InferenceDS.Vdim;                                    % process noise dimension
Ndim  = InferenceDS.Ndim;                                    % observation noise dimension

NOV = size(obs,2);                                           % number of input vectors

%------------------------------------------------------------------------------------------------------------------
%-- ERROR CHECKING

if (nargin ~= 8) error(' [ ekf ] Not enough input arguments.'); end

if (Xdim~=size(state,1)) error(' [ ekf ] Prior state dimension differs from InferenceDS.statedim'); end
if (Xdim~=size(Pstate,1)) error(' [ ekf ] Prior state covariance dimension differs from InferenceDS.statedim'); end
if (Odim~=size(obs,1)) error(' [ ekf ] Observation dimension differs from InferenceDS.obsdim'); end
if U1dim
  [dim,nop] = size(U1);
  if (U1dim~=dim) error(' [ ekf ] Exogenous input U1 dimension differs from InferenceDS.U1dim'); end
  if (dim & (NOV~=nop)) error(' [ ekf ] Number of observation vectors and number of exogenous input U1 vectors do not agree!'); end
end
if U2dim
  [dim,nop] = size(U2);
  if (U2dim~=dim) error(' [ ekf ] Exogenous input U2 dimension differs from InferenceDS.U2dim'); end
  if (dim & (NOV~=nop)) error(' [ ekf ] Number of observation vectors and number of exogenous input U2 vectors do not agree!'); end
end

%------------------------------------------------------------------------------------------------------------------

xh   = zeros(Xdim,NOV);
xh_  = zeros(Xdim,NOV);
yh_  = zeros(Odim,NOV);
inov = zeros(Odim,NOV);

if (U1dim==0), UU1=zeros(0,1); end
if (U2dim==0), UU2=zeros(0,1); end

%--------------------------------------- Loop over all input vectors --------------------------------------------
for i=1:NOV,

    if (U1dim~=0)
        UU1 = U1(:,i);            % get exogenous input
    end
    if (U2dim~=0)
        UU2 = U2(:,i);            % get exogenous input
    end

    %------------------------------------------------------
    % TIME UPDATE

    % linearize FFUN
    [A,G] = feval(InferenceDS.linearize, InferenceDS, state, pNoise.mu, oNoise.mu, UU1, UU2, 'A','G');

    xh_(:,i) = feval(InferenceDS.ffun, InferenceDS, state, pNoise.mu, UU1);
    Px_      = A*Pstate*A' + G*pNoise.cov*G';

    % MEASUREMENT UPDATE

    % linearize HFUN
    [C,H] = feval(InferenceDS.linearize, InferenceDS, xh_(:,i), pNoise.mu, oNoise.mu, UU1, UU2, 'C','H');

    Py        = C*Px_*C' + H*oNoise.cov*H';
    KG        = Px_ * C' * inv(Py);
    yh_(:,i)  = feval(InferenceDS.hfun, InferenceDS, xh_(:,i), oNoise.mu, UU2);

    if isempty(InferenceDS.innovation)
        inov(:,i) = obs(:,i) - yh_(:,i);
    else
        inov(:,i) = feval(InferenceDS.innovation, InferenceDS, obs(:,i), yh_(:,i));  % inovation (observation error)
    end

    xh(:,i)   = xh_(:,i) + KG * inov(:,i);
    Px        = Px_ - KG*Py*KG';

    state  = xh(:,i);
    Pstate = Px;


    if pNoise.adaptMethod switch InferenceDS.inftype
    %---------------------- UPDATE PROCESS NOISE SOURCE IF NEEDED --------------------------------------------
    case 'parameter'  %--- parameter estimation
        switch pNoise.adaptMethod
        case 'anneal'
            pNoise.cov = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov) , pNoise.adaptParams(2)));
        case 'lambda-decay'
            pNoise.cov = (1/pNoise.adaptParams(1)-1) * Pstate;
        case 'robbins-monro'
            nu = 1/pNoise.adaptParams(1);
            pNoise.cov = (1-nu)*pNoise.cov + nu*KG*(KG*inov*inov')';
            pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
        otherwise
            error(' [ekf] Unkown process noise adaptation method!');
        end

    case 'joint'  %--- joint estimation
        idx = pNoise.idxArr(end,:); % get indexs of parameter block of combo-gaussian noise source
        ind1 = idx(1); ind2 = idx(2);
        idxRange = ind1:ind2;
        switch pNoise.adaptMethod
        case 'anneal'
            pNoise.cov(idxRange,idxRange) = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov(idxRange,idxRange)), pNoise.adaptParams(2)));
        case 'lambda-decay'
            param_length = ind2-ind1+1;
            pNoise.cov(idxRange,idxRange) = (1/pNoise.adaptParams(1)-1) * Pstate(end-param_length+1:end,end-param_length+1:end);
        case 'robbins-monro'
            param_length = ind2-ind1+1;
            nu = 1/pNoise.adaptParams(1);
            subKG = KG(end-param_length+1:end,:);
            pNoise.cov(idxRange,idxRange) = (1-nu)*pNoise.cov(idxRange,idxRange) + nu*subKG*(subKG*inov*inov')';
            pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
        otherwise
            error(' [ekf] Unkown process noise adaptation method!');
        end

    %--------------------------------------------------------------------------------------------------
    end; end


end   %--- for loop


if (nargout>4),
  InternalVariablesDS.xh_   = xh_;
  InternalVariablesDS.Px_   = Px_;
  InternalVariablesDS.yh_   = yh_;
  InternalVariablesDS.inov  = inov;
  InternalVariablesDS.Pinov = Py;
  InternalVariablesDS.KG    = KG;
end