No package 'orocos-bfl' found

// Copyright (C) 2009 Tinne De Laet <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: mixtureParticleFilter.cpp 2009-02-02 tdelaet $ #include "mixtureParticleFilter.h" #include "../pdf/mixture.h" #define SV StateVar #define MV MeasVar #define MeasModel MeasurementModel #define STATE_AND_MEAS_VAR_DIFFERENT template <typename SV, typename MV> MixtureParticleFilter<SV,MV>::MixtureParticleFilter(Mixture<SV> * prior, ConditionalPdf<SV,SV> * proposal, int resampleperiod, double resamplethreshold, int resamplescheme, int maintainMixturePeriod) : Filter<SV,MV>(prior) , _proposal(proposal) , _sample(WeightedSample<SV>(prior->DimensionGet())) , _resampleScheme(resamplescheme) , _created_post(true) , _newMixtureWeights(prior->NumComponentsGet()) , _sumWeights(prior->NumComponentsGet()) , _old_samplesVec(prior->NumComponentsGet()) , _new_samplesVec(prior->NumComponentsGet()) , _new_samples_unweightedVec(prior->NumComponentsGet()) , _maintainMixturePeriod(maintainMixturePeriod) { /* 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... */ // Post is equal to prior at timetep 1 this->_post = prior->Clone(); // Initialise vector of lists of samples for(int i = 0 ; i< dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet(); i++) { _old_samplesVec[i] = (dynamic_cast<MCPdf<SV> *>(prior->ComponentGet(i))->ListOfSamplesGet()); } _new_samplesVec = _old_samplesVec; // 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> MixtureParticleFilter<SV,MV>::MixtureParticleFilter(Mixture<SV> * prior, Mixture<SV> * post, ConditionalPdf<SV,SV> * proposal, int resampleperiod, double resamplethreshold, int resamplescheme, int maintainMixturePeriod) : Filter<SV,MV>(prior) , _proposal(proposal) , _resampleScheme(resamplescheme) , _created_post(false) , _newMixtureWeights(prior->NumComponentsGet()) , _sumWeights(prior->NumComponentsGet()) , _old_samplesVec(prior->NumComponentsGet()) , _new_samplesVec(prior->NumComponentsGet()) , _new_samples_unweightedVec(prior->NumComponentsGet()) , _maintainMixturePeriod(maintainMixturePeriod) { 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); for(int i =0 ; i < prior->NumComponentsGet() ; i++) { bool ret = (dynamic_cast<MCPdf<SV> *>(this->_post->ComponentGet(i)))->ListOfSamplesSet(prior->ComponentGet(i)->ListOfSamplesGet()); } // Initialise vector of lists of samples for(int i =0 ; i < prior->NumComponentsGet() ; i++) { _old_samplesVec[i] = (prior->ComponentGet(i)->ListOfSamplesGet()); } _new_samplesVec = _old_samplesVec; // 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> MixtureParticleFilter<SV,MV>::~MixtureParticleFilter() { if (_created_post) delete this->_post; } template <typename SV, typename MV> MixtureParticleFilter<SV,MV>::MixtureParticleFilter(const MixtureParticleFilter<SV,MV> & filter) : Filter<SV,MV>(filter) , _created_post(true) { // Clone the Mixture posterior of filter this->_post = filter.PostGet().Clone(); this->_newMixtureWeights.resize(this->_post->NumComponentsGet()); this->_sumWeights.resize(this->_post->NumComponentsGet()); } template <typename SV, typename MV> void MixtureParticleFilter<SV,MV>::Reset(Mixture<SV> * prior) { this->_prior = prior; delete this->_post; this->_post = prior->Clone(); this->_newMixtureWeights.resize(dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet()); this->_sumWeights.resize(dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet()); } template <typename SV, typename MV> void MixtureParticleFilter<SV,MV>::ProposalSet(ConditionalPdf<SV,SV> * const cpdf) { _proposal = cpdf; } template <typename SV, typename MV> ConditionalPdf<SV,SV> * MixtureParticleFilter<SV,MV>::ProposalGet() { return _proposal; } // Proposal step can be executed for each component in the mixture seperately template <typename SV, typename MV> bool MixtureParticleFilter<SV,MV>::ProposalStepInternal(SystemModel<SV> * const sysmodel, const SV & u, MeasurementModel<MV,SV> * const measmodel, const MV & z, const SV & s) { bool result = true; for(int i = 0 ; i< dynamic_cast<Mixture<SV> *>(this->_post)->NumComponentsGet(); i++) { result = result && this->ProposalStepInternalOne(i,sysmodel,u,measmodel,z,s); } return result; } // Proposal step for one component template <typename SV, typename MV> bool MixtureParticleFilter<SV,MV>::ProposalStepInternalOne(int component, 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_samplesVec[component]= (dynamic_cast<MCPdf<SV> *>(dynamic_cast<Mixture<SV> *>(this->_post)->ComponentGet(component)))->ListOfSamplesGet(); _ns_it = _new_samplesVec[component].begin(); for ( _os_it=_old_samplesVec[component].begin(); _os_it != _old_samplesVec[component].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