particlefilt.h

InnovLabSimu在vc++下实现

	CParticleFilt(unsigned NumOfParticles,const PF_SPA::StateRange& StateRangeVal):
	CParticleFltInitState(NumOfParticles,StateRangeVal)
	{
		//__asm int 3;
	}
	~CParticleFilt(void)
	{

	}

public:
	/**
	*接口函数，开始运行本粒子滤波器
	*/
	bool RunFlt(CPredictMod& PredictModel,control_type ControlVarible,CGlobalMap &GlobalMap,
		float* ObserveData,size_t ObserveDataSize,CResampler Resampler)
	{
		ParticleSet_type OldParticleSet(m_vParticleSet);
		m_vParticleSet.clear();
		double ProbOnLikelihood;
		
		for (size_t i=0;i<OldParticleSet.size();i++)
		{

#ifdef SAMPLE_CONSIDER_BOOL_MAP//若确定要考虑bool地图来确定粒子的有效性的话则前一步的粒子权重已经设过1或0，主要这里要考虑
			OldParticleSet[i]=SampleForwardConsiderMap(PredictModel,ControlVarible,GlobalMap,OldParticleSet[i]);//前向采样，将结果暂存，注意这里已经付了一次权值1或0
			ProbOnLikelihood= GetParticleWeight(OldParticleSet[i],ObserveData,ObserveDataSize, GlobalMap);//获取可能性，并准备付权值
			OldParticleSet[i].Weight=OldParticleSet[i].Weight*ProbOnLikelihood;
#else
			OldParticleSet[i]=SampleForward(PredictModel,ControlVarible,OldParticleSet[i]);//前向采样，将结果暂存，注意这里已经付了一次权值1或0
			ProbOnLikelihood= GetParticleWeight(OldParticleSet[i],ObserveData,ObserveDataSize, GlobalMap);//获取可能性，并准备付权值
			OldParticleSet[i].Weight=ProbOnLikelihood;
#endif	
		}
		if (NormalrizeWeightOfAllParticle(OldParticleSet))//权重归一化
		{
			m_vParticleSet=Resampler.Low_Variance_Sampler(OldParticleSet);//重采样，若考虑bool地图，可能导致粒子数目减少，因此需注意要增加部分随机点
		}
		else//所有权重都极小，表示所有粒子都不行，进行重初始化产生随机粒子
		{
			InitFlt(GlobalMap);
		}
#ifdef SAMPLE_CONSIDER_BOOL_MAP//由于考虑地图会导致重采样阶段减少粒子，因此为保持粒子数，增加一定数目的随机粒子，以保持粒子总数为m_NumOfParticle个
		AddRandomParticleToParticleSet(m_vUniRandomProducer,m_vParticleSet,m_NumOfParticle-m_vParticleSet.size(),GlobalMap);
#endif		
		return true;
	}
protected:
	/**
	*@brief 对粒子集和中的所有粒子的权重进行归一化,若所有的权重之和=0，则不归一化，返回false,否则返回true,并对所有粒子进行权重归一化
	*/
	bool NormalrizeWeightOfAllParticle(ParticleSet_type& ParticleSet)
	{
		double SumOfAllWeight=0;
		for(ParticleSet_type::iterator it_ParticleSet=ParticleSet.begin();
			it_ParticleSet!=ParticleSet.end();it_ParticleSet++)
		{
			SumOfAllWeight+=(*(it_ParticleSet)).Weight;
		}
		if (SumOfAllWeight>0.0000000001)//累加不为0
		{
			for(ParticleSet_type::iterator it_ParticleSet=ParticleSet.begin();
				it_ParticleSet!=ParticleSet.end();it_ParticleSet++)
			{
				(*(it_ParticleSet)).Weight=(*(it_ParticleSet)).Weight/SumOfAllWeight;
			}
			return true;
		}
		return false;
	}
	/**
	*@brief 获得粒子滤波器的结果的期望
	*/
	particle_type GetExpectOfState(ParticleSet_type& ParticleSet)
	{
		if (ParticleSet.size()>0)
		{
			particle_type SumOfParticle;
			SumOfParticle.clear();
			for(ParticleSet_type::iterator it_ParticleSet=ParticleSet.begin();
				it_ParticleSet!=ParticleSet.end();it_ParticleSet++)
			{
				SumOfParticle=SumOfParticle+*(it_ParticleSet);
			}	
			return SumOfParticle/ParticleSet.size();
		}
		else
		{
			particle_type SumOfParticle(0);//若输入的粒子集不行，没有元素则返回一个0值的粒子
		}	
	}
