srukf.m

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

function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srukf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)

% SRUKF  Square Root Unscented Kalman Filter (Sigma-Point Kalman Filter variant)
%
%   [xh, Sx, pNoise, oNoise, InternalVariablesDS] = SRUKF(state, Sstate, 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) )
%         Sstate               lower triangular Cholesky factor of state covariance at time k-1    ( Sx(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          SPOK 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 )
%         Sx                   Cholesky factor of state covariance at time k  ( Sx(k) )
%         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 )
%            .Sx_                 predicted state covariance (Cholesky factor)
%            .yh_                 predicted observation ( E[y(k)|Y(k-1)] )
%            .inov                inovation signal
%            .Pinov               inovation covariance
%            .KG                  Kalman gain
%
%   Required InferenceDS fields:
%         .spkfParams           SPKF parameters = [alpha beta kappa] with
%                                   alpha  :  UKF scale factor
%                                   beta   :  UKF covariance correction factor
%                                   kappa  :  UKF secondary scaling parameter
%   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(' [ srukf ] Not enough input arguments.'); end

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

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

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

% Get UKF scaling parameters
alpha = InferenceDS.spkfParams(1);
beta  = InferenceDS.spkfParams(2);
kappa = InferenceDS.spkfParams(3);

% 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;


switch  InferenceDS.inftype

%======================================= PARAMETER ESTIMATION VERSION ===========================================
case 'parameter'

    L = Xdim+Ndim;                                  % augmented state dimension
    nsp = 2*L+1;                                    % number of sigma-points
    kappa = alpha^2*(L+kappa)-L;                    % compound scaling parameter

    W = [kappa 0.5 0]/(L+kappa);                    % sigma-point weights
    W(3) = W(1) + (1-alpha^2) + beta;

    sqrtW = W;
    possitive_W3 = (W(3) > 0);                      % is zero'th covariance weight possitive?
    sqrtW(1:2) = sqrt(W(1:2));                      % square root weights
    sqrtW(3) = sqrt(abs(W(3)));

    Sqrt_L_plus_kappa = sqrt(L+kappa);

    Zeros_Xdim_X_Ndim     = zeros(Xdim,Ndim);
    Zeros_Ndim_X_Xdim     = zeros(Ndim,Xdim);

    Sv = pNoise.cov;
    dv = diag(Sv);
    Sn = oNoise.cov;
    mu_n = oNoise.mu;

    Sx = Sstate;

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

        UU2 = cvecrep(U2(:,i),nsp);

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

        xh_(:,i) = state;

        if pNoise.adaptMethod
        %--------------------------------------------
            switch pNoise.adaptMethod

            case 'lambda-decay'
                Sx_ = sqrt(pNoise.adaptParams(1))*Sx;

            case {'anneal','robbins-monro'}
                Sx_ = Sx + Sv;

            end
        %---------------------------------------------
        else
            Sx_ = Sx;
        end

        Z   = cvecrep([xh_(:,i); mu_n],nsp);
        Sz  = [Sx_ Zeros_Xdim_X_Ndim; Zeros_Ndim_X_Xdim Sn];
        sS  = Sqrt_L_plus_kappa * Sz;
        Z(:,2:nsp) = Z(:,2:nsp) + [sS -sS];

        Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);

        temp1 = Z(1:Xdim,:) - cvecrep(xh_(:,i),nsp);


        %-- Calculate predicted observation mean, dealing with angular discontinuities if needed
        if isempty(oA_IdxVec)
            yh_(:,i) = W(1)*Y_(:,1) + W(2)*sum(Y_(:,2:nsp),2);
            temp2 = Y_ - cvecrep(yh_(:,i),nsp);
        else
            obs_pivotA = Y_(oA_IdxVec,1);      % extract pivot angle
            Y_(oA_IdxVec,1) = 0;
            Y_(oA_IdxVec,2:end) = subangle(Y_(oA_IdxVec,2:end),cvecrep(obs_pivotA,nsp-1));  % subtract pivot angle mod 2pi
            yh_(:,i) = W(1)*Y_(:,1) + W(2)*sum(Y_(:,2:nsp),2);
            yh_(oA_IdxVec,i) = 0;
            for k=2:nsp,
                yh_(oA_IdxVec,i) = addangle(yh_(oA_IdxVec,i), W(2)*Y_(oA_IdxVec,k));   % calculate UT mean ... mod 2pi
            end
            oFoo = cvecrep(yh_(:,i),nsp);
            temp2 = Y_ - oFoo;
            temp2(oA_IdxVec,:) = subangle(Y_(oA_IdxVec,:), oFoo(oA_IdxVec,:));
            yh_(oA_IdxVec,i) = addangle(yh_(oA_IdxVec,i), obs_pivotA);  % add pivot angle back to calculate actual predicted mean
        end


        [foo,Sy] = qr((sqrtW(2)*temp2(:,2:nsp))',0);       % QR update of observation error Cholesky factor. NOTE: here Sy
                                                           % is the UPPER Cholesky factor (Matlab excentricity)

        if possitive_W3                                    % deal with possible negative zero'th covariance weight
            Sy = cholupdate(Sy,sqrtW(3)*temp2(:,1),'+');
        else
            Sy = cholupdate(Sy,sqrtW(3)*temp2(:,1),'-');   % NOTE: here Sy  is the UPPER Cholesky factor (Matlab excentricity)
        end

        Sy = Sy';                                          % We need the lower triangular Cholesky factor

        Pxy = W(3)*temp1(:,1)*temp2(:,1)' + W(2)*temp1(:,2:nsp)*temp2(:,2:nsp)';

        KG = (Pxy/Sy')/Sy;


        if isempty(InferenceDS.innovation)
            inov(:,i) = obs(:,i) - yh_(:,i);
        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



        Sx_ = Sx_';

        cov_update_vectors = KG*Sy;      % Correct covariance. This is equivalent to :  Px = Px_ - KG*Py*KG';
        for j=1:Odim
            Sx_ = cholupdate(Sx_,cov_update_vectors(:,j),'-');
        end

        Sx = Sx_';
        state = xh(:,i);

        if pNoise.adaptMethod
        %--- update process noise if needed -----------------------
            switch pNoise.adaptMethod

            case 'anneal'