samplepredictmodel.h

InnovLabSimu在vc++下实现

#pragma once
//#include "CoderBasePosePredict.h"

#include "boost/random.hpp"
#include "../SelfLoc/GlobalMap.h"
/********************************************************************
	created:	2008/07/09
	created:	9:7:2008   9:12
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\SamplePredictModel.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
	file base:	SamplePredictModel
	file ext:	h
	author:		GaoYang
	
	purpose:	对存在噪声的预测模型进行采样，外部输入精确预测模型，本类对其加噪声进行采样，产生粒子供粒子滤波器使用
*********************************************************************/
//typedef boost::mt11213b base_generator_type;
#define EXPECT 0
#define SIGMA_WHEELDIS 30///<空置量，或者说轮位移的高斯分布的标准差或者叫均方差
#define SIGMA_ORINT 0.8///<空置量，或者说轮位移的高斯分布的标准差或者叫均方差
typedef boost::rand48 base_generator_type1;
typedef boost::normal_distribution<> distribution_type1;

//typedef CCoderBasePosePredict predict_model_type;
//typedef RobotLoc state_type;
//typedef WheelDis control_type;

template<class control_type,class predict_model_type,class particle_type>//control_type类型中必须有AddNoise函数进行噪声叠加，PredictModel必须重载()进行预测运算,state_type被直接输入PredictModel，因此是状态模型，非粒子
class CSamplePredictModel//:public CGlobalMap
{
public:
	CSamplePredictModel(void):Randomgenerator((unsigned __int64)45),NormDist(EXPECT,SIGMA_WHEELDIS),
		NormRandom(Randomgenerator,NormDist),NormDist_Ori(EXPECT,SIGMA_ORINT),m_OriRandomgenerator(Randomgenerator,NormDist_Ori)
	{

	}
public:
	~CSamplePredictModel(void)
	{

	}
	/**
	*@brief对预测模型进行采样,control_type类型中必须有AddNoise函数进行噪声叠加，PredictModel必须重载()进行预测运算,可只由设定随机种子
	*返回说采样得到的粒子
	*
	*@parm PredictModel 预测模型
	*@parm PreState 上一状态
	*@parm ControlVarible 控制量
	*@parm RandomRumberSeed随机变量发生器种子
	*/
	particle_type SampleModel(predict_model_type& PredictModel,particle_type& PreState,control_type& NoisyControlVarible,unsigned RandomRumberSeed)
	{
		Randomgenerator.seed(RandomRumberSeed);
		NormRandom(Randomgenerator,NormDist);///正态分布随机数发生器
		//NoisyWheelDis NoisyControlVarible(ControlVarible);
		NoisyControlVarible.AddNoise(NormRandom);//噪声叠加
		
		return PredictModel(NoisyControlVarible,PreState,m_OriRandomgenerator());//PredictModel.fx(ControlVarible,PreState);
	}
	/**
	*@brief对预测模型进行采样,control_type类型中必须有AddNoise函数进行噪声叠加，PredictModel必须重载()进行预测运算
	*返回说采样得到的粒子
	*
	*@parm PredictModel 预测模型
	*@parm PreState 上一状态
	*@parm ControlVarible 控制量
	*/
	particle_type SampleModel(predict_model_type& PredictModel,particle_type& PreState,control_type& NoisyControlVarible)
	{
		NoisyControlVarible.AddNoise(NormRandom);//噪声叠加
		return PredictModel(NoisyControlVarible,PreState,m_OriRandomgenerator());//PredictModel.fx(ControlVarible,PreState);
	}
	
	/**
	*@brief考虑地图的影响，对预测模型进行采样使采样点不落入障碍物内部,
	*control_type类型中必须有AddNoise函数进行噪声叠加，PredictModel必须重载()进行预测运算
	*返回说采样得到的粒子
	*
	*@parm PredictModel 预测模型
	*@parm PreState 上一状态
	*@parm ControlVarible 控制量
	*/
	particle_type SampleModelConsiderMap(predict_model_type& PredictModel,particle_type& PreState,control_type& NoisyControlVarible,CGlobalMap& GlobalMap)
	{
		NoisyControlVarible.AddNoise(NormRandom);//噪声叠加
		particle_type SampledParticle;
		SampledParticle.AddRobotLoc(PredictModel(NoisyControlVarible,PreState,m_OriRandomgenerator()));
		SampledParticle.TransferRealCoord2MapCoord();//将实际坐标转化为地图坐标去查地图
		if (GlobalMap.QueryboolMap(SampledParticle.XCoord,SampledParticle.YCoord))//得到的位置在空白处的话
		{
			SampledParticle.Weight=1;//权值设为1
		}
		else//得到的位置在障碍物内部的话
		{
			SampledParticle.Weight=0;
		}
		SampledParticle.TransferMapCoord2RealCoord();//将地图坐标转化实际坐标
		return SampledParticle;
	}
protected:
	//static unsigned __int64 Randomseed;
	base_generator_type1 Randomgenerator;///<boost中的随机数发生器,给轮子读数加噪声
	distribution_type1 NormDist;///<boost中的分布发生器给轮子读数加噪声用
	distribution_type1 NormDist_Ori;///<boost中的分布发生器给朝向值加噪声
	boost::variate_generator<base_generator_type1&, distribution_type1 >m_OriRandomgenerator;///<boost中的随机变量发生器,给朝向值加噪声
	boost::variate_generator<base_generator_type1&, distribution_type1 > NormRandom;///<boost中的随机变量发生器
};