particlefilter.cpp

Go to the documentation of this file.

// Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or
// (at your option) any later version.
//
// This program 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 Lesser General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// $Id: particlefilter.cpp 29787 2008-12-09 19:38:08Z kgadeyne $

#include "particlefilter.h"
#include "../pdf/mcpdf.h"

#define SV StateVar
#define MV MeasVar
#define MeasModel MeasurementModel

#define STATE_AND_MEAS_VAR_DIFFERENT

template <typename SV, typename MV>
ParticleFilter<SV,MV>::ParticleFilter(MCPdf<SV> * prior,
                                      ConditionalPdf<SV,SV> * proposal,
                                      int resampleperiod,
                                      double resamplethreshold,
                                      int resamplescheme)
  : Filter<SV,MV>(prior)
  , _proposal(proposal)
  , _sample(WeightedSample<SV>(prior->DimensionGet()))
  , _resampleScheme(resamplescheme)
  , _created_post(true)
{
  /* Initialize Post, at time = 0, post = prior
     To be more clean, this should be done in the filter base class,
     but this is impossible because of the pure virtuals...
  */
  this->_post = new MCPdf<SV>(prior->NumSamplesGet(),prior->DimensionGet());
  // Post is equal to prior at timetep 1
  /* Note: Dirty cast should be avoided by not demanding an MCPdf as
     prior and just sample from the prior instead  :-(
  */
  bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
  assert(ret);

  // Initialise lists of samples
  _old_samples = (prior->ListOfSamplesGet());
  _new_samples = _old_samples;


  // You have to choose for dynamic resampling by specifying a threshold != 0 OR give me a fixed resample period != 0
  assert(!(resampleperiod == 0 && resamplethreshold == 0));
  assert(!(resampleperiod != 0 && resamplethreshold != 0));

  // dynamic resampling
  if (resampleperiod == 0)
   _dynamicResampling = true;
  // fixed period resampling
  else
    _dynamicResampling = false;
  _resamplePeriod = resampleperiod;
  _resampleThreshold = resamplethreshold;
}



template <typename SV, typename MV>
ParticleFilter<SV,MV>::ParticleFilter(MCPdf<SV> * prior,
                                      MCPdf<SV> * post,
                                      ConditionalPdf<SV,SV> * proposal,
                                      int resampleperiod,
                                      double resamplethreshold,
                                      int resamplescheme)
  : Filter<SV,MV>(prior),
    _proposal(proposal),
    _resampleScheme(resamplescheme),
    _created_post(false)
{
  this->_post = post;
  // Post is equal to prior at timestep 1
  /* Note: Dirty cast should be avoided by not demanding an MCPdf as
     prior and just sample from the prior instead  :-(
  */
  bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesSet(prior->ListOfSamplesGet());
  assert(ret);

  // Initialise lists of samples
  _old_samples = (prior->ListOfSamplesGet());
  _new_samples = _old_samples;

  // You have to choose for dynamic resampling by specifying a threshold != 0 OR give me a fixed resample period != 0
  assert(!(resampleperiod == 0 && resamplethreshold == 0));
  assert(!(resampleperiod != 0 && resamplethreshold != 0));

  // dynamic resampling
  if (resampleperiod == 0)
   _dynamicResampling = true;
  // fixed period resampling
  else
    _dynamicResampling = false;

  _resamplePeriod = resampleperiod;
  _resampleThreshold = resamplethreshold;
}




template <typename SV, typename MV>
ParticleFilter<SV,MV>::~ParticleFilter()
{
  if (_created_post)
    delete this->_post;
}

template <typename SV, typename MV>
ParticleFilter<SV,MV>::ParticleFilter(const ParticleFilter<SV,MV> & filter)
  : Filter<SV,MV>(filter),
    _created_post(true)
{
  // Copy constructor of MCPdf
  // Probably a bug...
  this->_post = new MCPdf<SV>(dynamic_cast<MCPdf<SV> *>(filter._post));
}

template <typename SV, typename MV> void
ParticleFilter<SV,MV>::ProposalSet(ConditionalPdf<SV,SV> * const cpdf)
{
  _proposal = cpdf;
}

template <typename SV, typename MV> ConditionalPdf<SV,SV> *
ParticleFilter<SV,MV>::ProposalGet()
{
  return _proposal;
}

