gssm.m

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

    error('[ setparams ] Incorrect number of input arguments.');

  end


%===============================================================================================
function new_state = ffun(model, state, V, U1)

% FFUN  State transition function (system dynamics).
%
%   Generates the next state of the system NEW_STATE given
%   the current STATE, exogenous input U1 and process noise term V. If STATE, U1 and V are matrices
%   then FFUN is calculated for each column vector of these matrices, resulting in an equal number
%   of columns in NEW_STATE. MODEL is a GSSM derived data structure describing the system

%-- This function must be defined by the user!


%===============================================================================================
function  tranprior = prior(model, nextstate, state, U1, pNoiseDS)

% PRIOR  Transition prior function
%
%   Calculates P(nextstate|state). If you plan to run a particle filter on this mode, you should
%   define this.
%
%   INPUT
%         model          GSSM data structure
%         nextstate      state at time k
%         state          state at time k-1
%         U1             exogeneous input to FFUN at time k-1
%         pNoiseDS       (optional) process noise NoiseDS data structure to use for evaluation of
%                        transition prior. If this is ommitted, model.pNoise, is used.
%   OUTPUT
%         tranprior      p(x(k)|x(k-1))
%
%-- This function must be defined by the user!

%===============================================================================================
function observ = hfun(model, state, N, U2)

% HFUN  State observation function.
%
%   OBSERV = HFUN(MODEL, STATE, N, U2) generates the current possibly nonlinear observation of the
%   system state, OBSERV, given the current STATE, exogenous input U and observation noise term V.
%   If STATE, U2 and N are matrices then HFUN is calculated for each column vector of these matrices,
%   resulting in an equal number of columns in OBSERV. MODEL is a GSSM derived data structure describing
%   the system.

%-- This function must be defined by the user!


%===============================================================================================
function llh = likelihood(model, obs, state, U2, oNoiseDS)

% LIKELIHOOD  Observation likelihood function
%
% Function-handle to the observation likelihood function that calculates p(y|x) for a
% given realization of the state variable 'state' and a particular observation instance 'obs'.
%
%   i.e. Calculates the value of P(OBS|STATE) = P(y|x)
%
%   INPUT
%         model          GSSM data structure
%         obs            observation at time k
%         state          state at time k
%         U2             exogeneous input to HFUN at time k
%         oNoiseDS       (optional) measurement noise NoiseDS data structure to use for evaluation of
%                        transition prior. If this is ommitted, model.oNoise, is used.
%   OUTPUT
%         llh            p(y(k)|x(k))
%
%-- This function must be defined by the user!


%===============================================================================================
function INNOV = innovation(model, obs, observ)

% INNOVATION  Innovation model
%
%   INNOV = INNOVATION(MODEL, STATE, OBS, OBSERV) : Calculates the innovation signal (difference) between the
%   output of HFUN, i.e. OBSERV (the predicted system observation) and an actual 'real world' observation OBS.
%   This function might be as simple as INNOV = OBS - OBSERV, which is the default case, but can also be more
%   complex for complex measurement processes where for example multiple (possibly false) observations can be
%   observed for a given hidden ground truth.

%-- This function must be redefined by the user if the specific real world observation process dictates it


%===============================================================================================
function out = linearize(model, state, V, N, U1, U2, term, idxVector)

