geninfds.m

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

    %  LINEARIZE_STATE  Linearization function of meta system for state estimation
    %
    %    varargout = linearize_state(InferenceDS, state, V, N, U1, U2, varargin)
    %
    %    INPUT
    %         InferenceDS     : (InferenceDS) Inference data structure
    %         state           : (c-vector) meta system state vector
    %         V               : (c-vector) meta system process noise vector
    %         N               : (c-vector) meta system observation noise vector
    %         U1              : (c-vector) meta system exogenous input 1
    %         U2              : (c-vector) meta system exogenous input 2
    %         varargin        : (strings) linearization terms wanted, e.g. 'A','B','G',....
    %    OUTPUT
    %         varargout       : (matrices) linearization terms corresponding with varargin strings

  nop = length(varargin);

  for k=1:nop,
    varargout{k} = InferenceDS.model.linearize( InferenceDS.model, state, V, N, U1, U2, varargin{k});
  end


%===========================================================================================================
%================================ PARAMETER ESTIMATION FUNCTIONS ===========================================

function new_state = ffun_parameter(InferenceDS, state, V, U1)

    %  FFUN_PARAMETER  State transition function of meta system for parameter estimation
    %
    %    new_state = ffun_parameter(InferenceDS, state, V, U1)
    %
    %    INPUT
    %         InferenceDS     : (InferenceDS) Inference data structure
    %         state           : (c-vector) meta system system state vector
    %         V               : (c-vector) meta system process noise vector
    %         U1              : (c-vector) meta system exogenous input 1
    %    OUTPUT
    %         new_state       : (c-vector) updated meta system state vector
    %
    % Relationship between input arguments and external model (GSSM) variables
    %
    %   state -> external model parameters or a subset (specified by InferenceDS.paramParamIdxVec) thereof.
    %   U1    -> this is usually an empty matrix
    %   V     -> synthetic process noise (speeds up convergence)
    %

    new_state = state + V;

%-------------------------------------------------------------------------------------
function tran_prior = prior_parameter(InferenceDS, nextstate, state, U1, pNoiseDS)

    %  PRIOR_STATE  Calculates the transition prior probability P(x_k|x_(k-1))
    %
    %    tranprior = prior_parameter(InferenceDS, nextstate, state, pNoiseDS)
    %
    %    INPUT
    %         InferenceDS     : (InferenceDS) Inference data structure
    %         nextstate       : (c-vector)  system state at time k
    %         state           : (c-vector)  system state at time k-1
    %         U1              : (c-vector) meta system exogenous input 1
    %         pNoiseDS        : (NoiseDS)   process noise data structure
    %    OUTPUT
    %         tranprior       : scalar probability P(x_k|x_(k-1))

    X = nextstate - state;

    tran_prior = pNoiseDS.likelihood( pNoiseDS, X);


%-------------------------------------------------------------------------------------
function observ = hfun_parameter_bothp(InferenceDS, state, N, U2)

    %  HFUN_PARAMETER_BOTHP  State observation function of meta system for parameter estimation using both ffun and hfun
    %                     from the underlying GSSM.
    %
    %    observ = hfun_parameter_bothp(InferenceDS, state, N, U2)
    %
    %    INPUT
    %         InferenceDS     : (InferenceDS) Inference data structure
    %         state           : (c-vector) meta system state vector
    %         N               : (c-vector) meta system observation noise vector
    %         U2              : (c-vector) meta system exogenous input 2
    %    OUTPUT
    %         observ          : (c-vector) meta system observation vector
    %
    % Relationship arguments and external model (GSSM) variables
    %
    %   state  -> external model parameters or a subset (specified by InferenceDS.paramParamIdxVec) thereof
    %   U2     -> [external_state(k-1) external_U1(k-1) external_state(k) external_U2(k)]'
    %   N      -> [external_process_noise(k-1) external_observation_noise(k)]'
    %   observ -> [external_state(k) external_observation(k)]'

    [dim,nov] = size(state);

    observ = zeros(InferenceDS.obsdim,nov);

    dimX  = InferenceDS.model.statedim;
%    dimO  = InferenceDS.model.obsdim;
    dimV  = InferenceDS.model.Vdim;
    dimN  = InferenceDS.model.Ndim;
    dimU1 = InferenceDS.model.U1dim;