template <typename SV, typename MV> bool
ParticleFilter<SV,MV>::ProposalStepInternal(SystemModel<SV> * const sysmodel,
                                            const SV & u,
                                            MeasurementModel<MV,SV> * const measmodel,
                                            const MV & z,
                                            const SV & s)
{
  // Get all samples from the current post through proposal density
  _old_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();

  _ns_it = _new_samples.begin();
  for ( _os_it=_old_samples.begin(); _os_it != _old_samples.end() ; _os_it++)
    {
      const SV& x_old = _os_it->ValueGet();
      _proposal->ConditionalArgumentSet(0,x_old);

      if (!sysmodel->SystemWithoutInputs())
        {
          _proposal->ConditionalArgumentSet(1,u);
          if (this->_proposal_depends_on_meas)
            {
          #ifndef STATE_AND_MEAS_VAR_DIFFERENT
              _proposal->ConditionalArgumentSet(2,z);
              if (!measmodel->SystemWithoutSensorParams())
                _proposal->ConditionalArgumentSet(3,s);
              #endif
            }

        }
      else // System without inputs
        {
          if (this->_proposal_depends_on_meas)
            {
          #ifndef STATE_AND_MEAS_VAR_DIFFERENT
              _proposal->ConditionalArgumentSet(1,z);
              if (!measmodel->SystemWithoutSensorParams())
                _proposal->ConditionalArgumentSet(2,s);
              #endif

            }
        }
      // Bug, make sampling method a parameter!
      _proposal->SampleFrom(_sample, DEFAULT,NULL);
      _ns_it->ValueSet(_sample.ValueGet());
      _ns_it->WeightSet(_os_it->WeightGet());
      _ns_it++;
    }

  (this->_timestep)++;

  // Update the list of samples
  return (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);

}


template <typename SV, typename MV> bool
ParticleFilter<SV,MV>::UpdateWeightsInternal(SystemModel<SV> * const sysmodel,
                                             const SV & u,
                                             MeasurementModel<MV,SV> * const measmodel,
                                             const MV & z,
                                             const SV & s)
{
  Probability weightfactor = 1;

  // Update the weights
  // Same remarks as for the system update!
  _new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
  _os_it = _old_samples.begin();

  for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
    {
      const SV& x_new = _ns_it->ValueGet();
      const SV& x_old = _os_it->ValueGet();

      if (sysmodel == NULL)
        {
          if (measmodel->SystemWithoutSensorParams() == true)
            weightfactor = measmodel->ProbabilityGet(z,x_new);
          else
            weightfactor = measmodel->ProbabilityGet(z,x_new,s);
        }
      else // We do have a system model
        {
      _proposal->ConditionalArgumentSet(0,x_old);
          if (measmodel->SystemWithoutSensorParams() == true)
            {
              weightfactor = measmodel->ProbabilityGet(z,x_new);
              if (sysmodel->SystemWithoutInputs() == false)
                {
                  _proposal->ConditionalArgumentSet(1,u);
                  if (this->_proposal_depends_on_meas){
                    #ifndef STATE_AND_MEAS_VAR_DIFFERENT
                    _proposal->ConditionalArgumentSet(2,z);
                    #endif
                  }

                  if (_proposal->ProbabilityGet(x_new) != 0)
                    weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,x_old,u) / _proposal->ProbabilityGet(x_new) );
                  else weightfactor = 0;
                }
              else // we do have a system without inputs
                {
                  if (this->_proposal_depends_on_meas){
                    #ifndef STATE_AND_MEAS_VAR_DIFFERENT
                    _proposal->ConditionalArgumentSet(1,z);
                    #endif
                  }
                  if ( _proposal->ProbabilityGet(x_new) != 0)
                    weightfactor = weightfactor * ( sysmodel->ProbabilityGet(x_new,_os_it->ValueGet()) / _proposal->ProbabilityGet(x_new) );
                  else weightfactor = 0;
                }
            }
          else // System with sensor Parameters
            {
              weightfactor = measmodel->ProbabilityGet(z,x_new,s);
            }
        }
      _ns_it->WeightSet(_ns_it->WeightGet() * weightfactor);

      _os_it++;
    }
  // Update the sample list of post the SumofWeights of the pdf
  return (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples);

}

