geninfds.m

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

            InferenceDS.stateAngleCompIdxVec = [];
            InferenceDS.obsAngleCompIdxVec = [];
            if isfield(model,'stateAngleCompIdxVec'),
               for k=1:length(model.stateAngleCompIdxVec),
                 idx = find(InferenceDS.paramFFunOutIdxVec == model.stateAngleCompIdxVec(k));
                 InferenceDS.obsAngleCompIdxVec = [InferenceDS.obsAngleCompIdxVec idx];
               end
            end
            if isfield(model,'obsAngleCompIdxVec'),
               for k=1:length(model.obsAngleCompIdxVec),
                 idx = find(InferenceDS.paramHFunOutIdxVec == model.obsAngleCompIdxVec(k));
                 InferenceDS.obsAngleCompIdxVec = [InferenceDS.obsAngleCompIdxVec idx];
               end
            end


        %...................................................................................................................
        case 'ffun'

            %--- parameter dimensions ---
            InferenceDS.statedim = length(paramParamIdxVec);                  % state dimension
            InferenceDS.obsdim = length(paramFFunOutIdxVec);                  % observation dimension
            InferenceDS.U1dim = 0;                                            % exogenous input 1 dimension
            InferenceDS.U2dim = model.U1dim + model.statedim;                 % exogenous input 2 dimension
            InferenceDS.Vdim = InferenceDS.statedim;                          % process noise dimension
            InferenceDS.Ndim = model.Vdim;                                    % observation noise dimension

            %--- functions ---
            InferenceDS.ffun = @ffun_parameter;                               % state transition function functionhandle
            InferenceDS.hfun = @hfun_parameter_f;                             % state observation function functionhandle
            if isfield(model,'linearize')
                InferenceDS.linearize = @linearize_parameter_f;               % linearization function functionhandle
            else
                InferenceDS.linearize = @linearize_generic;
            end
            InferenceDS.prior = @prior_parameter;
            if isfield(model,'prior'),
                InferenceDS.likelihood = @likelihood_parameter_f;
            end
            InferenceDS.innovation = [];

            %--- copy/setup fixed linear model parameters ---
            InferenceDS.A         = eye(InferenceDS.statedim);
            InferenceDS.B         = [];
            InferenceDS.G         = eye(InferenceDS.statedim);

            %--- other stuff ---
            % Index vectors indicating the presence of angular components in the state and observation vectors
            InferenceDS.stateAngleCompIdxVec = [];
            InferenceDS.obsAngleCompIdxVec = [];
            if isfield(model,'stateAngleCompIdxVec'),
               for k=1:length(model.stateAngleCompIdxVec),
                 idx = find(InferenceDS.paramFFunOutIdxVec == model.stateAngleCompIdxVec(k));
                 InferenceDS.obsAngleCompIdxVec = [InferenceDS.obsAngleCompIdxVec idx];
               end
            end


        %...................................................................................................................
        case 'hfun'

            %--- parameter dimensions ---
            InferenceDS.statedim = length(paramParamIdxVec);                  % state dimension
            InferenceDS.obsdim = length(paramHFunOutIdxVec);                  % observation dimension
            InferenceDS.U1dim = 0;                                            % exogenous input 1 dimension
            InferenceDS.U2dim = model.U2dim + model.statedim;                 % exogenous input 2 dimension
            InferenceDS.Vdim = InferenceDS.statedim;                          % process noise dimension
            InferenceDS.Ndim = model.Ndim;                                    % observation noise dimension

            %--- functions ---
            InferenceDS.ffun      = @ffun_parameter;                          % state transition function functionhandle
            InferenceDS.hfun      = @hfun_parameter_h;                        % state observation function functionhandle
            if isfield(model,'linearize')
                InferenceDS.linearize = @linearize_parameter_h;               % linearization function functionhandle
            else
                InferenceDS.linearize = @linearize_generic;
            end
            InferenceDS.prior = @prior_parameter;
            if isfield(model,'likelihood'),
                InferenceDS.likelihood = @likelihood_parameter_h;
            end
            InferenceDS.innovation = [];

            %--- copy/setup fixed linear model parameters ---
            InferenceDS.A         = eye(InferenceDS.statedim);
            InferenceDS.B         = [];
            InferenceDS.G         = eye(InferenceDS.statedim);


            %--- other stuff ---
            % Index vectors indicating the presence of angular components in the state and observation vectors
            InferenceDS.stateAngleCompIdxVec = [];
            InferenceDS.obsAngleCompIdxVec = [];
            if isfield(model,'obsAngleCompIdxVec'),
               for k=1:length(model.obsAngleCompIdxVec),
                 idx = find(InferenceDS.paramHFunOutIdxVec == model.obsAngleCompIdxVec(k));
                 InferenceDS.obsAngleCompIdxVec = [InferenceDS.obsAngleCompIdxVec idx];
               end
            end


        otherwise
            error(' The only valid values for the funselect field are : ''both'' , ''ffun'' and ''hfun''.');

        end



    %---
    %--- JOINT ESTIMATION --------------------------------------------------------------------------------
    %---
    case 'joint'

        % Check parameter index vector
        if ~isfield(ArgDS,'paramParamIdxVec')
            paramParamIdxVec = 1:model.paramdim;
        else
            paramParamIdxVec = ArgDS.paramParamIdxVec;
            % Check vector entries
            if ((max(paramParamIdxVec) > model.paramdim) | (min(paramParamIdxVec) < 1))
                error(' [ geninfds::parameter ] Parameter index vector has illegal entries');
            end
            % Check for duplicate index entries
            if checkdups(paramParamIdxVec)
                error(' [ geninfds::parameter ] Duplicate parameter index vector entries not allowed.');
            end
        end
        InferenceDS.paramParamIdxVec = paramParamIdxVec;                  % copy index vector in InferenceDS

        %--- dimensions ---
        pdim = length(paramParamIdxVec);
        InferenceDS.paramParamIdxVec = paramParamIdxVec;                  % save index vector in InferenceDS
        InferenceDS.statedim  = model.statedim + pdim;                    % state dimension
        InferenceDS.obsdim    = model.obsdim;                             % observation dimension
        InferenceDS.U1dim     = model.U1dim;                              % exogenous input 1 dimension
        InferenceDS.U2dim     = model.U2dim;                              % exogenous input 2 dimension
        InferenceDS.Vdim      = model.Vdim + pdim;                        % process noise dimension
        InferenceDS.Ndim      = model.Ndim;                               % observation noise dimension

        %--- functions ---
        InferenceDS.ffun      = @ffun_joint;                              % state transition function functionhandle
        InferenceDS.hfun      = @hfun_joint;                              % state observation function functionhandle
        if isfield(model, 'prior'),
            InferenceDS.prior = @prior_joint;
        end
        if isfield(model, 'likelihood')
            InferenceDS.likelihood = @likelihood_joint;
        end
        if isfield(model, 'innovation')
            InferenceDS.innovation = @innovation_state;
        else
            InferenceDS.innovation = [];
        end
        if isfield(model, 'linearize')
            InferenceDS.linearize = @linearize_joint;                     % linearization function functionhandle
        else
            InferenceDS.linearize = @linearize_generic;
        end

        %--- other stuff ---
        % Index vectors indicating the presence of angular components in the state and observation vectors
        if isfield(model,'stateAngleCompIdxVec'),
            InferenceDS.stateAngleCompIdxVec = model.stateAngleCompIdxVec;
        else
            InferenceDS.stateAngleCompIdxVec = [];
        end
        if isfield(model,'obsAngleCompIdxVec'),
            InferenceDS.obsAngleCompIdxVec = model.obsAngleCompIdxVec;
        else
            InferenceDS.obsAngleCompIdxVec = [];
        end


