geninfds.m

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

            D(1:dimH0,dimX+dimU1+1:end) = extD(hfun_idx,:);
            varargout{k} = D;

        %--- H = dhfun/dn
        case 'H'
            H = zeros(InferenceDS.obsdim, InferenceDS.Ndim);
            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');
            extG = model.linearize( model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'G');
            extH = model.linearize( model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'H');
            tempCG = extC*extG;
            H(1:dimH0,1:dimV) = tempCG(hfun_idx,:);
            H(1:dimH0,dimV+1:end) = extH(hfun_idx,:);
            varargout{k} = H;

        %----
        otherwise
            error('[ InferenceDS.linearize ] Unknown linearization term.');

        end

    end


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

    %  LINEARIZE_PARAMETER_BOTHP  Linearization function of meta system for parameter estimation using both ffun
    %                          and hfun from the underlying GSSM.
    %
    %    varargout = linearize_parameter_bothp(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_state(k) 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 according 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_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);

    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);
            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);
            C(1:dimF0,:) = extJFW(ffun_idx,:);
            C(dimF0+1:dimF0+dimH0,:) = extJHW(hfun_idx,:);
            varargout{k} = C;

        %--- D = dhfun/dU2
        case 'D'
            D = zeros(InferenceDS.obsdim, InferenceDS.U2dim);
            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');
            D(1:dimF0,1:dimX) = extA(ffun_idx,:);
            D(1:dimF0,dimX+1:dimX+dimU1) = extB(ffun_idx,:);
            D(dimF0+1:dimF0+dimH0,dimX+dimU1+1:dimX+dimU1+dimX) = extC(hfun_idx,:);
            D(dimF0+1:dimF0+dimH0,dimX+dimU1+dimX+1:end) = extD(hfun_idx,:);
            varargout{k} = D;

        %--- H = dhfun/dn
        case 'H'
            H = zeros(InferenceDS.obsdim, InferenceDS.Ndim);
            extG = model.linearize( model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'G');
            extH = model.linearize( model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'H');
            H(1:dimF0,1:dimV) = extG(ffun_idx,:);
            H(dimF0+1:dimF0+dimH0,dimV+1:end) = extH(hfun_idx,:);
            varargout{k} = H;

        %----
        otherwise
            error('[ InferenceDS.linearize ] Unknown linearization term.');

        end

    end

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

    %  LINEARIZE_PARAMETER_F  Linearization function of meta system for parameter estimation using only
    %                         ffun from the underlying GSSM.
    %
    %    varargout = linearize_parameter_f(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)]'
    %   V     -> synthetic process noise (speeds up convergence)
    %   N     -> [external_process_noise(k-1)]'


    % 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;
    dimV  = model.Vdim;
    dimU1 = model.U1dim;

    if isempty(U2)
        ext_state_1     = [];
        ext_U1          = [];
    else
        ext_state_1     = U2(1:dimX);
        ext_U1          = U2(dimX+1:dimX+dimU1);
    end
    if isempty(N),
        ext_proc_noise  = [];
    else
        ext_proc_noise  = N(1:dimV);
    end

    ffun_idx = InferenceDS.paramFFunOutIdxVec;

    dimF0 = length(ffun_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);
            extJFW = model.linearize( model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'JFW', InferenceDS.paramParamIdxVec);
            C = extJFW(ffun_idx,:);
            varargout{k} = C;

        %--- D = dhfun/dU2
        case 'D'
            D = zeros(InferenceDS.obsdim, InferenceDS.U2dim);
            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');
            D(1:dimF0,1:dimX) = extA(ffun_idx,:);
            D(1:dimF0,dimX+1:dimX+dimU1) = extB(ffun_idx,:);
            varargout{k} = D;

        %--- H = dhfun/dn
        case 'H'
            H = zeros(InferenceDS.obsdim, InferenceDS.Ndim);
            extG = model.linearize( model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'G');
            H(1:dimF0,1:dimV) = extG(ffun_idx,:);
            varargout{k} = H;

        %---
        otherwise
            error('[ InferenceDS.linearize ] Unknown linearization term.');

        end

    end


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

    %  LINEARIZE_PARAMETER_H  Linearization function of meta system for parameter estimation using
    %                         only hfun from the underlying GSSM.
    %
    %    varargout = linearize_parameter_h(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) external_U2(k)]'
    %   V     -> synthetic process noise (speeds up convergence)
    %   N     -> [external_observation_noise(k)]'


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

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

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

    hfun_idx = InferenceDS.paramHFunOutIdxVec;

    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} = Infe