geninfds.m

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

%    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);
       HFunOut = InferenceDS.model.hfun( InferenceDS.model, ext_state_2(:,k), ext_obs_noise(:,k), ext_U2(:,k));
       observ(:,k) = HFunOut(hfun_idx);
    end

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

    %  HFUN_PARAMETER_BOTH   State observation function of meta system for parameter estimation using the full system
    %                        dynamics of the underlying GSSM as observation, i.e. observ=hfun(ffun(x))
    %
    %
    %    observ = hfun_parameter_both(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-1) external_U1(k-1) 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;
    dimV  = InferenceDS.model.Vdim;
    dimN  = InferenceDS.model.Ndim;
    dimU1 = InferenceDS.model.U1dim;
    dimU2 = InferenceDS.model.U2dim;

    ext_state_1     = U2(1:dimX,:);
    ext_U1          = U2(dimX+1:dimX+dimU1,:);
    ext_U2          = U2(dimX+dimU1+1:dimX+dimU1+dimU2,:);
    ext_obs_noise   = N(dimV+1:dimV+dimN,:);
    ext_proc_noise = N(1:dimV,:);
    
    hfun_idx = InferenceDS.paramHFunOutIdxVec;

%    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);
       % calculate X(k)=ffun(X(k-1))
       ext_state_2 = InferenceDS.model.ffun( InferenceDS.model, ext_state_1(:,k), ext_proc_noise(:,k), ext_U1(:,k));
       HFunOut = InferenceDS.model.hfun( InferenceDS.model, ext_state_2, ext_obs_noise(:,k), ext_U2(:,k));
       observ(:,k) = HFunOut(hfun_idx);
    end



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

    %  LIKELIHOOD_PARAMETER_F  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_f(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;

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

    ffun_idx = InferenceDS.paramFFunOutIdxVec;

    dimF0 = length(ffun_idx);

    ext_nextstate = obs(1:dimF0,:);

    % 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(k) = InferenceDS.model.prior( InferenceDS.model, ext_nextstate(:,k), ext_state_1(:,k), ext_U1, oNoiseDS);

    end


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

    %  LIKELIHOOD_PARAMETER_H  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_h(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;
%    dimU2 = InferenceDS.model.U2dim;

    ext_state_2     = U2(1:dimX,:);
    ext_U2          = U2(dimX+1:end,:);

%    hfun_idx = InferenceDS.paramHFunOutIdxVec;

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

        llh(k) = InferenceDS.model.likelihood( InferenceDS.model, obs(:,k), ext_state_2(:,k), ext_U2(:,k), oNoiseDS);

    end


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

    %  LIKELIHOOD_PARAMETER_BOTH  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_both(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_U2          = U2(dimX+dimU1+1:end,:);

%    hfun_idx = InferenceDS.paramHFunOutIdxVec;

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

        ext_state_2 = InferenceDS.model.ffun( InferenceDS.model, ext_state_1(:,k), [], ext_U1(:,k));

        llh(k) = InferenceDS.model.likelihood( InferenceDS.model, obs(:,k), ext_state_2, ext_U2(:,k), oNoiseDS);

    end

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

    %  LINEARIZE_PARAMETER_BOTH  Linearization function of meta system for parameter estimation using both ffun
    %                            and hfun from the underlying GSSM in a
    %                            cascading (i.e. y=hfun(ffun(state,U1,V),U2,N))
    %
    %    varargout = linearize_parameter_both(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
    %
    % 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
    %   U2     -> [external_state(k-1) external_U1(k-1) external_U2(k)]'
    %   V     -> synthetic process noise (speeds up convergence)
    %   N     -> [external_process_noise(k-1) external_observation_noise(k)]'


    % Setup temporary model to use for linearization purposes
    model = InferenceDS.model;                                                      % copy existing model
    if ~isempty(state),
        model = model.setparams( model, state, InferenceDS.paramParamIdxVec);   % set parameters acording to state variable
    end

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

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

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

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

    for k=1:length(varargin)

        switch varargin{k}

        %--- A = dffun/dstate
        case 'A'
            varargout{k} = InferenceDS.A;

        %--- B = dffun/dU1
        case 'B'
            varargout{k} = InferenceDS.B;

        %--- G = dffun/dv
        case 'G'
            varargout{k} = InferenceDS.G;

        %--- C = dhfun/dstate
        case 'C'
            C = zeros(InferenceDS.obsdim, InferenceDS.statedim);
            ext_state_2 = model.ffun( model, ext_state_1, ext_proc_noise, ext_U1);
            extC = model.linearize( model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'C');
            extJFW = model.linearize( model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'JFW', InferenceDS.paramParamIdxVec);
            extJHW = model.linearize( model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'JHW', InferenceDS.paramParamIdxVec);
            Ctemp = extC*extJFW + extJHW;
            C(1:dimH0,:) = Ctemp(hfun_idx,:);
            varargout{k} = C;

        %--- D = dhfun/dU2
        case 'D'
            D = zeros(InferenceDS.obsdim, InferenceDS.U2dim);
            ext_state_2 = model.ffun( model, ext_state_1, ext_proc_noise, ext_U1);
            extA = model.linearize( model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'A');
            extB = model.linearize( model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'B');
            extC = model.linearize( model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'C');
            extD = model.linearize( model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'D');
            tempCA = extC*extA;
            tempCB = extC*extB;
            D(1:dimH0,1:dimX) = tempCA(hfun_idx,:);
            D(1:dimH0,dimX+1:dimX+dimU1) = tempCB(hfun_idx,:);