template <typename SV, typename MV> bool
ParticleFilter<SV,MV>::DynamicResampleStep()
{
  // Resampling?
  bool resampling = false;
  double sum_sq_weigths = 0.0;

  // Resampling if necessary
  if ( this->_dynamicResampling)
    {
      // Check if sum of 1 / \sum{(wi_normalised)^2} < threshold
      // This is the criterion proposed by Liu
      // BUG  foresee other methods of approximation/calculating
      // effective sample size.  Let the user implement this in !
      _new_samples = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
      for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
        {
          sum_sq_weigths += pow(_ns_it->WeightGet(),2);
        }
      if ((1.0 / sum_sq_weigths) < _resampleThreshold)
        {
          // #define __RESAMPLE_DEBUG__
#ifdef __RESAMPLE_DEBUG__
          cout << "resampling now: " << this->_timestep
               << "\tN_eff: " << (1.0 / sum_sq_weigths) << endl;
#endif // __RESAMPLE_DEBUG__
          resampling = true;
        }
    }
  if (resampling == true)
    return this->Resample();
  else
    return true;
}


template <typename SV, typename MV> bool
ParticleFilter<SV,MV>::StaticResampleStep()
{
  // Resampling if necessary
  if ( (!this->_dynamicResampling) &&  (((this->_timestep) % _resamplePeriod) == 0) && (this->_timestep != 0))
    return this->Resample();
  return true;
}


template <typename SV, typename MV> bool
ParticleFilter<SV,MV>::UpdateInternal(SystemModel<StateVar>* const sysmodel,
                                      const StateVar& u,
                                      MeasurementModel<MeasVar,StateVar>* const measmodel,
                                      const MeasVar& z,
                                      const StateVar& s)
{
  bool result = true;

  // Only makes sense if there is a system model?
  // Bug, not completely true, but should do for now...
  if (sysmodel != NULL)
    {
      result = result && this->StaticResampleStep();
      result = result && this->ProposalStepInternal(sysmodel,u,measmodel,z,s);

    }
  // Updating the weights only makes sense using a measurement model
  if (measmodel != NULL)
    {
      result = result && this->UpdateWeightsInternal(sysmodel,u,measmodel,z,s);
      result = result && this->DynamicResampleStep();
    }

  return result;
}

template <typename SV, typename MV> bool
ParticleFilter<SV,MV>::Resample()
{
  int NumSamples = (dynamic_cast<MCPdf<SV> *>(this->_post))->NumSamplesGet();
  // #define __PARTICLEFILTER_DEBUG__
#ifdef __PARTICLEFILTER_DEBUG__
  cout << "PARTICLEFILTER: resampling now" << endl;
  _new_samples= (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
  for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
    {
      cout << "PF: Old samples:\n";
      cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
    }
#endif // __PARTICLEFILTER_DEBUG
  switch(_resampleScheme)
    {
    case MULTINOMIAL_RS:
      {
        (dynamic_cast<MCPdf<SV> *>(this->_post))->SampleFrom(_new_samples_unweighted, NumSamples,RIPLEY,NULL);
        break;
      }
    case SYSTEMATIC_RS:{break;}
    case STRATIFIED_RS:{break;}
    case RESIDUAL_RS:{break;}
    default:
      {
        cerr << "Sampling method not supported" << endl;
        break;
      }
    }
  bool result = (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesUpdate(_new_samples_unweighted);
#ifdef __PARTICLEFILTER_DEBUG__
  cout << "PARTICLEFILTER: after resampling" << endl;
  _new_samples= (dynamic_cast<MCPdf<SV> *>(this->_post))->ListOfSamplesGet();
  for ( _ns_it=_new_samples.begin(); _ns_it != _new_samples.end() ; _ns_it++)
    {
      cout << "PF: New samples:\n";
      cout << _ns_it->ValueGet() << _ns_it->WeightGet() << endl;
    }
#endif // __PARTICLEFILTER_DEBUG

  return result;
}


template<typename SV, typename MV> MCPdf<SV> *
ParticleFilter<SV,MV>::PostGet()
{
  return (MCPdf<SV>*)Filter<SV,MV>::PostGet();
}