geninfds.m

Matlab toolbox that contains functions of Kalman filter and random system simulation.

    %  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 = feval(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 = feval(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 = feval(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 = feval(InferenceDS.model.innovation, InferenceDS.model, obs, observ);

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

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

      switch varargin{k}

      case 'A'
          varargout{k} = feval(InferenceDS.model.linearize, InferenceDS.model, state, V, N, U1, U2, 'A');

       case 'B'
          varargout{k} = feval(InferenceDS.model.linearize, InferenceDS.model, state, V, N, U1, U2, 'B');

       case 'G'
          varargout{k} = feval(InferenceDS.model.linearize, InferenceDS.model, state, V, N, U1, U2, 'G');

       case 'C'
          varargout{k} = feval(InferenceDS.model.linearize, InferenceDS.model, state, V, N, U1, U2, 'C');

       case 'D'
          varargout{k} = feval(InferenceDS.model.linearize, InferenceDS.model, state, V, N, U1, U2, 'D');

       case 'H'
          varargout{k} = feval(InferenceDS.model.linearize, InferenceDS.model, state, V, N, U1, U2, 'H');

      end


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

    dimFO = length(ffun_idx);
    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);
        % FFUN part of observation
        FFunOut = feval(InferenceDS.model.ffun, InferenceDS.model, ext_state_1(:,k), ext_proc_noise(:,k), ext_U1(:,k));
        % HFUN part of observation
        HFunOut = feval(InferenceDS.model.hfun, InferenceDS.model, ext_state_2(:,k), ext_obs_noise(:,k), ext_U2(:,k));
        observ(1:dimFO,k) = FFunOut(ffun_idx);
        observ(dimFO+1:dimFO+dimHO,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;
    hfun_idx = InferenceDS.paramHFunOutIdxVec;

    dimFO = length(ffun_idx);
    dimHO = length(hfun_idx);

    innov=zeros(InferenceDS.obsdim*nov);

    innov(1:dimF0,:) = obs(1:dimF0,:) - observ(1:dimF0,:);
    innov(dimF0+1:obsdim,:) = feval(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;

    dimFO = length(ffun_idx);
    dimHO = length(hfun_idx);

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