particlefilt.h

InnovLabSimu在vc++下实现

#pragma once

#include <ctime>            // std::time
#include "SIR_PF_Share.h"
#include "../Localization.h"
#include "../LocalizationIncludeHead.h"
#include "../SelfLoc/GlobalMap.h"
//#define NUM_OF_PARTICLE 1000//粒子数目
// ***************************************************************
//  ParticleFilt   version:  1.0   ·  date: 07/11/2008
//  -------------------------------------------------------------
//  粒子类，从外部继承状态，添加了几个函数与权重
//  -------------------------------------------------------------
//  Copyright (C) 2008 - All Rights Reserved
// ***************************************************************
// 
// ***************************************************************

/********************************************************************
	created:	2008/07/11
	created:	11:7:2008   8:33
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\particleFlt\ParticleFilt.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\particleFlt
	file base:	ParticleFilt
	file ext:	h
	author:		GaoYang
	
	purpose:	粒子滤波器的基类，包括了开放的公用接口函数以及粒子滤波器的粒子集，本类供CParticleFilt继承
*********************************************************************/
//template<class CPredictMod,class CMeasureMod, class control_type,class particle_type>
//class CParticleFltInterFace
//{
//public:
//	/**
//	*@brief 平均分布所有粒子初始化滤波器
//	*
//	*@parm pCheckFun 用来判断的实际函数的指针。计划为本类的派生类中的判断函数，纯虚函数
//	*/
//	virtual bool InitFlt(bool (*pCheckFun)(CParticle& ParticleTobeCheck))=0;
//	/**
//	*@brief 前向采样，或者叫采样状态变化方程
//	*
//	*@parm 
//	*/
//	virtual bool SampleForward(CPredictMod&,control_type)=0;//前向的采样获得采样点，
//	/**
//	*@brief 重采样，或者叫采样状态变化方程
//	*
//	*@parm 
//	*/
//	virtual void Resample()=0;
//	/**
//	*接口函数，开始运行本粒子滤波器
//	*/
//	virtual bool RunFlt()=0;
//	/**
//	*@brief 权值计算
//	*
//	*@parm 
//	*/
////	virtual WEIGHT_TYPE GetParticleWeight(particle_type& Particle,float* ObserveData,size_t ObserveDataSize)=0; 
//protected:
//	
//private:
//};
/********************************************************************
	created:	2008/07/11
	created:	11:7:2008   9:17
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\particleFlt\ParticleFilt.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\particleFlt
	file base:	ParticleFilt
	file ext:	h
	author:		GaoYang
	
	purpose:	量测模型，以获得p(Zt|Xt)
*********************************************************************/

typedef unsigned size;
typedef boost::mt19937 base_generator_type;
typedef boost::uniform_real<> distribution_type;
typedef boost::variate_generator<base_generator_type, boost::uniform_real<> > uniform_random_type;

/********************************************************************
	created:	2008/07/11
	created:	11:7:2008   9:00
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\particleFlt\ParticleFilt.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\particleFlt
	file base:	ParticleFilt
	file ext:	h
	author:		GaoYang
	
	purpose:	粒子滤波器的初始化及初始状态，在给定的状态空间范围内平均分布生成粒子，包括了生成平均分布粒子函数，以及检测粒子是否有效的函数
	            包括粒子集，本类供CParticleFilt继承
*********************************************************************/
typedef std::vector<CParticle> ParticleSet_type;
//
class CParticleFltInitState
{
public:
	 CParticleFltInitState();
	 CParticleFltInitState(unsigned NumOfParticles,const PF_SPA::StateRange& StateRangeVal);
	 ~CParticleFltInitState();
protected:
	/**
	*@brief 平均分布所有粒子初始化滤波器
	*
	*@parm pCheckFun 用来判断的实际函数的指针。判断该粒子是否有可能存在，即它是否落在空白区域内计划为本类的派生类中的判断函数
	*注意要求该函数当输入的位置为可能位置时返回false
	*/
	bool InitFlt(bool (CGlobalMap::*pCheckFun)(CParticle& ParticleTobeCheck));
	bool InitFlt(CGlobalMap& GlobalMap);
	/**
	*@brief 将粒子集初始化为一个粒子
	*
	*/
	bool  CParticleFltInitState::InitFlt(CParticle InitLoc);

