objecttracker.cpp

InnovLabSimu在vc++下实现

//#include "StdAfx.h"
#include "ObjectTracker.h"

CObjectTracker::CObjectTracker(void):m_mTranslateMatrix(2,4), m_cObserveMode(4,2,m_mTranslateMatrix),my_filter(4,2,2)
,CInitStateOfUKF()
{
	InitFlt();
#ifdef CONSIDER_TIME_IN_VEL
	//LARGE_INTEGER NumOfCount;
	//if (QueryPerformanceFrequency(&NumOfCount))
	//{
	//	//__asm int 3;
	//	m_dTimePerSystemCount=(double)(1000.0/NumOfCount.QuadPart);//每一个点的毫秒数
	//}	
#endif
}

CObjectTracker::~CObjectTracker(void)
{
}
bool CObjectTracker::SetFltState(STATE_STYLE& State,VARIANCE& StateVariance,VARIANCE& ModelNoiseVariance)
{
	if (State.size()>0)
	{
		my_filter.m_mStateMeanResult=State;
	}
	else
	{
		return false;
	}

	if (StateVariance.size1()>0)
	{
		my_filter.m_mStateCov=StateVariance;
	}
	else
	{
		return false;
	}

	if (ModelNoiseVariance.size1()>0)
	{
		m_cAdaptiveOptimiser.m_mNewModelNoise=ModelNoiseVariance;
	}
	else
	{
		return false;
	}
	return true;	
}

bool CObjectTracker::InitFlt(void)
{
#ifdef CONSIDER_TIME_IN_VEL
	InitStateAndCov(300,1,10,1,
		400,1,100,0.2,true);//设置初始状态和方差，注意速度项考虑了时间，单位为度或毫米/秒
	InitNoiseVar(0.5,0.5,
		200,0.1,700,1,
		100,0.1,true);	
	//////////////////////////转移矩阵设置////////////////////////////////////////////////

#else
	InitStateAndCov(100,1,10,1,100,1,100,1,true);
	InitNoiseVar(0.1,0.1,20,0.8,0.6,0.08,100,0.1,true);

#endif
	//////////////////////////转移矩阵设置////////////////////////////////////////////////
	ClearMatrix(m_mTranslateMatrix);//转移阵清0
	for (int i=0;i<2;i++)
	{
		m_mTranslateMatrix(i,i)=1;		
	}
	//////////////////////////////////////////////////////////////////////////
	m_cObserveMode.InitObserve(m_mTranslateMatrix);//注意需初始化观测模型中的转移阵，观测模型内用的是引用
	my_filter.initFilter(m_sInitialStateVal,m_vInitStateCov);		

	m_cAdaptiveOptimiser.initAdapter(m_vInitModelNoise,1.1);
	return true;
}

