pf.m

递归贝叶斯估计的工具包

function [estimate, ParticleFilterDS, pNoise, oNoise] = pf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)

% PF  Generic Particle Filter
%
%   This filter is also known as the 'Bootstrap Particle Filter' or the 'Condensation Algorithm'
%
%   [estimate, ParticleFilterDS, pNoise, oNoise] = PF(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)
%
%   This filter assumes the following standard state-space model:
%
%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
%     y(k) = hfun[x(k),n(k),U2(k)]
%
%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
%   system.
%
%   INPUT
%         ParticleFilterDS     Particle filter data structure. Contains set of particles as well as their corresponding
%                              weights.
%         pNoise               process noise data structure
%         oNoise               observation noise data structure
%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
%         InferenceDS          Inference data structure generated by GENINFDS function.
%
%   OUTPUT
%         estimate             State estimate generated from posterior distribution of state given all observation. Type of
%                              estimate is specified by 'InferenceDS.estimateType'
%         ParticleFilterDS     Updated Particle filter data structure. Contains set of particles as well as their corresponding weights.
%         pNoise               process noise data structure     (possibly updated)
%         oNoise               observation noise data structure (possibly updated)
%
%   ParticleFilterDS fields:
%         .N                   (scalar) number of particles
%         .particles           (statedim-by-N matrix) particle buffer
%         .weights             (1-by-N r-vector) particle weights
%
%   Required InferenceDS fields:
%         .estimateType        (string) Estimate type : 'mean', 'mode', etc.
%         .resampleThreshold   (scalar) If the ratio of the 'effective particle set size' to the total number of particles
%                                       drop below this threshold  i.e.  (N_efective/N) < resampleThreshold
%                                       the particles will be resampled.  (N_efective is always less than or equal to N)
%
%   See also
%   KF, EKF, UKF, CDKF, SRUKF, SRCDKF

%   Copyright  (c) Rudolph van der Merwe (2002)
%
%   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 by contacting
%   [e-mail].  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.

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


if (nargin ~= 7) error(' [ pf ] Incorrect number of input arguments.'); end

%--------------------------------------------------------------------------------------------------

Xdim  = InferenceDS.statedim;                            % extract state dimension
Odim  = InferenceDS.obsdim;                              % extract observation dimension
U1dim = InferenceDS.U1dim;                               % extract exogenous input 1 dimension
U2dim = InferenceDS.U2dim;                               % extract exogenous input 2 dimension
Vdim  = InferenceDS.Vdim;                                % extract process noise dimension
Ndim  = InferenceDS.Ndim;                                % extract observation noise dimension

N          = ParticleFilterDS.N;                         % number of particles
particles  = ParticleFilterDS.particles;                 % copy particle buffer
weights    = ParticleFilterDS.weights;                   % particle weights
St         = round(InferenceDS.resampleThreshold * N);   % resample threshold

onoise = zeros(InferenceDS.Ndim,N);
normWeights = cvecrep(1/N,N);

NOV = size(obs,2);                                           % number of input vectors

if (U1dim==0), UU1=zeros(0,N); end
if (U2dim==0), UU2=zeros(0,N); end

estimate   = zeros(Xdim,NOV);


for j=1:NOV,
%---------------------------------------------------------------------------

    if U1dim UU1 = cvecrep(U1(:,j),N); end
    if U2dim UU2 = cvecrep(U2(:,j),N); end

    processNoise = feval(pNoise.sample, pNoise, N);
    particlesPred = feval(InferenceDS.ffun, InferenceDS, particles, processNoise, UU1);


    %-----------------------------------------------------------------------
    % EVALUATE IMPORTANCE WEIGHTS

    OBS = cvecrep(obs(:,j),N);

    likelihood = feval(InferenceDS.likelihood, InferenceDS, OBS, particlesPred, UU2, oNoise) + 1e-99;

    weights = weights .* likelihood;
    weights = weights / sum(weights);



if (0)
prior = feval(InferenceDS.prior, InferenceDS, particlesPred, particles, UU1, pNoise) + 1e-99;
figure(20);
subplot(411)
stem(prior);
ylabel('prior');
subplot(412);
stem(likelihood);
ylabel('likelihood');
subplot(413);
%stem(foobuf);
ylabel('proposal');
subplot(414);
stem(weights);
ylabel('weights');
drawnow
end


    %-----------------------------------------------------------------------
    % RESAMPLE

    S = 1/sum(weights.^2);     % calculate effective particle set size

    if (S < St)                  % resample if S is below threshold
        outIndex  = residualresample(1:N,weights);
        particles = particlesPred(:,outIndex);
        weights   = normWeights;
    else
        particles  = particlesPred;
    end

    %-----------------------------------------------------------------------
    % CALCULATE ESTIMATE

    switch InferenceDS.estimateType

    case 'mean'
        estimate(:,j) = sum(rvecrep(weights,Xdim).*particles,2);          % expected mean

    otherwise
        error(' [ pf ] Unknown estimate type.');

    end

    if pNoise.adaptMethod switch InferenceDS.inftype
    %---------------------- UPDATE PROCESS NOISE SOURCE IF NEEDED --------------------------------------------
    case 'parameter'  %--- parameter estimation
        switch pNoise.adaptMethod
        case 'anneal'
            pNoise.cov = max(pNoise.adaptParams(1) * pNoise.cov , pNoise.adaptParams(2));
        otherwise
            error(' [pf] Unkown process noise adaptation method!');
        end

    case 'joint'  %--- joint estimation
        idx = pNoise.idxArr(end,:); % get indexs of parameter block of combo noise source
        idxRange = idx(1):idx(2);
        switch pNoise.adaptMethod
        case 'anneal'
            pNoise.cov(idxRange,idxRange) = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov(idxRange,idxRange)), pNoise.adaptParams(2)));
        otherwise
            error(' [pf] Unkown process noise adaptation method!');
        end
        pNoise.noiseSources{pNoise.N}.cov = pNoise.cov(idxRange,idxRange);

    %--------------------------------------------------------------------------------------------------
    end; end


%--------------------------------------------------------------------------
end     %.. loop over input variables


ParticleFilterDS.particles = particles;
ParticleFilterDS.weights   = weights;