%    dimU2 = InferenceDS.model.U2dim;

    ext_state_1     = U2(1:dimX,:);
    ext_proc_noise  = N(1:dimV,:);
    ext_U1          = U2(dimX+1:dimX+dimU1,:);
    ext_state_2     = U2(dimX+dimU1+1:dimX+dimU1+dimX,:);
    ext_obs_noise   = N(dimV+1:dimV+dimN,:);
    ext_U2          = U2(dimX+dimU1+dimX+1:end,:);

    ffun_idx = InferenceDS.paramFFunOutIdxVec;
    hfun_idx = InferenceDS.paramHFunOutIdxVec;

    dimF0 = length(ffun_idx);
    dimH0 = length(hfun_idx);

    % loop over all input vectors
    for k=1:nov,
        % set model parameter vector
        InferenceDS.model = InferenceDS.model.setparams( InferenceDS.model, state(:,k), InferenceDS.paramParamIdxVec);
        % FFUN part of observation
        FFunOut = InferenceDS.model.ffun( InferenceDS.model, ext_state_1(:,k), ext_proc_noise(:,k), ext_U1(:,k));
        % HFUN part of observation
        HFunOut = InferenceDS.model.hfun( InferenceDS.model, ext_state_2(:,k), ext_obs_noise(:,k), ext_U2(:,k));
        observ(1:dimF0,k) = FFunOut(ffun_idx);
        observ(dimF0+1:dimF0+dimH0,k) = HFunOut(hfun_idx);
    end

%-------------------------------------------------------------------------------------
function innov = innovation_parameter_bothp(InferenceDS, obs, observ)

    %  INNOVATION_PARAMETER_BOTHP  Calculates the innovation signal (difference) between the
    %   output of HFUN, i.e. OBSERV (the predicted system observation) and an actual
    %   'real world' observation OBS.
    %
    %    innov = innovation_parameter_bothp(InferenceDS, obs, observ)
    %
    %    INPUT
    %         InferenceDS     : (InferenceDS) Inference data structure
    %         obs             : (c-vector)  real-world observation vector
    %         observ          : (c-vector)  meta system observation vector
    %    OUTPUT
    %         inov            : (c-vector) innovation sequence

    [dim,nov] = size(observ);

    ffun_idx = InferenceDS.paramFFunOutIdxVec;

    dimF0 = length(ffun_idx);

    innov=zeros(InferenceDS.obsdim*nov);

    innov(1:dimF0,:) = obs(1:dimF0,:) - observ(1:dimF0,:);
    innov(dimF0+1:obsdim,:) = InferenceDS.model.innovation( InferenceDS.model, obs(dimF0+1:obsdim,:), ...
                                    observ(dimF0+1:obsdim,:));

%-------------------------------------------------------------------------------------
function llh = likelihood_parameter_bothp(InferenceDS, obs, state, U2, oNoiseDS)

    %  LIKELIHOOD_PARAMETER_BOTHP  Calculates the likelood of a real-world observation obs given
    %                           a realization of the predicted observation for a given state,
    %                           i.e. p(y|x) = p(obs|state)
    %
    %    llh = likelihood_parameter_bothp(InferenceDS, obs, observ)
    %
    %    INPUT
    %         InferenceDS     : (InferenceDS) Inference data structure
    %         obs             : (c-vector)  real-world observation vector
    %         state           : (c-vector)  meta system state vector
    %         U2              : (c-vector) meta system exogenous input 2
    %         oNoiseDS        : (NoiseDS)   observation noise data structure
    %    OUTPUT
    %         llh             : scalar  likelihood

    [dim,nov] = size(state);

    llh = zeros(1,nov);

    dimX  = InferenceDS.model.statedim;
%    dimO  = InferenceDS.model.obsdim;
    dimU1 = InferenceDS.model.U1dim;
%    dimU2 = InferenceDS.model.U2dim;

    ext_state_1     = U2(1:dimX,:);