	std::vector<uniform_random_type> m_vUniRandomProducer;///<为随机产生粒子点，给每一维状态对应一个平均分布随机变量发生器，这是所有维的随机变量发生器集合

	/**
	*@brief 产生一个可用的粒子，保证不会落入不可能区域，比如障碍物区域，产生成功则返回真
	*
	*@parm vUniRandom 状态空间所有维，每维一个随机数发生器，组成的一个vector。其内容按维数顺序存放
	*@parm Particle 产生的粒子
	*@parm pCheckFun 用来判断的实际函数的指针。计划为本类的派生类中的判断函数，注意要求该函数当输入的位置为可能位置时返回false
	*/
	bool Generate1Particle(std::vector<uniform_random_type>&vUniRandom,CParticle& Particle,bool(CGlobalMap::*pCheckFun)(CParticle& ParticleTobeCheck));
	
	/**
	*@brief 产生一个可用的粒子，保证不会落入不可能区域，比如障碍物区域，产生成功则返回真
	*
	*@parm vUniRandom 状态空间所有维，每维一个随机数发生器，组成的一个vector。其内容按维数顺序存放
	*@parm Particle 产生的粒子
	*@parm GlobalMap 地图
	*/
	bool  Generate1Particle(std::vector<uniform_random_type>&vUniRandom,CParticle& Particle,CGlobalMap& GlobalMap);

	/**
	*@brief 判断产生的粒子是否有可能，即在状态范围内，仍然可能有部分区域是粒子不能存在的，比如地图中的障碍物内
	*判断粒子是否真的可能存在
	*
	*@parm Particle 产生的粒子
	*@parm pCheckFun 用来判断的实际函数的指针。计划为本类的派生类中的判断函数，注意要求该函数当输入的位置为可能位置时返回false
	*@output 粒子可能则返回true
	*/
	bool IsParticlePossible(CParticle& Particle,bool (CGlobalMap::*pCheckFun)(CParticle& ParticleTobeCheck))
	{
		//return !((pCheckFun)(Particle));
		return true;
	}
	ParticleSet_type m_vParticleSet;///<粒子集
protected:
	unsigned m_NumOfParticle;///<粒子数
	std::vector<std::vector<float>>m_vStateRangeVal;///<状态空间的大小范围
	float m_fInitWeight;///<初始的权重，所有粒子都取这值，该值在构造函数中赋值为1/m_NumOfParticle
	//float Get1DimensionSample(RandomGenerater& ORandomGenerater);///<获得状态空间中某一维的取值
};
/********************************************************************
	created:	2008/07/14
	created:	14:7:2008   10:21
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\particleFlt\ParticleFilt.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\particleFlt
	file base:	ParticleFilt
	file ext:	h
	author:		GaoYang
	
	purpose:	重采样类，负责重采样，接口函数为 Low_Variance_Sampler，以后可以包含更多重采样算法
*********************************************************************/
class CResampler
{
public:
	/**
	*@brief 标准LowVariance重采样算法，将高权值者复制,接口函数
	*/
	ParticleSet_type Low_Variance_Sampler(ParticleSet_type & ParticleSet);
	/**
	*@brief 供Low_Variance_Sampler使用的函数，用于对每一个点进行确认是否需要复制，是的话复制
	*/
	void ResampleEverySample(ParticleSet_type & OldParticleSet,double& Random_r,ParticleSet_type & NewParticleSet);
protected:
private:
};

/********************************************************************
created:	2008/07/08
created:	8:7:2008   15:22
filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\ParticleFilt.h
file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
file base:	ParticleFilt
file ext:	h
author:		GaoYang

purpose:	用于实现SIR粒子滤波器的主类，需要输入动态模型和观测模型
*********************************************************************/
#include "SamplePredictModel.h"
#include "SamplePredictModel.cpp"
#include "GetMeasureProbability.h"
#define SAMPLE_CONSIDER_BOOL_MAP

template<class particle_type, class control_type,class CPredictMod,class CMeasureMod>//状态量、空置量、预测数学模型（动态模型）、观测数学模型
class CParticleFilt:public CParticleFltInitState,
	CSamplePredictModel<control_type,CPredictMod,particle_type>//,public CParticleFltInterFace<CPredictMod,CMeasureMod,control_type,particle_type>
	,public CGetMeasureProbability
{

public:
	CParticleFilt(void)//模板类中，所有外面用到的函数均需写到头文件中
	{

	}