cdkf.m

有关kalman滤波及其一些变形滤波算法

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

% CDKF  Central Difference Kalman Filter  (Sigma-Point Kalman Filter variant)
%
%   [xh, Px, pNoise, oNoise, InternalVariablesDS] = cdkf(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                 innovation signal
%           .Pinov                innovation covariance
%           .KG                   Kalman gain
%
%   Required InferenceDS fields:
%         .spkfParams           SPKF parameters = [h] with
%                                    h  :  CDKF scale factor / difference step size
%
%   Copyright (c) Oregon Health & Science University (2006)
%
%   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 from
%   http://choosh.csee.ogi.edu/rebel/.  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;                                % extract state dimension
Odim  = InferenceDS.obsdim;                                  % extract observation dimension
U1dim = InferenceDS.U1dim;                                   % extract exogenous input 1 dimension
U2dim = InferenceDS.U2dim;                                   % extract exogenous input 2 dimension
Vdim  = InferenceDS.Vdim;                                    % extract process noise dimension
Ndim  = InferenceDS.Ndim;                                    % extract observation noise dimension

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

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

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

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

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

h = InferenceDS.spkfParams;
hh = h^2;

W1 = [(hh - Xdim - Vdim)/hh   1/(2*hh);                  % sigma-point weights set 1
      1/(2*h)                sqrt(hh-1)/(2*hh)];

W2      = W1;
W2(1,1) = (hh - Xdim - Ndim)/hh ;                        % sigma-point weights set 2

Zeros_Xdim_X_Vdim = zeros(Xdim,Vdim);
Zeros_Vdim_X_Xdim = zeros(Vdim,Xdim);
Zeros_Xdim_X_Ndim = zeros(Xdim,Ndim);
Zeros_Ndim_X_Xdim = zeros(Ndim,Xdim);

% Get index vectors for any of the state or observation vector components that are angular quantities
% which have discontinuities at +- Pi radians ?

sA_IdxVec = InferenceDS.stateAngleCompIdxVec;
oA_IdxVec = InferenceDS.obsAngleCompIdxVec;


nsp1                    = 2*(Xdim+Vdim) + 1;             % number of sigma points (first set)
nsp2                    = 2*(Xdim+Ndim) + 1;             % number of sigma points (second set)

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

Sv = chol(pNoise.cov)';                     % matrix square root of process noise covariance
Sn = chol(oNoise.cov)';                     % matrix square root of measurement noise covariance
Sx = chol(Pstate)';                                     % matrix square root of state covariance

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

    if U1dim, UU1 = cvecrep(U1(:,i),nsp1); end
    if U2dim, UU2 = cvecrep(U2(:,i),nsp2); end

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

    Z   = cvecrep([state; pNoise.mu],nsp1);
    Zm  = Z;                                                 % copy needed for possible angle components section
    Sz  = [Sx Zeros_Xdim_X_Vdim; Zeros_Vdim_X_Xdim Sv];
    hSz = h*Sz;
    hSzM = [hSz -hSz];
    Z(:,2:nsp1) = Z(:,2:nsp1) + hSzM;

    %-- Calculate predicted state mean, dealing with angular discontinuities if needed
    if isempty(sA_IdxVec)
        X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1);  % propagate sigma-points through process model
        xh_(:,i) = W1(1,1)*X_(:,1) + W1(1,2)*sum(X_(:,2:nsp1),2);
        A = W1(2,1) * ( X_(:,2:Xdim+Vdim+1) - X_(:,Xdim+Vdim+2:nsp1) ) ;
        B = W1(2,2) * ( X_(:,2:Xdim+Vdim+1) + X_(:,Xdim+Vdim+2:nsp1) - cvecrep(2*X_(:,1),Xdim+Vdim));
    else
        Z(sA_IdxVec,2:nsp1) = addangle(Zm(sA_IdxVec,2:nsp1), hSzM(sA_IdxVec,:));      % fix sigma-point set for angular components
        X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1); % propagate sigma-points through process model
        state_pivotA = X_(sA_IdxVec,1);                                % extract pivot angle
        X_(sA_IdxVec,1) = 0;
        X_(sA_IdxVec,2:end) = subangle(X_(sA_IdxVec,2:end),cvecrep(state_pivotA,nsp1-1));  % subtract pivot angle mod 2pi
        xh_(:,i) = W1(1,1)*X_(:,1) + W1(1,2)*sum(X_(:,2:nsp1),2);
        xh_(sA_IdxVec,i) = 0;
        for k=2:nsp1,
            xh_(sA_IdxVec,i) = addangle(xh_(sA_IdxVec,i), W1(1,2)*X_(sA_IdxVec,k));     % calculate CDKF mean ... mod 2pi
        end
        A = W1(2,1) * ( X_(:,2:Xdim+Vdim+1) - X_(:,Xdim+Vdim+2:nsp1) ) ;
        B = W1(2,2) * ( X_(:,2:Xdim+Vdim+1) + X_(:,Xdim+Vdim+2:nsp1) - cvecrep(2*X_(:,1),Xdim+Vdim));
        A(sA_IdxVec,:) = subangle(W1(2,1)*X_(sA_IdxVec,2:Xdim+Vdim+1), W1(2,1)*X_(sA_IdxVec,Xdim+Vdim+2:nsp1));
        B(sA_IdxVec,:) = addangle(W1(2,2)*X_(sA_IdxVec,2:Xdim+Vdim+1), W1(2,2)*X_(sA_IdxVec,Xdim+Vdim+2:nsp1));
        % Note for above line : Remember, X_(sA_IdxVec,1) = 0, so the last term of B expression need not be subtracted
        xh_(sA_IdxVec,i) = addangle(xh_(sA_IdxVec,i), state_pivotA);  % add pivot angle back to calculate actual predicted mean
    end

    [temp,Sx_] = qr([A B]',0);
    Sx_= Sx_';

    Z = cvecrep([xh_(:,i); oNoise.mu],nsp2);
    Zm = Z;                                             % copy needed for possible angle components section
    Sz = [Sx_ Zeros_Xdim_X_Ndim; Zeros_Ndim_X_Xdim Sn];
    hSz = h*Sz;
    hSzM = [hSz -hSz];
    Z(:,2:nsp2) = Z(:,2:nsp2) + hSzM;

    %-- Calculate predicted observation mean, dealing with angular discontinuities if needed
    if isempty(oA_IdxVec)
        Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
        yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);
        C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
        D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - cvecrep(2*Y_(:,1),Xdim+Ndim));
    else
        Z(oA_IdxVec,2:nsp2) = addangle(Zm(oA_IdxVec,2:nsp2), hSzM(oA_IdxVec,:));  % fix sigma-point set for angular components
        Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
        obs_pivotA = Y_(oA_IdxVec,1);      % extract pivot angle
        Y_(oA_IdxVec,1) = 0;
        Y_(oA_IdxVec,2:nsp2) = subangle(Y_(oA_IdxVec,2:nsp2),cvecrep(obs_pivotA,nsp2-1));  % subtract pivot angle mod 2pi
        yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);           % pediction of observation
        yh_(oA_IdxVec,i) = 0;
        for k=2:nsp2,
           yh_(oA_IdxVec,i) = addangle(yh_(oA_IdxVec,i), W2(1,2)*Y_(oA_IdxVec,k));   % calculate CDKF mean ... mod 2pi
        end
        C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
        D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - cvecrep(2*Y_(:,1),Xdim+Ndim));
        C(oA_IdxVec,:) = subangle(W2(2,1)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,1)*Y_(oA_IdxVec, Xdim+Ndim+2:nsp2));
        D(oA_IdxVec,:) = addangle(W2(2,2)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,2)*Y_(oA_IdxVec, Xdim+Ndim+2:nsp2));
        % Note for above line : Remember, Y_(oA_IdxVec,1) = 0, so the last term of D expression need not be subtracted
        yh_(oA_IdxVec,i) = addangle(yh_(oA_IdxVec,i), obs_pivotA);  % add pivot angle back to calculate actual predicted mean
    end

    [temp,Sy] = qr([C D]',0);
    Sy = Sy';


    %------------------------------------------------------
    % MEASUREMENT UPDATE

    Syx1 = C(:,1:Xdim);
    Syw1 = C(:,Xdim+1:end);

    Pxy = Sx_*Syx1';

    KG = (Pxy / Sy') / Sy;

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


    if isempty(sA_IdxVec)
       xh(:,i) = xh_(:,i) + KG*inov(:,i);
    else
       upd = KG*inov(:,i);
       xh(:,i) = xh_(:,i) + upd;
       xh(sA_IdxVec,i) = addangle(xh_(sA_IdxVec,i), upd(sA_IdxVec));
    end

    state = xh(:,i);

    [temp,Sx] = qr([Sx_-KG*Syx1 KG*Syw1 KG*D]',0);
    Sx=Sx';

    Px = Sx*Sx';


    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) * Px;
        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(' [ cdkf ] Unknown process noise adaptation method!');
        end

        Sv = chol(pNoise.cov)';

    case 'state'  %--- state estimation
        switch pNoise.adaptMethod
        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(' [ cdkf ] Process noise adaptation method not allowed!');
        end

        Sv = chol(pNoise.cov)';

    case 'joint'  %--- joint estimation
        idx = pNoise.idxArr(end,:); % get indexs of parameter block of combo-gaussian noise source
        ind1 = idx(1); ind2 = idx(2);
        switch pNoise.adaptMethod
        case 'anneal'
            pNoise.cov(ind1:ind2,ind1:ind2) = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov(ind1:ind2,ind1:ind2)), pNoise.adaptParams(2)));
        case 'lambda-decay'
            param_length = ind2-ind1+1;
            pNoise.cov(ind1:ind2,ind1:ind2) = (1/pNoise.adaptParams(1)-1) * Px(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(ind1:ind2,ind1:ind2) = (1-nu)*pNoise.cov(ind1:ind2,ind1:ind2) + nu*subKG*(subKG*inov*inov')';
            pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
        otherwise
            error(' [ cdkf ] Unknown process noise adaptation method!');
        end

        Sv = chol(pNoise.cov)';

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

end   %--- for loop


if (nargout>4),
  InternalVariablesDS.xh_   = xh_;
  InternalVariablesDS.Px_   = Sx_*Sx_';
  InternalVariablesDS.yh_   = yh_;
  InternalVariablesDS.inov  = inov;
  InternalVariablesDS.Pinov = Sy*Sy';
  InternalVariablesDS.KG    = KG;
end