gensysnoiseds.m

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

function [pNoise, oNoise, InferenceDS] = gensysnoiseds(InferenceDS, estimatorType, pNoiseAdaptMethod, pNoiseAdaptParams, ...
                                          oNoiseAdaptMethod, oNoiseAdaptParams)

% GENSYSNOISEDS  Generate process and observation noise data structures for a given InferenceDS data structure
%                and algorithm type. All ReBEL estimation algorithms take an inference data structure (InferenceDS),
%                as well as two system noise data structures (process noise and observation noise) as arguments.
%
%   [pNoise, oNoise] = gensysnoiseds(InferenceDS, estimatorType, pNoiseAdaptMethod, pNoiseAdaptParams, oNoiseAdaptMethod, oNoiseAdaptParams))
%
%   INPUT
%          InferenceDS         (InferenceDS) Inference data structure generated from a GSSM file by 'geninfds'
%          estimatorType       (string) type of estimator to be used (i.e. 'kf', 'ukf', 'ekf', 'pf', etc.)
%          pNoiseAdaptMethod  <<optional>> (string) Process noise covariance adaptation method :
%                                      'anneal'        : annealing
%                                      'lambda-decay'  : RLS like lambda decay
%                                      'robbins-monro' : Robbins-Monro stochastic approximation
%                               If this field is set, then pNoiseAdaptParams must also be set.
%          pNoiseAdaptParams  <<optional>> (vector) noise adaptation parameters. Depend on pNoiseAdaptMethod
%                                 if 'anneal'        : [annealing_factor minimum_allowed_variance]
%                                 if 'lambda-decay'  : [lambda_factor minimum_allowed_variance]
%                                 if 'robbins-monro' : [1/nu_initial 1/nu_final]
%          oNoiseAdaptMethod  <<optional>> Observation noise covariance adaptation method : same as above
%                                          except the only allowed method is 'robbins-monro'
%          oNoiseAdaptParams  <<optional>> Same as above for process noise
%
%   OUTPUT
%          pNoise              (NoiseDS) process noise data structure
%          oNoise              (NoiseDS) observation noise data structure
%          InferenceDS         (InferenceDS) updated inference data structure
%
%     See also
%     GENINFDS, GENNOISEDS
%
%   Copyright (c) Oregon Health & Science University (2006)
%
%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
%   academic use only (see included license file) and can be obtained from
%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
%   software should contact [e-mail] for commercial licensing information.
%
%   See LICENSE (which should be part of the main toolkit distribution) for more
%   detail.

%=============================================================================================

%=== ERROR CHECKING ==========================================================================

if ((nargin < 2) | rem(nargin,2))
    error(' [ gensysnoiseds ] Not enough input parameters.');
end

if (nargout ~= 3)
    error(' [ gensysnoiseds ] Not enough output arguments.');
end


error(consistent(InferenceDS,'InferenceDS'));         %-- check for consistency of InferenceDS data structure

InferenceDS.esttype = estimatorType;                  % store estimator type


%=== INFERENCE TYPE SPECIFIC STRUCTURE ==================================================================

switch (InferenceDS.inftype)

