mixtureParticleFilter.h

Go to the documentation of this file.

// Copyright (C) 2009 Tinne De Laet <first dot last at gmail dot com>
// $Id: mixtureParticlefilter.h 2009-02-03 tdelaet $

 /***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU General Public                   *
 *   License as published by the Free Software Foundation;                 *
 *   version 2 of the License.                                             *
 *                                                                         *
 *   As a special exception, you may use this file as part of a free       *
 *   software library without restriction.  Specifically, if other files   *
 *   instantiate templates or use macros or inline functions from this     *
 *   file, or you compile this file and link it with other files to        *
 *   produce an executable, this file does not by itself cause the         *
 *   resulting executable to be covered by the GNU General Public          *
 *   License.  This exception does not however invalidate any other        *
 *   reasons why the executable file might be covered by the GNU General   *
 *   Public License.                                                       *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU General Public             *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/

#ifndef __MIXTURE_PARTICLE_FILTER__
#define __MIXTURE_PARTICLE_FILTER__

#include "filter.h"
#include "../pdf/conditionalpdf.h"
#include "../pdf/mcpdf.h"
#include "../pdf/mixture.h"

// RS stands for Resample Scheme
// TODO: Work this better out
#define DEFAULT_RS MULTINOMIAL_RS // Default scheme = MULTINOMIAL
#define MULTINOMIAL_RS 0
/* Carpenter, Clifford and Fearnhead:
   Efficient implementation of particle
   filters for non-linear systems.

   See eg.

   @Article{        gordon93,
   author =      {Gordon, Neil and Salmond, D. J. and Smith, A. F. M.},
   title =       {Novel approach to nonlinear/non-Gaussian state estimation},
   journal =     {IEE Proceedings-F},
   year =        {1993},
   volume =      {140},
   number =      {2},
   pages =       {107--113},
   annote =      {Multinomial Sampling}}
*/

#define SYSTEMATIC_RS 1
// A lot of possible systematic approaches to resampling are described
// in literature.  The goal is to reduce the Monte Carlo Variance of
// Particle filters.  One example is
/*
@Article{        carpenter99-improved,
  author =       {Carpenter, J. and Clifford, P. and Fearnhead, P.},
  title =        {An {I}mproved {P}article {F}ilter for {N}on-linear {P}roblems},
  journal =      {Radar, Sonar and Navigation, IEE Proceedings -},
  year =         {1999},
  volume =       {146},
  number =       {1},
  pages =        {2--7},
  month =        {February},
  annote =       {Describes systematic resampling, variance reduction}
}
*/

// KG TODO Check if systematic resampling = deterministic resampling
// as described by kitagawa

#define STRATIFIED_RS 2
// Generate N samples not uniformly on [0,1] but generate 1 sample
// uniformly in for each "stratum" u_j ~ U(j-1/N,j/N)
// Variance reduction!
/*
  @Article{         kitagawa96,
  author =       {Kitagawa, G.},
  title =        {{M}onte {C}arlo filter and smoother for non-{G}aussian nonlinear state space models },
  journal =      {Journal of Computational and Graphical Statistics},
  year =         {1996},
  volume =       {5},
  number =       {1},
  pages =        {1--25},
  annote =       {describes deterministic and stratified resampling}
  }
*/

#define RESIDUAL_RS 3
// sample "deterministically" the integer part of N \times w_j , the
// perform multinomial sampling for the resulting N - \sum [N \times
// w_j] where [] denotes the integer part.
/*
  See eg. p.19
  @Article{         liuchen98,
  author =       {Liu, J. S. and Chen, R.},
  title =        {Sequential {M}onte {C}arlo methods for dynamic systems},
  journal =      {Journal of the American Statistical Association},
  year =         {1998},
  volume =       {93},
  pages =        {1032--1044}}
*/

#define MINIMUM_VARIANCE_RS 4
/*
  See eg.
  @TechReport{     carpenter99,
  author =       {Carpenter, J. and Clifford, P. and Fearnhead, P.},
  title =        {Building robust simulation-based filters for evolving data sets},
  institution =  {Department of Statistics, University of Oxford},
  year =         {99},
  note =         {\url{http://www.stats.ox.ac.uk/pub/clifford/Particle_Filters/}}}
*/

