particlefilt.cpp

来自「InnovLabSimu在vc++下实现」· C++ 代码 · 共 207 行

#include "ParticleFilt.h"


CParticleFltInitState:: CParticleFltInitState(void)
{
}

CParticleFltInitState:: CParticleFltInitState(unsigned NumOfParticles,const PF_SPA::StateRange& StateRangeVal)
{
	m_NumOfParticle=NumOfParticles;
	m_vStateRangeVal=StateRangeVal.m_vStateRage;
	//unsigned StateDimension=StateRangeVal.QueryStateDimension();
	m_fInitWeight=1.0/m_NumOfParticle;
}

 CParticleFltInitState::~CParticleFltInitState(void)
{
}

bool  CParticleFltInitState::InitFlt(bool (CGlobalMap::*pCheckFun)(CParticle& ParticleTobeCheck))
{		
////////////////////////为状态的每一维产生一个平均分布的随机变量发生器//////////////////////////////////////////////////
	//std::vector<uniform_random_type> vUniRandom;///<每一维状态对应一个平均分布随机变量发生器
	for (unsigned i=0;i<m_vStateRangeVal.size();i++)
	{	
		base_generator_type generator(static_cast<unsigned int>(std::time(0))+i);///随即变量发生器
		distribution_type uni_dist(m_vStateRangeVal[i][0],m_vStateRangeVal[i][1]);///<每一维状态的分布范围设置好
		uniform_random_type UniRandom(generator, uni_dist);
		m_vUniRandomProducer.push_back(UniRandom);///<将产生的随机变量发生器按维数顺序存起来
	}
////////////////开始产生点//////////////////////////////////////////////////////////
	for(unsigned i=0;i<m_NumOfParticle;i++)
	{
		CParticle Particle;
		if (Generate1Particle(m_vUniRandomProducer,Particle,pCheckFun))
		{
			m_vParticleSet.push_back(Particle);
		}
		else
		{
			return false;//产生粒子失败
		}		
	}	
	return true;
}
bool  CParticleFltInitState::InitFlt(CParticle InitLoc)
{		
	////////////////////////为状态的每一维产生一个平均分布的随机变量发生器//////////////////////////////////////////////////
	//std::vector<uniform_random_type> vUniRandom;///<每一维状态对应一个平均分布随机变量发生器
	for (unsigned i=0;i<m_vStateRangeVal.size();i++)
	{	
		base_generator_type generator(static_cast<unsigned int>(std::time(0))+i);///随即变量发生器
		distribution_type uni_dist(m_vStateRangeVal[i][0],m_vStateRangeVal[i][1]);///<每一维状态的分布范围设置好
		uniform_random_type UniRandom(generator, uni_dist);
		m_vUniRandomProducer.push_back(UniRandom);///<将产生的随机变量发生器按维数顺序存起来
	}
	////////////////开始产生点//////////////////////////////////////////////////////////
	for (int i=0;i<m_NumOfParticle;i++)
	{
		m_vParticleSet.push_back(InitLoc);
	}
	return true;
}
bool  CParticleFltInitState::InitFlt(CGlobalMap& GlobalMap)
{		
	////////////////////////为状态的每一维产生一个平均分布的随机变量发生器//////////////////////////////////////////////////
	//std::vector<uniform_random_type> vUniRandom;///<每一维状态对应一个平均分布随机变量发生器
	//__asm int 3;
	m_vUniRandomProducer.clear();
	for (unsigned i=0;i<m_vStateRangeVal.size();i++)
	{	
		base_generator_type generator(static_cast<unsigned int>(std::time(0))+i);///随即变量发生器
		distribution_type uni_dist(m_vStateRangeVal[i][0],m_vStateRangeVal[i][1]);///<每一维状态的分布范围设置好
		uniform_random_type UniRandom(generator, uni_dist);
		m_vUniRandomProducer.push_back(UniRandom);///<将产生的随机变量发生器按维数顺序存起来
	}
	////////////////开始产生点//////////////////////////////////////////////////////////
	for(unsigned i=0;i<m_NumOfParticle;i++)
	{
		CParticle Particle;
		if (Generate1Particle(m_vUniRandomProducer,Particle,GlobalMap))
		{
			m_vParticleSet.push_back(Particle);
		}
		else
		{
			return false;//产生粒子失败
		}		
	}	
	return true;
}
bool  CParticleFltInitState::Generate1Particle(std::vector<uniform_random_type>&vUniRandom,CParticle& Particle,bool (CGlobalMap::*pCheckFun)(CParticle& ParticleTobeCheck))
{
	CParticle ParticleTmp;
	do
	{
		for (unsigned i=0;i<m_vStateRangeVal.size();i++)
		{	
			ParticleTmp.ChangeState(i,vUniRandom[i]());
		}
	}while(!IsParticlePossible(ParticleTmp,pCheckFun));//反复生成点直到生成了可行的粒子才结束
	return true;
}
bool  CParticleFltInitState::Generate1Particle(std::vector<uniform_random_type>&vUniRandom,CParticle& Particle,CGlobalMap& GlobalMap)
{
	//CParticle ParticleTmp;
	do
	{
		for (unsigned i=0;i<m_vStateRangeVal.size();i++)
		{	
			Particle.ChangeState(i,vUniRandom[i]());
		}
	}while(GlobalMap.IsAPossibleLocate(Particle));//反复生成点直到生成了可行的粒子才结束
	Particle.XCoord=Particle.TransferMapCoord2RealCoord(Particle.XCoord);
	Particle.YCoord=Particle.TransferMapCoord2RealCoord(Particle.YCoord);
	Particle.Weight=m_fInitWeight;///<赋初始权重
	return true;
}


