#pragma once
#include <Windows.h>
#include "UKF_ObjectTracHead.h"
//#include <device/robot.h>
/********************************************************************
	created:	2008/02/23
	created:	23:2:2008   22:01
	filename: 	c:\boosttest\MyTargetTrack\MyTargetTrack\ObjectTracker.h
	file path:	c:\boosttest\MyTargetTrack\MyTargetTrack
	file base:	ObjectTracker
	file ext:	h
	author:		GaoYang
	
	purpose:	作為一個目標跟蹤器,進行目標跟蹤,包含ukf濾波器及自適應調整器.使用前首先要調InitFlt,開始跟蹤時用RunTrack函數
				其中的初值方差等都需要在InitFlt函數修改.
				結果放在其內類成員my_filter的成員變量my_filter.m_mStateMeanResult中.
	todo:       目前還沒有考慮返回方差,狀態等
*********************************************************************/
class CObjectTracker:public CInitStateOfUKF
{
public:
	CObjectTracker(void);
public:
	virtual~CObjectTracker(void);

public:
	TRANSTATE_MAT_TYPE m_mTranslateMatrix;//觀測模型用的轉移矩陣必須聲明在m_cObserveMode之前
	CPredictModel m_cPredictMode;///<預測模型
	CObservModel m_cObserveMode;///<觀測模型
	CUKalmanFilter my_filter;///<濾波器
	CAdaptiveOptimaser m_cAdaptiveOptimiser;///<自適應調整器
public:
	/**
	*@brief 初始化濾波器,設定初值等,其具體值需在本函數內修改
	*/
	bool InitFlt(void);
	
public:
	/**
	*@brief 開始運行跟蹤，觀測，獲得速度
	*@parm dl 觀測者左輪位移
	*@parm dr 觀測者右輪位移
	*@parm OBSERVEDATA 觀測數據,boost中的vector格式,目前僅有2個內容,其中的角度,輸入時必須為弧度制,內部會依據USE_DEGREE_ANGLE宏是否定義來確定是否需要再轉化為角度
	*@parm LIPreTimeCOunt上一次進行跟蹤時的系統時針計數點數
	*/
	void RunTrack(double dl,double dr,OBSERVE_STYLE& OBSERVEDAT,double TimeStep);
	/**
	*@brief 開始運行跟蹤，觀測，獲得速度，本函數中的自適應調節器由外部指定，多目標跟蹤模式時使用
	*@parm dl 觀測者左輪位移
	*@parm dr 觀測者右輪位移
	*@parm OBSERVEDATA 觀測數據,boost中的vector格式,目前僅有2個內容,其中的角度,輸入時必須為弧度制,內部會依據USE_DEGREE_ANGLE宏是否定義來確定是否需要再轉化為角度
	*@parm LIPreTimeCOunt上一次進行跟蹤時的系統時針計數點數
	*@parm pAdaptiveOptimizer自適應調節器類指針
	*@output 若跟蹤沒有成功則返回假
	*/
	bool RunTrack(double dl,double dr,OBSERVE_STYLE& OBSERVEDATA,double TimeStep,CAdaptiveOptimaser* pAdaptiveOptimizer);
#ifdef CONSIDER_TIME_IN_VEL
	//double m_dTimePerSystemCount;///<每一個系統時間點對應的ms數
	//double m_dTimeStep;///<時間步長單位毫秒
	//LARGE_INTEGER m_LICurrentTimeCount;///<當前時間的點數
#endif

	/**
	*供外部改變濾波器的狀態，狀態方差，模型噪聲方差，以按需要濾波
	*/
	bool SetFltState(STATE_STYLE& State,VARIANCE& StateVariance,VARIANCE& ModelNoiseVariance);

public:
	// 簡單判斷一下輸入的觀測數據是否合理,與上一次的相差太多則認為不合理.此函數輸入的觀測數據中的角度單位為度
	bool FiltObserveData(OBSERVE_STYLE& OBSERVEDATA,OBSERVE_STYLE& Pre_OBSERVEDATA);
};