geninfds.m

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

                InferenceDS.innovation = [];
            end

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


        %...................................................................................................................
        case 'both-p'

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

            %--- ffun/hfun types ---
            InferenceDS.ffun_type = 'lti';                                    % state transition function type
            if stringmatch('nl',{model.ffun_type, model.hfun_type})      % state observation function type
                InferenceDS.hfun_type = 'nl';
            elseif stringmatch('nla',{model.ffun_type, model.hfun_type})
                InferenceDS.hfun_type = 'nla';
            else
                InferenceDS.hfun_type = 'ltv';
            end

            %--- functions ---
            InferenceDS.ffun = @ffun_parameter;                               % state transition function functionhandle
            InferenceDS.hfun = @hfun_parameter_bothp;                            % state observation function functionhandle
            if isfield(model,'linearize')
                InferenceDS.linearize = @linearize_parameter_bothp;                  % linearization function functionhandle
            else
                InferenceDS.linearize = @linearize_generic;
            end
            InferenceDS.prior = @prior_parameter;
            if (isfield(model,'likelihood') & isfield(model,'prior')),
                InferenceDS.likelihood = @likelihood_parameter_bothp;
            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
            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;                                            % expgenous 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

            %--- ffun/hfun types ---
            InferenceDS.ffun_type = 'lti';                                    % state transition function type
            if stringmatch('nl',{model.ffun_type})                       % state observation function type;
                InferenceDS.hfun_type = 'nl';
            elseif stringmatch('nla',{model.ffun_type})
                InferenceDS.hfun_type = 'nla';
            else
                InferenceDS.hfun_type = 'ltv';
            end

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

            %--- ffun/hfun types ---
            InferenceDS.ffun_type = 'lti';                                    % state transition function type
            if stringmatch('nl',{model.hfun_type})                            % state observation function type;
                InferenceDS.hfun_type = 'nl';
            elseif stringmatch('nla',{model.hfun_type})
                InferenceDS.hfun_type = 'nla';
            else
                InferenceDS.hfun_type = 'ltv';
            end

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

        %--- ffun/hfun types ---
        if stringmatch(model.ffun_type,{'nla','ltv','lti'})          % state observation function type if additive process
           InferenceDS.ffun_type = 'nla';                                 % noise
        else
           InferenceDS.ffun_type = 'nl';                                  % otherwise, default to full nonlinear
        end
        InferenceDS.hfun_type = model.hfun_type;                          % state observation function type;

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