ParticleSet_type CResampler::Low_Variance_Sampler(ParticleSet_type & ParticleSet)
{
	ParticleSet_type NewParticleSet;
	if (ParticleSet.size()>0)
	{
		srand(std::time(0));
		double r=(float)rand()/(RAND_MAX*ParticleSet.size());
		ResampleEverySample(ParticleSet,r,NewParticleSet);
		return NewParticleSet;
	}
	else
	{
		NewParticleSet.push_back(CParticle(0));
		return NewParticleSet;
	}

}
void CResampler::ResampleEverySample(ParticleSet_type & OldParticleSet,double& Random_r,ParticleSet_type & NewParticleSet)
{
	size_t ParticleSetSize=OldParticleSet.size();
	WEIGHT_TYPE c=OldParticleSet[0].Weight;//第一个权重
	size_t IndexOfOldParticleSet=0;
	for (unsigned m=1;m<ParticleSetSize;m++)
	{
		double U=Random_r+(double)(m-1)/ParticleSetSize;
		while (U>c)
		{
			IndexOfOldParticleSet++;
			c=c+OldParticleSet[IndexOfOldParticleSet].Weight;
		}
		NewParticleSet.push_back(OldParticleSet[IndexOfOldParticleSet]);
	}
}

//template<class particle_type, class control_type,class CPredictMod,class CMeasureMod>
//bool CParticleFilt<particle_type,control_type,CPredictMod,CMeasureMod>::AddRandomParticleToParticleSet(std::vector<uniform_random_type>&vUniRandom,ParticleSet_type ParticleSetTobeAdd,
//									size_t NumOfParticleTobeAdd,bool (*pCheckFun)(particle_type& StateTobeCheck))
//{
//	
//}


//template<class state_type, class control_type,class CSampleForwardMod,class CMeasureMod>
//CParticleFilt<state_type,control_type,CSampleForwardMod,CMeasureMod>::~CParticleFilt(void)
//{
//}
//template<class state_type, class control_type,class CSampleForwardMod,class CMeasureMod>
//bool CParticleFilt<state_type,control_type,CSampleForwardMod,CMeasureMod>::InitFlt(bool (*pCheckFun)(CParticle& ParticleTobeCheck))
//{		
//	////////////////////////为状态的每一维产生一个平均分布的随机变量发生器//////////////////////////////////////////////////
//	std::vector<uniform_random_type> vUniRandom;///<每一维状态对应一个平均分布随机变量发生器
//	for (unsigned i=0;i<m_vStateRangeVal.size();i++)
//	{	
//		base_generator_type generator(static_cast<unsigned int>(std::time(0))+i);///随即变量发生器
//		distribution_type uni_dist(m_vStateRangeVal[i][0],m_vStateRangeVal[i][1]);///<每一维状态的分布范围设置好
//		uniform_random_type UniRandom(generator, uni_dist);
//		vUniRandom.push_back(UniRandom);///<将产生的随机变量发生器按维数顺序存起来
//	}
//	////////////////开始产生点//////////////////////////////////////////////////////////
//	for(unsigned i=0;i<m_NumOfParticle;i++)
//	{
//		CParticle Particle;
//		if (Generate1Particle(vUniRandom,Particle,pCheckFun))
//		{
//			m_vParticleSet.push_back(Particle);
//		}
//		else
//		{
//			return false;//产生粒子失败
//		}		
//	}	
//	return true;
//}
//template<class state_type, class control_type,class CSampleForwardMod,class CMeasureMod>
//bool CParticleFilt<state_type,control_type,CSampleForwardMod,CMeasureMod>::Generate1Particle(std::vector<uniform_random_type>&vUniRandom,CParticle& Particle,bool (*pCheckFun)(CParticle& ParticleTobeCheck))
//{
//	CParticle ParticleTmp;
//	do
//	{
//		for (unsigned i=0;i<m_vStateRangeVal.size();i++)
//		{	
//			ParticleTmp.ChangeState(i,vUniRandom[i]());
//		}
//	}while(!IsParticlePossible(ParticleTmp,pCheckFun));//反复生成点直到生成了可行的粒子才结束
//	return true;
//}