%    ext_U1          = U2(dimX+1:dimX+dimU1,:);
    ext_state_2     = U2(dimX+dimU1+1:dimX+dimU1+dimX,:);
    ext_U2          = U2(dimX+dimU1+dimX+1:end,:);

    ffun_idx = InferenceDS.paramFFunOutIdxVec;
    hfun_idx = InferenceDS.paramHFunOutIdxVec;

    dimF0 = length(ffun_idx);
    dimH0 = length(hfun_idx);

    ext_nextstate = obs(1:dimF0,:);
    ext_obs = obs(dimF0+1:dimF0+dimH0,:);

    % loop over all input vectors
    for k=1:nov,

        % set model parameter vector
        InferenceDS.model = InferenceDS.model.setparams( InferenceDS.model, state(:,k), InferenceDS.paramParamIdxVec);

        % FFUN part of likelihood
        llh_f = InferenceDS.model.prior( InferenceDS.model, ext_nextstate(:,k), ext_state_1(:,k), oNoiseDS.noiseSources{1});

        % HFUN part of likelihood
        llh_h = InferenceDS.model.likelihood( InferenceDS.model, ext_obs(:,k), ext_state_2(:,k), ext_U2(:,k), oNoiseDS.noiseSources{2});

        llh(k) = llh_f * llh_h;       % we assume independence

    end


%-------------------------------------------------------------------------------------
function observ = hfun_parameter_f(InferenceDS, state, N, U2)

    %  HFUN_PARAMETER_F   State observation function of meta system for parameter estimation using only ffun
    %                     from the underlying GSSM.
    %
    %    observ = hfun_parameter_f(InferenceDS, state, N, U2)
    %
    %    INPUT
    %         InferenceDS     : (InferenceDS) Inference data structure
    %         state           : (c-vector) meta system state vector
    %         N               : (c-vector) meta system observation noise vector
    %         U2              : (c-vector) meta system exogenous input 2
    %    OUTPUT
    %         observ          : (c-vector) meta system observation vector
    %
    % Relationship between arguments and external model (GSSM) variables
    %
    %   state  -> external model parameters or a subset (specified by InferenceDS.paramParamIdxVec) thereof
    %   U2     -> [external_state(k-1) external_U1(k-1)]'
    %   N      -> [external_process_noise(k-1)]'
    %   observ -> [external_state(k)]'

    [dim,nov] = size(state);

    observ = zeros(InferenceDS.obsdim,nov);

    dimX  = InferenceDS.model.statedim;
    dimV  = InferenceDS.model.Vdim;
    dimU1 = InferenceDS.model.U1dim;

    ext_state_1     = U2(1:dimX,:);
    ext_proc_noise  = N(1:dimV,:);
    ext_U1          = U2(dimX+1:dimX+dimU1,:);

    ffun_idx = InferenceDS.paramFFunOutIdxVec;

%    dimF0 = length(ffun_idx);

    % loop over all input vectors
    for k=1:nov,
        % set model parameter vector
        InferenceDS.model = InferenceDS.model.setparams( InferenceDS.model, state(:,k), InferenceDS.paramParamIdxVec);
        FFunOut  = InferenceDS.model.ffun( InferenceDS.model, ext_state_1(:,k), ext_proc_noise(:,k), ext_U1(:,k));
        observ(:,k) = FFunOut(ffun_idx);
    end

%-------------------------------------------------------------------------------------
function observ = hfun_parameter_h(InferenceDS, state, N, U2)

    %  HFUN_PARAMETER_H   State observation function of meta system for parameter estimation using only hfun
    %                     from the underlying GSSM.
    %
    %    observ = hfun_parameter_h(InferenceDS, state, N, U2)
    %
    %    INPUT
    %         InferenceDS     : (InferenceDS) Inference data structure
    %         state           : (c-vector) system state vector
    %         N               : (c-vector) observation noise vector
    %         U2              : (c-vector) exogenous input 2
    %    OUTPUT
    %         observ          : (c-vector) observation vector
    %
    % Relationship between input arguments and external model (GSSM) variables
    %
    %   state  -> external model parameters or a subset (specified by InferenceDS.paramParamIdxVec) thereof
    %   U2     -> [external_state(k) external_U2(k)]'
    %   N      -> [external_observation_noise(k)]'
    %   observ -> [external_observation(k)]'

    [dim,nov] = size(state);

    observ = zeros(InferenceDS.obsdim,nov);

    dimX  = InferenceDS.model.statedim;
%    dimO  = InferenceDS.model.obsdim;
    dimN  = InferenceDS.model.Ndim;
    dimU2 = InferenceDS.model.U2dim;

    ext_state_2     = U2(1:dimX,:);
    ext_U2          = U2(dimX+1:dimX+dimU2,:);
    ext_obs_noise   = N(1:dimN,:);

    hfun_idx = InferenceDS.paramHFunOutIdxVec;