%----------------------------------------- STATE ESTIMATION ---------------------------------------------
case 'state'

    %--- Generate/convert or copy noise sources from GSSM data structure

    pNoise = InferenceDS.model.pNoise;                            % process noise data structure
    oNoise = InferenceDS.model.oNoise;                            % observation noise data structure


    %--- KALMAN FILTER FAMILY : Checks and conversion

    if stringmatch(estimatorType, {'kf','ekf','ukf','cdkf','srukf','srcdkf'})

        % If default noise source is not Guassian, define a Gaussian noise source with the same dimension, mean and covariance
        % if available
        if ~stringmatch(pNoise.ns_type, {'gaussian','combo-gaussian'})
            Arg.type = 'gaussian';         % standard Gaussian noise source
            Arg.cov_type = 'full';          % with full covariance matrix
            Arg.dim = pNoise.dim;          % process noise dimension
            if isfield(pNoise,'mu')
              Arg.mu = pNoise.mu;
            else
              %warning(' [ gensysnoiseds ] Process noise data structure does not have a defined mean vector. Default assigned.');
              Arg.mu = zeros(Arg.dim,1);     % default : zero mean
            end
            if isfield(pNoise,'cov')
              Arg.cov = pNoise.cov;
            else
              %warning(' [ gensysnoiseds ] Process noise data structure does not have a defined covariance matrix. Default assigned.');
              Arg.cov  = eye(Arg.dim);         % default : covariance
            end
            pNoise = gennoiseds(Arg);      % generate process noise data structure
        end
        if ~stringmatch(oNoise.ns_type, {'gaussian','combo-gaussian'})
            Arg.type = 'gaussian';         % standard Gaussian noise source
            Arg.cov_type = 'full';          % with full covariance matrix
            Arg.dim = oNoise.dim;          % process noise dimension
            if isfield(oNoise,'mu')
              Arg.mu = oNoise.mu;
            else
              %warning(' [ gensysnoiseds ] Observation noise data structure does not have a defined mean vector. Default assigned.');
              Arg.mu = zeros(Arg.dim,1);     % default : zero mean
            end
            if isfield(oNoise,'cov')
              Arg.cov = oNoise.cov;
            else
              %warning(' [ gensysnoiseds ] Observation noise data structure does not have a defined covariance matrix. Default assigned.');
              Arg.cov  = eye(Arg.dim);         % default : covariance
            end
            oNoise = gennoiseds(Arg);      % generate observation noise data structure
        end

        if stringmatch(estimatorType, {'srukf','srcdkf'})   % Check for square root Kalman algorithms
            %-- process noise
            switch (pNoise.cov_type)                        % Determine cov_type of Gaussian noise source
            case 'diag'
                pNoise = convgausns(pNoise,'sqrt-diag');
                %warning(' [ gensysnoiseds ] Converting process noise source covariance type to ''sqrt-diag''.');
            case 'full'
                pNoise = convgausns(pNoise,'sqrt');
                %warning(' [ gensysnoiseds ] Converting process noise source covariance type to ''sqrt''.');
            end
            %-- observation noise
            switch (oNoise.cov_type)                        % Determine cov_type of Gaussian noise source
            case 'diag'
                oNoise = convgausns(oNoise,'sqrt-diag');
                %warning(' [ gensysnoiseds ] Converting observation noise source covariance type to ''sqrt-diag''.');
            case 'full'
                oNoise = convgausns(oNoise,'sqrt');
                %warning(' [ gensysnoiseds ] Converting observation noise source covariance type to ''sqrt''.');
            end
        end

        InferenceDS.InovUpdateMaskIdxVec = []; % Innovation update mask index vector ... Indicates which components
                                               % of the innovation vector should be ignored when calculating a Kalman
                                               % state update

    end


    %--------------------------------------------------------------------------------------------
    %--- PARTICLE FILTER FAMILY : Checks and conversion


    %----------------------------------------------------------------------------------
    if stringmatch(estimatorType, 'gspf')   % 'Gaussian Sum Particle Filter'

        % If process noise source is not a GMM, define a GMM noise source with the same dimension, mean and covariance
        % if available
        if ~stringmatch(pNoise.ns_type, 'gmm')

            Arg.type = 'gmm';              % GMM noise source
            Arg.cov_type = 'sqrt';         % GSPF use square-root covariance matrices
            Arg.dim = pNoise.dim;          % process noise dimension
            Arg.M = 1;                     % single component
            Arg.weights = [1];             % component weight
            if isfield(pNoise,'mu')
              Arg.mu = pNoise.mu;
            else
              %warning(' [ gensysnoiseds ] Process noise data structure does not have a defined mean vector. Default assigned');
              Arg.mu = zeros(Arg.dim,1);     % default : zero mean
            end
            Arg.cov = repmat(zeros(Arg.dim),[1 1 1]); % default covariance buffer

            if isfield(pNoise,'cov')
              if isfield(pNoise,'cov_type') cov_type = pNoise.cov_type; else cov_type='full'; end
              switch cov_type
              case {'sqrt','sqrt-diag'}
                  Arg.cov(:,:,1) = pNoise.cov;
              case {'full','diag'}
                  Arg.cov(:,:,1) = chol(pNoise.cov)';
              otherwise
                  error('[ gensysnoiseds::gspf ] Unknown process noise covariance type.');
              end
            else
              warning(' [ gensysnoiseds::gspf ] Process noise data structure does not have a defined covariance matrix. Default assigned.');
              Arg.cov(:,:,1) = repmat(eye(Arg.dim),[1 1 1]);  % default : covariance  (Cholesky factor)
            end
            pNoise = gennoiseds(Arg);      % generate process noise data structure

        else

        % Make sure the GMM component densities is of cov_type 'sqrt'

            %-- process noise
            switch (pNoise.cov_type)                        % Determine cov_type of Gaussian noise source
            case 'diag'
                pNoise = convgausns(pNoise,'sqrt-diag');
                %warning(' [ gensysnoiseds ] Converting process noise source covariance type to ''sqrt-diag''.');
            case 'full'
                pNoise = convgausns(pNoise,'sqrt');
                %warning(' [ gensysnoiseds ] Converting observation noise source covariance type to ''sqrt''.');
            end

        end

    end


    %----------------------------------------------------------------------------------
    if stringmatch(estimatorType, 'gmsppf')   % 'Gaussian Mixture Sigma-Point Particle Filter'

        % If process noise source is not a GMM, define a GMM noise source with the same dimension, mean and covariance
        % if available
        if ~stringmatch(pNoise.ns_type, 'gmm')
            Arg.type = 'gmm';              % GMM noise source
            Arg.cov_type = 'sqrt';         % GSPF use square-root covariance matrices
            Arg.dim = pNoise.dim;          % process noise dimension
            Arg.M = 1;                     % single component
            Arg.weights = [1];             % component weight
            if isfield(pNoise,'mu')
              Arg.mu = pNoise.mu;
            else
              warning(' [ gensysnoiseds ] Process noise data structure does not have a defined mean vector. Default assigned');
              Arg.mu = zeros(Arg.dim,1);     % default : zero mean
            end
            Arg.cov = repmat(zeros(Arg.dim),[1 1 1]); % default covariance buffer
            if isfield(pNoise,'cov')
              if isfield(pNoise,'cov_type') cov_type = pNoise.cov_type; else cov_type='full'; end
              switch cov_type
                case {'sqrt','sqrt-diag'}
                    Arg.cov(:,:,1) = pNoise.cov;
                case {'full','diag'}
                    Arg.cov(:,:,1) = chol(pNoise.cov)';
                otherwise
                    error(' [ gensysnoiseds::gmsppf ] Unknown process noise covariance type.');
              end

            else
              warning(' [ gensysnoiseds::gmsppf ] Process noise data structure does not have a defined covariance matrix. Default assigned.');
              Arg.cov(:,:,1) = repmat(eye(Arg.dim),[1 1 1]);  % default : covariance  (Cholesky factor)
            end
            pNoise = gennoiseds(Arg);      % generate process noise data structure
        else
            % Make sure the GMM component densities is of cov_type 'sqrt'
            %-- process noise
            switch (pNoise.cov_type)                        % Determine cov_type of Gaussian noise source
            case 'diag'
                pNoise = convgausns(pNoise,'sqrt-diag');
                %warning(' [ gensysnoiseds ] Converting process noise source covariance type to ''sqrt-diag''.');
            case 'full'
                pNoise = convgausns(pNoise,'sqrt');
                %warning(' [ gensysnoiseds ] Converting observation noise source covariance type to ''sqrt''.');
            end
        end

        % If observation noise source is not a GMM, define a GMM noise source with the same dimension, mean and covariance
        % if available
        if ~stringmatch(oNoise.ns_type, 'gmm')
            Arg.type = 'gmm';              % GMM noise source
            Arg.cov_type = 'sqrt';         % GSPF use square-root covariance matrices
            Arg.dim = oNoise.dim;          % observation noise dimension
            Arg.M = 1;                     % single component
            Arg.weights = [1];             % component weight
            if isfield(oNoise,'mu')
              Arg.mu = oNoise.mu;
            else
              warning(' [ gensysnoiseds::gmsppf ] Observation noise data structure does not have a defined mean vector. Default assigned');
              Arg.mu = zeros(Arg.dim,1);     % default : zero mean
            end
            Arg.cov = repmat(zeros(Arg.dim),[1 1 1]); % default covariance buffer
            if isfield(oNoise,'cov')
              if isfield(oNoise,'cov_type') cov_type = oNoise.cov_type; else cov_type='full'; end
              switch cov_type
              case {'sqrt','sqrt-diag'}
                  Arg.cov(:,:,1) = oNoise.cov;
              case {'full','diag'}
                  Arg.cov(:,:,1) = chol(oNoise.cov)';
              otherwise
                  error(' [ gensysnoiseds::gmsppf ] Unknown observation noise covariance type.');
              end
            else
              warning(' [ gensysnoiseds::gmsppf ] Observation noise data structure does not have a defined covariance matrix. Default assigned.');
              Arg.cov(:,:,1) = repmat(eye(Arg.dim),[1 1 1]);  % default : covariance  (Cholesky factor)
            end
            oNoise = gennoiseds(Arg);      % generate process noise data structure
        else
            % Make sure the GMM component densities is of cov_type 'sqrt'
            %-- process noise
            switch (oNoise.cov_type)                        % Determine cov_type of Gaussian noise source
            case 'diag'
                oNoise = convgausns(oNoise,'sqrt-diag');
                %warning(' [ gensysnoiseds ] Converting process noise source covariance type to ''sqrt-diag''.');
            case 'full'
                oNoise = convgausns(oNoise,'sqrt');
                %warning(' [ gensysnoiseds ] Converting observation noise source covariance type to ''sqrt''.');
            end
        end

    end


    %--- Setup noise source tags

    pNoise.tag = 'state';                   % tag this as a state variable noise sources
    oNoise.tag = 'obs';                     % tag this as a observation variable noise source