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%   rvdmerwe@ece.ogi.edu.  Businesses wishing to obtain a copy of the software should%   contact ericwan@ece.ogi.edu for commercial licensing information.%%   See LICENSE (which should be part of the main toolkit distribution) for more%   detail.%=============================================================================================Xdim  = InferenceDS.statedim;                                % state dimensionOdim  = InferenceDS.obsdim;                                  % observation dimensionU1dim = InferenceDS.U1dim;                                   % exogenous input 1 dimensionU2dim = InferenceDS.U2dim;                                   % exogenous input 2 dimensionVdim  = InferenceDS.Vdim;                                    % process noise dimensionNdim  = InferenceDS.Ndim;                                    % observation noise dimensionNOV = size(obs,2);                                           % number of input vectors%------------------------------------------------------------------------------------------------------------------%-- ERROR CHECKINGif (nargin ~= 8) error(' [ ekf ] Not enough input arguments.'); endif (Xdim~=size(state,1)) error(' [ ekf ] Prior state dimension differs from InferenceDS.statedim'); endif (Xdim~=size(Pstate,1)) error(' [ ekf ] Prior state covariance dimension differs from InferenceDS.statedim'); endif (Odim~=size(obs,1)) error(' [ ekf ] Observation dimension differs from InferenceDS.obsdim'); endif 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!'); endendif 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!'); endend%------------------------------------------------------------------------------------------------------------------xh   = zeros(Xdim,NOV);xh_  = zeros(Xdim,NOV);yh_  = zeros(Odim,NOV);inov = zeros(Odim,NOV);if (U1dim==0), UU1=zeros(0,1); endif (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; endend   %--- for loopif (nargout>4),  InternalVariablesDS.xh_   = xh_;  InternalVariablesDS.Px_   = Px_;  InternalVariablesDS.yh_   = yh_;  InternalVariablesDS.inov  = inov;  InternalVariablesDS.Pinov = Py;  InternalVariablesDS.KG    = KG;end