namespace BFL
{


  template <typename StateVar, typename MeasVar> class MixtureParticleFilter
    : public Filter<StateVar,MeasVar>
    {
    protected:
      virtual bool UpdateInternal(SystemModel<StateVar>* const sysmodel,
                                  const StateVar& u,
                                  MeasurementModel<MeasVar,StateVar>* const measmodel,
                                  const MeasVar& z,
                                  const StateVar& s);


      ConditionalPdf<StateVar,StateVar> * _proposal;

      WeightedSample<StateVar>  _sample;
      vector<vector<WeightedSample<StateVar> > > _old_samplesVec;
      vector<vector<WeightedSample<StateVar> > > _new_samplesVec;
      vector< vector<Sample<StateVar> > > _new_samples_unweightedVec;
      typename vector<WeightedSample<StateVar> >::iterator _os_it;
      typename vector<WeightedSample<StateVar> >::iterator _ns_it;
      vector<Probability> _newMixtureWeights;
      vector<Probability> _sumWeights;


      int _resamplePeriod;

      double _resampleThreshold;

      int _resampleScheme;

      bool _dynamicResampling;

      bool _proposal_depends_on_meas;

      bool _created_post;

      //Mixture Pdf.
      int _maintainMixturePeriod;


      virtual bool ProposalStepInternal(SystemModel<StateVar> * const sysmodel,
                                        const StateVar & u,
                                        MeasurementModel<MeasVar,StateVar> * const measmodel,
                                        const MeasVar & z,
                                        const StateVar & s);


      virtual bool ProposalStepInternalOne(int component, SystemModel<StateVar> * const sysmodel,
                                        const StateVar & u,
                                        MeasurementModel<MeasVar,StateVar> * const measmodel,
                                        const MeasVar & z,
                                        const StateVar & s);


      virtual bool UpdateWeightsInternal(SystemModel<StateVar> * const sysmodel,
                                         const StateVar & u,
                                         MeasurementModel<MeasVar,StateVar> * const measmodel,
                                         const MeasVar & z,
                                         const StateVar & s);


      virtual bool UpdateWeightsInternalOne(int component, SystemModel<StateVar> * const sysmodel,
                                         const StateVar & u,
                                         MeasurementModel<MeasVar,StateVar> * const measmodel,
                                         const MeasVar & z,
                                         const StateVar & s);

      virtual bool DynamicResampleStep();


      virtual bool DynamicResampleStepOne(int component);


      virtual bool StaticResampleStep();

      virtual bool Resample();


      virtual bool ResampleOne(int component);


      virtual bool MaintainMixtureStep();

      virtual bool MaintainMixture();


    public:

      MixtureParticleFilter(Mixture<StateVar> * prior,
                     ConditionalPdf<StateVar,StateVar> * proposal,
                     int resampleperiod = 0,
                     double resamplethreshold = 0,
                     int resamplescheme = DEFAULT_RS,
             int maintainMixturePeriod = 1 );



      MixtureParticleFilter(Mixture<StateVar> * prior,
                     Mixture<StateVar> * post,
                     ConditionalPdf<StateVar,StateVar> * proposal,
                     int resampleperiod = 0,
                     double resamplethreshold = 0,
                     int resamplescheme = DEFAULT_RS,
             int maintainMixturePeriod = 1 );

      virtual ~MixtureParticleFilter();

      MixtureParticleFilter(const MixtureParticleFilter<StateVar,MeasVar> & filt);

      virtual void Reset(Mixture<StateVar> * prior);


      virtual void ProposalSet(ConditionalPdf<StateVar,StateVar>* const cpdf);


      ConditionalPdf<StateVar,StateVar> * ProposalGet();

      // implement virtual function
      virtual Mixture<StateVar> * PostGet();
    };

#include "mixtureParticleFilter.cpp"

} // End namespace BFL

#endif // __MIXTURE_PARTICLE_FILTER__