% LINEARIZE
%
%   OUT = LINEARIZE(MODEL, STATE, V, N, U1, U2, TERM, IDXVECTOR) returns a linearized model of the
%   form
%           state(k) = A*state(k-1) + B*u1(k-1) + G*v(k-1)
%               y(k) = C*state(k)   + D*u2(k)   + H*n(k)
%
%   for an arbitrary model defined by this GSSM file. The string TERM specifies which of the
%   model terms are returned, i.e.
%
%   A = linearize(model, state, v, n, u1, u2, 'A') or
%   O = linearize(model, state, v, n, u1, u2, 'H') etc.
%
%   TERM can be one of the following, 'A','B','C','D','G','H','JFW','JHW' , where 'JFW' and 'JHW'
%   are the partial derivatives of FFUN and HFUN with respect to the system parameters.
%
%   INDEX_VECTOR is an optional argument indicating which subset of the independent vector should be used to calculate
%   any specific derivative. This will result in a Jacobian matrix with a reduced number of columns,
%   corresponding with the subvector as defined by index_vector. The default (when this argument is ommitted)
%   is to use the full vector.
%
%   Generic perturbation based linearization subunits are provided. These can (and should) be replaced
%   by user defined analytical derivative code if available. If no linearization function is available
%   or is not needed, a call to this function should return an error message.
%

  nia = nargin;                      % number of input arguments
  if (nia < 7)
    error('[ linearize ] Not enough input arguments! ');
  end

  epsilon = 1e-8;                    % perturbation step size

  switch (term)

    case 'A'
      %%%========================================================
      %%%             Calculate A = dffun/dstate
      %%%========================================================
      if (nia==7), index_vector=[1:model.statedim]; end
      liv = length(index_vector);
      A  = zeros(model.statedim, liv);
      %%%---------- replace this section if needed --------------
      f1 = model.ffun(model,state,V,U1);
      for j=1:liv,
        s = state;
        k = index_vector(j);
        s(k) = s(k) + epsilon;
        f2 = model.ffun(model,s,V,U1);
        A(:,j) = (f2-f1)/epsilon;
      end
      %%%--------------------------------------------------------
      out = A;


    case 'B'
      %%%========================================================
      %%%             Calculate B = dffun/dU1
      %%%========================================================
      if (nia==7), index_vector=[1:model.U1dim]; end
      liv = length(index_vector);
      B = zeros(model.statedim, liv);
      %%%---------- replace this section if needed --------------
      f1 = model.ffun(model,state,V,U1);
      for j=1:liv,
        Utemp = U1;
        k = index_vector(j);
        Utemp(k) = Utemp(k) + epsilon;
        f2 = model.ffun(model,state,V,Utemp);
        B(:,j) = (f2-f1)/epsilon;
      end
      %%%--------------------------------------------------------
      out = B;


    case 'C'
      %%%========================================================
      %%%             Calculate C = dhfun/dx
      %%%========================================================
      if (nia==7), index_vector=[1:model.statedim]; end
      liv = length(index_vector);
      C = zeros(model.obsdim, liv);
      %%%---------- replace this section if needed --------------
      f3 = model.hfun(model,state,N,U2);
      for j=1:liv,
        s = state;
        k = index_vector(j);
        s(k) = s(k) + epsilon;
        f4 = model.hfun(model,s,N,U2);
        C(:,j) = (f4-f3)/epsilon;
      end
      %%%--------------------------------------------------------
      out = C;


    case 'D'
      %%%========================================================
      %%%             Calculate D = dhfun/dU2
      %%%========================================================
      if (nia==7), index_vector=[1:model.U2dim]; end
      liv = length(index_vector);
      D = zeros(model.obsdim, liv);
      %%%---------- replace this section if needed --------------
      f3 = model.hfun(model,state,N,U2);
      for j=1:liv,
        Utemp = U2;
        k = index_vector(j);
        Utemp(k) = Utemp(k) + epsilon;
        f4 = model.hfun(model,state,N,Utemp);
        D(:,j) = (f4-f3)/epsilon;
      end
      %%%--------------------------------------------------------
      out = D;


    case 'G'
      %%%========================================================
      %%%             Calculate G = dffun/dv
      %%%========================================================
      if (nia==7), index_vector=[1:model.Vdim]; end
      liv = length(index_vector);
      G = zeros(model.statedim, liv);
      %%%---------- replace this section if needed --------------
      f1 = model.ffun(model,state,V,U1);
      for j=1:liv,
        Vtemp = V;
        k = index_vector(j);
        Vtemp(k) = Vtemp(k) + epsilon;
        f5 = model.ffun(model,state,Vtemp,U1);
        G(:,j) = (f5-f1)/epsilon;
      end
      %%%--------------------------------------------------------
      out = G;


    case 'H'
      %%%========================================================
      %%%             Calculate H = dhfun/dn
      %%%========================================================
      if (nia==7), index_vector=[1:model.Ndim]; end
      liv = length(index_vector);
      H = zeros(model.obsdim, liv);
      %%%---------- replace this section if needed --------------
      f3 = model.hfun(model,state,N,U2);
      for j=1:liv,
        Ntemp = N;
        k = index_vector(j);
        Ntemp(k) = Ntemp(k) + epsilon;
        f6 = model.hfun(model,state,Ntemp,U2);
        H(:,j) = (f6-f3)/epsilon;
      end
      %%%--------------------------------------------------------
      out = H;


    case 'JFW'
      %%%========================================================
      %%%             Calculate  = dffun/dparameters
      %%%========================================================
      if (nia==7), index_vector=[1:model.paramdim]; end
      liv = length(index_vector);
      JFW = zeros(model.statedim, liv);
      %%%---------- replace this section if needed --------------
      f1 = model.ffun(model,state,V,U1);
      old_params = model.params;                         % save current model parameters
      for j=1:liv,
        params = old_params;
        k = index_vector(j);
        params(k) = params(k) + epsilon;
        model = setparams(model,params);
        f7 = model.ffun(model,state,V,U1);
        JFW(:,j) = (f7-f1)/epsilon;
      end
      %%%--------------------------------------------------------
      out = JFW;


    case 'JHW'
      %%%========================================================
      %%%             Calculate  = dhfun/dparameters
      %%%========================================================
      if (nia==7), index_vector=[1:model.paramdim]; end
      liv = length(index_vector);
      JHW = zeros(model.obsdim, liv);
      %%%---------- replace this section if needed --------------
      f3 = model.hfun(model,state,N,U2);
      old_params = model.params;                         % save current model parameters
      for j=1:liv,
        params = old_params;
        k = index_vector(j);
        params(k) = params(k) + epsilon;
        model = setparams(model,params);
        f8 = model.hfun(model,state,N,U2);
        JHW(:,j) = (f8-f3)/epsilon;
      end
      %%%--------------------------------------------------------
      out = JHW;

    otherwise
      error('[ linearize ] Invalid linearization term requested!');

  end

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