%-----------------------------------------------------------------------------------------------------------------------------------------------
otherwise

  error([' [ geninfds ] Inference type ''' ArgDS.type ''' not supported.']);

end


% add misc. default data fields

InferenceDS.uType = 'TMU';             % Update type : Time-and-Measurement Update is the default
                                       % Other options are : TU - time update only or MU - measurement update only.



return;



%***********************************************************************************************
%***                                                                                         ***
%***                               SUB FUNCTION BLOCK                                        ***
%***                                                                                         ***
%***********************************************************************************************


%===============================================================================================
%================================== STATE ESTIMATION FUNCTIONS =================================

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

    %  FFUN_STATE  State transition function of meta system for state estimation
    %
    %    new_state = ffun_state(InferenceDS, state, V, U1)
    %
    %    INPUT
    %         InferenceDS     : (InferenceDS) Inference data structure
    %         state           : (c-vector) meta 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

    new_state = InferenceDS.model.ffun( InferenceDS.model, state, V, U1);

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

    %  HFUN_STATE  State observation function of meta system for state estimation
    %
    %    observ = hfun_state(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

    observ = InferenceDS.model.hfun( InferenceDS.model, state, N, U2);

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

    %  PRIOR_STATE  Calculates the transition prior probability P(x_k|x_(k-1))
    %
    %    tranprior = prior_state(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))

    tran_prior = InferenceDS.model.prior( InferenceDS.model, nextstate, state, U1, pNoiseDS);


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

    %  LIKELIHOOD_STATE  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_state(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

    llh = InferenceDS.model.likelihood( InferenceDS.model, obs, state, U2, oNoiseDS);

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

    %  INNOVATION_STATE  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.
    %
    %    innov = innovation_state(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

    innov = InferenceDS.model.innovation( InferenceDS.model, obs, observ);

%-------------------------------------------------------------------------------------
function varargout = linearize_state(InferenceDS, state, V, N, U1, U2, varargin)