coderbaseposepredict.h

InnovLabSimu在vc++下实现

#pragma once
/********************************************************************
	created:	2008/07/09
	created:	9:7:2008   9:11
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\CoderBasePosePredict.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
	file base:	CoderBasePosePredict
	file ext:	h
	author:		GaoYang
	
	purpose:	基于编码器的机器人位置预测类，依据输入的上一刻位置，和编码器信息精确预测
*********************************************************************/
#include <math.h>
#include "../PathPlan/ObjectAngleCheck.h"

#include "../Localization.h"
#include "../RobotStruct.h"


#define CONSTTWOPI 6.283185307179586

class CCoderBasePosePredict: public ObjectAngleCheck
{
public:
	CCoderBasePosePredict(void);
public:
	~CCoderBasePosePredict(void);
	//void PredictNewCoord();///<依据编码器预测新的位置,并计算当前方差
	RobotLoc& fx(WheelDis&WheelDisVal,RobotLoc& PreState,float OrientNoise)///作为预测模型对外的接口函数
	{
		PreSelfLoc=PreState;
		return PredictNewCoord(WheelDisVal.dl,WheelDisVal.dr,OrientNoise);
	}
	 RobotLoc& operator()(WheelDis&WheelDisVal,RobotLoc& PreState,float OrientNoise)
	 {
		 PreSelfLoc=PreState;
		 return PredictNewCoord(WheelDisVal.dl,WheelDisVal.dr,OrientNoise);
	 }
protected:
	RobotLoc PreSelfLoc;///<按绝对坐标系记上一时刻自身的位姿;
	RobotLoc SelfLoc;///按绝对坐标系记当前自身的位姿;
	/**
	*@ brief 依据编码器预测运动情况，并求出角度变化
	*/
	void GetSelfOriention(float dl,float dr);

	RobotLoc& PredictNewCoord(float dl,float dr,float OrientNoise);///<依据编码器预测新的位置
	
	int CenterRadiu;//机器人中心的旋转半径,有正负的,沿正方向旋转时角速度为正,此时半径为正,反之为负
	int m_iRightNeg_LeftPosit;///<标志位，用于标示相对上一位置，机器人是向左转(值为1)，未转（0），右转（-1）
	float CenterTrackLength;///<两帧之间路径的长度
	float m_fdeltaOrientation;///<两帧之间机器人朝向变化，弧度
};