geninfds.m

来自「递归贝叶斯估计的工具包」· M 代码 · 共 1,458 行 · 第 1/5 页

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

    dimHO = length(hfun_idx);

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

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

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

        llh(k) = feval(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 = feval(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;

    dimFO = length(ffun_idx);
    dimHO = 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 = feval(model.ffun, model, ext_state_1, ext_proc_noise, ext_U1);
            extC = feval(model.linearize, model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'C');
            extJFW = feval(model.linearize, model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'JFW', InferenceDS.paramParamIdxVec);
            extJHW = feval(model.linearize, model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'JHW', InferenceDS.paramParamIdxVec);
            Ctemp = extC*extJFW + extJHW;
            C(1:dimHO,:) = Ctemp(hfun_idx,:)
            varargout{k} = C;

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

        %--- H = dhfun/dn
        case 'H'
            H = zeros(InferenceDS.obsdim, InferenceDS.Ndim);
            ext_state_2 = feval(model.ffun, model, ext_state_1, ext_proc_noise, ext_U1);
            extC = feval(model.linearize, model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'C');
            extG = feval(model.linearize, model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'G');
            extH = feval(model.linearize, model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'H');
            tempCG = extC*extG;
            H(1:dimHO,1:dimV) = tempCG(hfun_idx,:);
            H(1:dimHO,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 = feval(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_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;

    dimFO = length(ffun_idx);
    dimHO = 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 = feval(model.linearize, model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'JFW', InferenceDS.paramParamIdxVec);
            extJHW = feval(model.linearize, model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'JHW', InferenceDS.paramParamIdxVec);
            C(1:dimFO,:) = extJFW(ffun_idx,:);
            C(dimFO+1:dimFO+dimHO,:) = extJHW(hfun_idx,:);
            varargout{k} = C;

        %--- D = dhfun/dU2
        case 'D'
            D = zeros(InferenceDS.obsdim, InferenceDS.U2dim);
            extA = feval(model.linearize, model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'A');
            extB = feval(model.linearize, model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'B');
            extC = feval(model.linearize, model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'C');
            extD = feval(model.linearize, model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'D');
            D(1:dimFO,1:dimX) = extA(ffun_idx,:);
            D(1:dimFO,dimX+1:dimX+dimU1) = extB(ffun_idx,:);
            D(dimFO+1:dimFO+dimHO,dimX+dimU1+1:dimX+dimU1+dimX) = extC(hfun_idx,:);
            D(dimFO+1:dimFO+dimHO,dimX+dimU1+dimX+1:end) = extD(hfun_idx,:);
            varargout{k} = D;

        %--- H = dhfun/dn
        case 'H'
            H = zeros(InferenceDS.obsdim, InferenceDS.Ndim);
            extG = feval(model.linearize, model, ext_state_1, ext_proc_noise, [], ext_U1, [], 'G');
            extH = feval(model.linearize, model, ext_state_2, [], ext_obs_noise, [], ext_U2, 'H');
            H(1:dimFO,1:dimV) = extG(ffun_idx,:);
            H(dimFO+1:dimFO+dimHO,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