public:
	/**
	*@brief 获得粒子滤波器的结果的期望,无需输入粒子集合，GetExpectOfState对外接口函数
	*/
	particle_type GetStateExpect()
	{
		return GetExpectOfState(m_vParticleSet);
	}
	/**
	*@brief 平均分布所有粒子初始化滤波器
	*
	*@parm pCheckFun 用来判断的实际函数的指针。判断该粒子是否有可能存在，即它是否落在空白区域内计划为本类的派生类中的判断函数，注意要求该函数当输入的位置为可能位置时返回false
	*/
	bool InitParticleFlt(bool (CGlobalMap::*pCheckFun)(particle_type& StateTobeCheck))
	{
		//return InitFlt(pCheckFun);//初始化
		return true;
	}
	/**
	*@brief 平均分布所有粒子初始化滤波器
	*
	*@parm GlobalMap 地图
	*/
	bool InitParticleFlt(CGlobalMap& GlobalMap)
	{
		return InitFlt(GlobalMap);//初始化
	}
	bool InitParticleFlt(CParticle& InitLoc)
	{
		return InitFlt(InitLoc);//初始化
	}
	/**
	*@brief 初始化粒子滤波器平均分布所有粒子初始化滤波器,但在头上指定一个位置
	*
	*/
	bool InitParticleFltWithOneInitLoc(CParticle& InitLoc,CGlobalMap& GlobalMap)
	{
		InitFlt(GlobalMap);//初始化
		m_vParticleSet[0]=InitLoc;
		return true;
	}
#ifdef SAMPLE_CONSIDER_BOOL_MAP//若确定要考虑bool地图来确定粒子的有效性的话,这里要考虑地图影响

	/**
	*@brief 前向采样，获得新的粒子调用了前向采样模型中的采样函数 ,用到了地图，所以不是真正的通用函数，调用了从CSaamplePredictModel类中继承的函数
	*结果为得到的采样粒子
	*
	*@parm PredictModel 准确的预测数学模型
	*@parm ControlVarible 没有加噪声的控制变量，会在函数调用内部加噪声
	*@parm GlobalBoolMap 环境的bool地图，膨胀后的
	*@output 返回采样粒子
	*/
	particle_type SampleForwardConsiderMap(CPredictMod& PredictModel,control_type ControlVarible,CGlobalMap &GlobalMap,particle_type Particle)
	{
		return  SampleModelConsiderMap(PredictModel,Particle,ControlVarible,GlobalMap);
	}

#endif
	/**
	*@brief 前向采样，获得新的粒子调用了前向采样模型中的采样函数 ,不考虑地图，调用了从CSaamplePredictModel类中继承的函数
	*结果为得到的采样粒子
	*
	*@parm PredictModel 准确的预测数学模型
	*@parm ControlVarible 没有加噪声的控制变量，会在函数调用内部加噪声
	*@output 返回采样粒子
	*/
	particle_type SampleForward(CPredictMod& PredictModel,control_type ControlVarible,particle_type Particle)
	{
		return SampleModel(PredictModel,Particle,ControlVarible);
	}
	/**
	*@brief 依据输入的地图，粒子，观测值，获得该粒子的可能性，也就是权重
	*
	*@parm Particle 粒子
	*@parm ObserveData 观测值
	*@parm ObserveDataSize 观测值的大小
	*@parm GlobalMap 全局地图
	*/
	 WEIGHT_TYPE GetParticleWeight(particle_type& Particle,float* ObserveData,size_t ObserveDataSize,CGlobalMap & GlobalMap)
	 {	
		return GetProbOnLikelihoodFieldMod(ObserveData,ObserveDataSize,Particle, GlobalMap);//获得该采样位置的可能性
		
	 }
	 /**
	 *@brief 由于考虑地图会导致重采样阶段减少粒子，因此为保持粒子数，本函数增加指定数目的随机粒子，
	 *
	 *@parm vUniRandom 状态空间所有维，每维一个随机数发生器，组成的一个vector。其内容按维数顺序存放
	 *@parm ParticleSetTobeAdd 产生的粒子要加入到哪里去
	 *@parm NumOfParticleTobeAdd 需要加入的粒子数目，注意从1开始算起
	 *@parm pCheckFun 用来判断的实际函数的指针。计划为本类的派生类中的判断函数，但不知可行否
	 */
	 bool AddRandomParticleToParticleSet(std::vector<uniform_random_type>&vUniRandom,ParticleSet_type ParticleSetTobeAdd,
		 size_t NumOfParticleTobeAdd,CGlobalMap& GlobalMap)
	 {
		 CParticle Particle;
		 for(unsigned i=0;i<NumOfParticleTobeAdd;i++)
		 {

			 if (Generate1Particle(m_vUniRandomProducer,Particle,GlobalMap))
			 {
				 m_vParticleSet.push_back(Particle);
			 }
			 else
			 {
				 return false;//产生粒子失败
			 }		
		 }
	 }

	bool (*m_pParticleCheckFun)(particle_type& StateTobeCheck);///<用来检测粒子是否有效的函数指针，由InitParticleFlt函数输入
public:


private:


};