predictmodel.h

InnovLabSimu在vc++下实现

#pragma once
//#include "BayesFilter/unsFlt.hpp"
////#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
/********************************************************************
	created:	2008/01/03
	created:	3:1:2008   15:00
	filename: 	c:\boosttest\MyTargetTrack\MyTargetTrack\UncentedKalmanFilt\PredictModel.h
	file path:	c:\boosttest\MyTargetTrack\MyTargetTrack\UncentedKalmanFilt
	file base:	PredictModel
	file ext:	h
	author:		GaoYang
	
	purpose:	预测模型，不含噪声的理想模型包含了一个预测函数fx，输入状态给出预测状态及方差，存于自身的
				成员变量中,其中状态变量中的线速度定义为：距离变远为正。
*********************************************************************/
//#include <boost/numeric/ublas/matrix.hpp>
//#include <boost/numeric/ublas/vector.hpp>
//#include <boost/numeric/ublas/storage.hpp>
//#include <boost/numeric/ublas/matrix_proxy.hpp>


//#include "BayesFilter/bayesFlt.hpp"
#include "..\ObjectAngleCheck.h"
#include "RobotStruct.h"
#include "VelocityStructFile.h"
#include "UKF_COMMAN_HEAD.h"
#include "..\ShareObject.h"
#define HALF_PI 1.57079632679490
//#include <device/robot.h>
//using namespace std;
//using namespace Bayesian_filter;
//using namespace Bayesian_filter_matrix;
//using namespace boost::numeric::ublas;

using namespace personal_ShareObject_pathPlanUsingSpace;

class CPredictModel :public ObjectAngleCheck//,Bayesian_filter::Functional_predict_model
{
public:
	CPredictModel(void);
public:
	virtual ~CPredictModel(void);
	void VelocityPredict(double dl, double dr);///<预测函数
	/**
	*@brief 进行速度预测的函数fx_Degree调用。本函数中开始，考量时间，另外严格用物体的CV模型
	*
	*所有速度都考虑时间的影响，所有速度存储位置在m_vStateAfPredit中，单位为mm/s,或者 度/s
	*@input dl 左轮位移
	*@input dr 右轮位移
	*@input TimeStep 上一次计算到此次计算的间隔时间步长，
	*/
	void VelocityPredict2WithTimeConsider(double dl, double dr,double TimeStep);
	/**
	*直角坐标系中的XY速度分量向极坐标系中速度分量的转化,速度单位为每秒
	*@parm CurrentPredictX 为本轮预测中估计的当前位置X坐标
	*@parm CurrentPredictY 为本轮预测中估计的当前位置Y坐标
	*@parm DeltaX 为本轮预测中估计的当前位置与前一位置的X坐标差
	*/
	void OrthogonalCoordVel2PolarCoordVel(double& CurrentPredictX,double& CurrentPredictY,
		double DeltX,double DeltY,double& LinearV,double& AngularV,double TimeStep,
		ObjectPoleCoord& CurrenPredictPolePos)
	{
		double NextPredictX=CurrentPredictX+DeltX;
		double NextPredictY=CurrentPredictY+DeltY;
		LinearV=sqrt(pow(NextPredictX,2)+pow(NextPredictY,2));//暂存下一预测点距离
		AngularV=atan2(NextPredictY,NextPredictX);//暂存下一预测点角度，-Pi~Pi
		LimitAngleIn0_2Pi(AngularV);//暂存下一预测点角度，0~2Pi
		LinearV=(LinearV-CurrenPredictPolePos.distance)/TimeStep*1000;
		AngularV=GetAngulardisplacementIn2Angle(CurrenPredictPolePos.angle,AngularV)/TimeStep*1000;
	}
	double m_ddeltaLeftAndRight[2];///<机器人左右轮的位移0左，1右，单位毫米
	double m_ddeltaOrientation;///<相对上一次的朝向，本时刻朝向的角度变化，单位弧度，相对上一次，顺时针变为负
	int m_iRightNeg_LeftPosit;///<标志位，用于标示相对上一位置，机器人是向左转(值为1)，未转（0），右转（-1）
	STATE_STYLE& fx( STATE_STYLE& x,double dl,double dr)  ;///<接口函数，输入x为扩维后的状态变量

	STATE_STYLE& fx_Degree( STATE_STYLE& x,double dl,double dr)  ;///<角度型接口函数，输入x为扩维后的状态变量，其中的角度相关变量单位为度
	STATE_STYLE& fx_Degree2ConsiderTime( STATE_STYLE& x,double dl,double dr,double TimeStep);///<角度型接口函数，输入x为扩维后的状态变量，其中的角度相关变量单位为度,速度单位为每秒


protected:
	/** 
	*计算当前自己朝向相对上一次朝向的变化
	*/
	inline void  GetSelfOriention();
	deltaCoorinatePos m_sdeltaCoorinate;
	ObjectPoleCoord m_sObjectObserveCoord;
	ObjectPoleCoord m_sObjectPoleCoordP1_in_Current;///<预测物体在当前机器人坐标系中的极坐标
	ObjectPoleCoord m_sObjectPoleCoordP1_in_Pre;///<预测物体在前一时刻机器人坐标系中的极坐标
	ObjectPoleCoord m_sObjectPoleCoordP0_in_Current;///<上一时刻物体在当前机器人坐标系中的极坐标
	ObjectPoleCoord	m_sObjectPoleCoordP0_in_Pre;///<上一时刻物体在前一时刻机器人坐标系中的极坐标

	ObjectVelocity m_sObject_Velocity_in_Current,m_sObject_Velocity_in_pre;
	STATE_STYLE m_vStateAfPredit;///<经过预测后的状态结果，其中的角度相关变量分两种情况，若预测时用角度制的预测
	//则其中的角度相关变量即为度数单位，若预测时用弧度制，则其中的角度相关量即位弧度制。
	/**
	*输入的状态变量x中的角度相关量单位为弧度，此函数将状态变量保存于内部成员变量中，成员变量的角度单位都是弧度
	*/
	bool InputState(const STATE_STYLE& x);
public:
	/**
	*输入的状态变量x中的角度相关量单位为度，此函数将状态变量保存于内部成员变量中，成员变量的角度单位都是弧度
	*/
	bool InputState_Degree(const STATE_STYLE& x);
	
};