void CObjectTracker::RunTrack(double dl,double dr,OBSERVE_STYLE& OBSERVEDATA,double TimeStep)
{
	
#ifdef USE_DEGREE_ANGLE
		OBSERVEDATA(1)=TransAngleToDegree(OBSERVEDATA(1));
#endif
		//#ifdef  DEBUG_USE_FLAG
		//		ViewVector(my_filter.m_mStateMeanResult);
		//		m_mSigma_Viewer;
		//#endif
		//robot_console_printf("dl=%f\n",dl);
		
		//if (QueryPerformanceCounter(&m_LICurrentTimeCount))//把当前的读数值暂存在m_LICurrentTimeCount
		//{
		//	double StepCount=(double)(m_LICurrentTimeCount.QuadPart-LIPreTimeCount.QuadPart);
		//	m_dTimeStep=StepCount*m_dTimePerSystemCount;
		//	//LIPreTimeCount=CountNumTmp;
		//}
		//else
		//{
		//	//TRACE("No time count Getted!!\n");
		//}
		
		my_filter.Predict(my_filter.m_mStateMeanResult,my_filter.m_mStateCov,m_cPredictMode,
			m_vInitControlNoise,m_cAdaptiveOptimiser.m_mNewModelNoise,dl,dr,TimeStep);

		my_filter.Update(my_filter.m_mStateMeanResult,my_filter.m_mStateCov,m_cObserveMode,
			OBSERVEDATA,m_vInitObserveNoise,m_mTranslateMatrix);
		m_cAdaptiveOptimiser.ModelNoiseAdapter(my_filter.m_mResidual,my_filter.m_mPredictStateVar,my_filter.m_mStateCov,my_filter.m_mKalmanGain);
		//m_cAdaptiveOptimiser.ModelNoiseAdapter(my_filter.m_mResidual,my_filter.m_Covariance_S);
	/*	m_vResultDis.push_back(my_filter.m_mStateMeanResult(0));
		m_vResultAng.push_back(my_filter.m_mStateMeanResult(1));
		m_vResultDV.push_back(my_filter.m_mStateMeanResult(2));
		m_vResultAV.push_back(my_filter.m_mStateMeanResult(3));*/
}
bool CObjectTracker::RunTrack(double dl,double dr,OBSERVE_STYLE& OBSERVEDATA,double TimeStep,CAdaptiveOptimaser* pAdaptiveOptimizer)
{

#ifdef USE_DEGREE_ANGLE
	OBSERVEDATA(1)=TransAngleToDegree(OBSERVEDATA(1));
#endif
	//#ifdef  DEBUG_USE_FLAG
	//		ViewVector(my_filter.m_mStateMeanResult);
	//		m_mSigma_Viewer;
	//#endif
	//robot_console_printf("dl=%f\n",dl);
	
//	__asm int 3;
	//if (QueryPerformanceCounter(&m_LICurrentTimeCount))//把当前的读数值暂存在m_LICurrentTimeCount
	//{
	//	double StepCount=(double)(m_LICurrentTimeCount.QuadPart-LIPreTimeCount.QuadPart);
	//	m_dTimeStep=StepCount*m_dTimePerSystemCount;
	//	robot_console_printf("m_dTimeStep=%f",m_dTimeStep);
	//	//LIPreTimeCount=CountNumTmp;
	//}
	//else
	//{
	//	//TRACE("No time count Getted!!\n");
	//}

	my_filter.Predict(my_filter.m_mStateMeanResult,my_filter.m_mStateCov,m_cPredictMode,
		m_vInitControlNoise,pAdaptiveOptimizer->m_mNewModelNoise,dl,dr,TimeStep);

	my_filter.Update(my_filter.m_mStateMeanResult,my_filter.m_mStateCov,m_cObserveMode,
		OBSERVEDATA,m_vInitObserveNoise,m_mTranslateMatrix);
	pAdaptiveOptimizer->ModelNoiseAdapter(my_filter.m_mResidual,my_filter.m_mPredictStateVar,my_filter.m_mStateCov,my_filter.m_mKalmanGain);
	//m_cAdaptiveOptimiser.ModelNoiseAdapter(my_filter.m_mResidual,my_filter.m_Covariance_S);
	/*	m_vResultDis.push_back(my_filter.m_mStateMeanResult(0));
	m_vResultAng.push_back(my_filter.m_mStateMeanResult(1));
	m_vResultDV.push_back(my_filter.m_mStateMeanResult(2));
	m_vResultAV.push_back(my_filter.m_mStateMeanResult(3));*/
	return true;
}
// 简单判断一下输入的观测数据是否合理,与上一次的相差太多则认为不合理.此函数输入的观测数据中的角度单位为度
bool CObjectTracker::FiltObserveData(OBSERVE_STYLE& OBSERVEDATA,OBSERVE_STYLE& Pre_OBSERVEDATA)
{
#ifdef USE_DEGREE_ANGLE
	double AngleDis=GetShortDistanceBetween2Angle(OBSERVEDATA(1),Pre_OBSERVEDATA(1));
	if (fabs(Pre_OBSERVEDATA(0)-OBSERVEDATA(0))>1000||AngleDis>60)
	{
		return false;
	}
#else
	double AngleDis=fabs(GetAngulardisplacementIn2Angle(OBSERVEDATA(1),Pre_OBSERVEDATA(1)))
	if (fabs(Pre_OBSERVEDATA(0)-OBSERVEDATA(0))>1000||AngleDis>0.1667)
	{
		return false;
	}
#endif
	return true;
	
}