kf.m

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

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

% KF  Kalman Filter
%
%   [xh, Px, pNoise, oNoise, InternalVariablesDS] = kf(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

%   See also
%   UKF, CDKF, EKF, GMSPKF, PF, SPPF, GMSPPF, GSPF
%   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;                                    % 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(' [ kf ] Not enough input arguments!.'); end

if (Xdim~=size(state,1)) error('[ kf ] Prior state dimension differs from InferenceDS.statedim'); end

if (Xdim~=size(Pstate,1)) error('[ kf ] Prior state covariance dimension differs from InferenceDS.statedim'); end

if (Odim~=size(obs,1)) error('[ kf ] Observation dimension differs from InferenceDS.obsdim'); end

if U1dim
    [dim,nop] = size(U1);
    if (U1dim~=dim) error('[ kf ] Exogenous input U1 dimension differs from InferenceDS.U1dim'); end
    if (dim & (NOV~=nop)) error('[ kf ] 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('[ kf ] Exogenous input U2 dimension differs from InferenceDS.U2dim'); end
    if (dim & (NOV~=nop)) error('[ kf ] 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 = []; end
if (U2dim==0), UU2 = []; end

if pNoise.adaptMethod
    switch InferenceDS.inftype
      case 'joint'
          idx = pNoise.idxArr(end,:);     % get indeces of parameter block of combo-gaussian noise source
          ind1 = idx(1);                  % beginning index of parameter section
          ind2 = idx(2);                  % ending index of parameter section
          edim = ind2-ind1+1;         % infer parameter vector length
          idxRange = ind1:ind2;
      case 'state'
          ind1 = 1;
          ind2 = Xdim;
          edim = Xdim;
          idxRange = ind1:ind2;
    end
end

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

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

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

    A = InferenceDS.linearize( InferenceDS, state, pNoise.mu, [], UU1, [], 'A');
    G = InferenceDS.linearize( InferenceDS, state, pNoise.mu, [], UU1, [], 'G');

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

    %--- MEASUREMENT UPDATE
    C = InferenceDS.linearize( InferenceDS, xh_(:,i), [], oNoise.mu, [], UU2, 'C');
    H = InferenceDS.linearize( InferenceDS, xh_(:,i), [], oNoise.mu, [], UU2, 'H');

    Py        = C*Px_*C' + H*oNoise.cov*H';
    KG        = Px_ * C' * inv(Py);

    yh_(:,i)  = InferenceDS.hfun( InferenceDS, xh_(:,i), oNoise.mu, UU2);

    if isempty(InferenceDS.innovation)
        inov(:,i) = obs(:,i) - yh_(:,i);
    else
        inov(:,i) = 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(' [kf]unknown process noise adaptation method!');
        end

    case 'joint'  %--- joint estimation
        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'
            pNoise.cov(idxRange,idxRange) = (1/pNoise.adaptParams(1)-1) * Pstate(end-edim+1:end,end-edim+1:end);
        case 'robbins-monro'
            nu = 1/pNoise.adaptParams(1);
            subKG = KG(end-edim+1:end,:);
            pNoise.cov(idxRange,idxRange) = (1-nu)*InferenceDS.pNoise.cov(idxRange,idxRange) + nu*subKG*(subKG*inov*inov')';
            pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
        otherwise
            error(' [kf]unknown 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