S3315_RK3588/NeoTracker/src/Common/Arith_AglPredict.cpp

/*********版权所有（C）2018，武汉高德红外股份有限公司******************
* 文件名称： Arith_Predict.h
* 文件标识： 对跟踪的目标进行预测分析
* 内容摘要： 包含长短时预测、航迹预测等 
* 其它说明： "Arith_Precit.h"的函数、全局变量、宏定义，统一前缀为简写"NL_"
* 当前版本： 固化V1.0
* 创建作者： zy
* 创建日期： 2018.01.29
*****************************************************************************/
#include "Arith_AglPredict.h"
#include "Arith_Common.hpp"




/**********************************************************
* 函数名称： TO_ObjAglTrackPredict()
* 功能描述：目标当前帧轨迹预测：利用长短时目标方位、俯仰航迹
* 输入参数： SINT32 nWidth   --  图像宽度
*             SINT32 nHeight   --  图像高度
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：预测目标在当前帧的中心点, 以及搜索区间 20180129
**********************************************************/
void Predict_ObjAglTrackPredict(FilterMeanNL* pFilter, SINT32 nWidth, SINT32 nHeight, GLB_INPUT* p_GLB_Input)
{
	OBJ_ANGLE_R *pObjAglListsNear = &pFilter->ObjAglListsNear;
	OBJ_ANGLE_R *pObjAglListsLong = &pFilter->ObjAglListsLong;
	ANGLE_R *parHistoryListNear   = (ANGLE_R*)pObjAglListsNear->parHistoryList;
	ANGLE_R *parHistoryListLong    = (ANGLE_R*)pObjAglListsLong->parHistoryList;
	SINT32  nEndNear, nEndLong;
	UINT32  unFrmIdPreNear, unFrmIdPreLong;
	UINT32  unFrmId, unFrmStepNear, unFrmStepLong;
	ANGLE32F fAglPreNear, fAglPreLong;
	ANGLE32F fAglSpeedNear, fAglSpeedLong;
	ANGLE32F fAglPredictNear, fAglPredictLong;
	FLOAT32 fRPreNear, fRSpeedNear, fRPredictNear;
	FLOAT32 fRPreLong, fRSpeedLong, fRPredictLong;
	CENTERRECT crnSrRectNear, crnSrRectLong;
	SPEED32F fSpeedNear, fSpeedLong;
	SIZE32S snSrRadiusNear, snSrRadiusLong;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//初始化
	unFrmId = p_GLB_Input->unFrmId;
	pObjAglListsNear->arfPredict.unFrmID = unFrmId;
	pObjAglListsLong->arfPredict.unFrmID = unFrmId;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//目标短时间轨迹预测
	//计算当前帧与上一次轨迹记录的间隔帧数
	nEndNear = pObjAglListsNear->nEnd;
	unFrmIdPreNear = parHistoryListNear[nEndNear].unFrmID;
	if (unFrmId > unFrmIdPreNear)
	{
		unFrmStepNear = unFrmId - unFrmIdPreNear;
	}
	else
	{
		unFrmStepNear = unFrmIdPreNear - unFrmId;
	}

	//获取上一次轨迹记录的目标角度、角速度、距离、距离变化速度
	fAglPreNear = parHistoryListNear[nEndNear].afAngle;      //目标角度
	fAglSpeedNear = pObjAglListsNear->arfSpeed.afAngle;      //目标角速度 20171023zy:角速度是滤波后的结果
	fRPreNear = parHistoryListNear[nEndNear].fDist;          //距离      
	fRSpeedNear = pObjAglListsNear->arfSpeed.fDist;          //距离变化速度

	//估算当前帧预测的目标角度、距离
	fAglPredictNear.fAz = DEGLIM360(fAglPreNear.fAz + fAglSpeedNear.fAz * unFrmStepNear);
	fAglPredictNear.fPt = DEGLIM(fAglPreNear.fPt + fAglSpeedNear.fPt * unFrmStepNear);
	fRPredictNear = fRPreNear + fRSpeedNear * unFrmStepNear;
	pObjAglListsNear->arfPredict.afAngle = fAglPredictNear;      //20171023zy:根据预测的角度来计算预测的位置点      
	pObjAglListsNear->arfPredict.fDist   = fRPredictNear;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//估算当前帧搜索区域矩形
	//搜索区域中心

	// 稳定系到图像坐标系
	Pole targetCarNUEPole;
	targetCarNUEPole.alpha = fAglPredictNear.fPt;
	targetCarNUEPole.beta = fAglPredictNear.fAz;
	targetCarNUEPole.distance = 0;


	POINT32F imgPos = getImagePosFromStablePole(targetCarNUEPole, p_GLB_Input->stCamera, p_GLB_Input->servoInfo, p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr
	);
	crnSrRectNear.cx = round(imgPos.x);
	crnSrRectNear.cy = round(imgPos.y);

	//搜索区域半径
	fSpeedNear.vx = fAglSpeedNear.fAz / p_GLB_Input->stCamera.fAglReso;   //由角速度转换为线速度
	fSpeedNear.vy = fAglSpeedNear.fPt / p_GLB_Input->stCamera.fAglReso;   //由角速度转换为线速度 

	snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), 30 );   //20180129zy:  
	snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), 30 );  //20180129zy: g_DST_stPara.nPipeRadiusTrack
	crnSrRectNear.w = 2 * snSrRadiusNear.w + 1;
	crnSrRectNear.h = 2 * snSrRadiusNear.h + 1;
	crnSrRectNear.s = crnSrRectNear.w * crnSrRectNear.h;
	pFilter->crnObjPrediRtNear = crnSrRectNear;               //======20171023zy：短时轨迹的预测位置======
	IMGO_CenRect16S2MMRect32S(crnSrRectNear, &pFilter->mrnObjPrediRtNear,nWidth, nHeight);

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//记录目标长时间轨迹（每秒记录一次）
	//在长时间轨迹更新第2个位置之前，一直跟短时间轨迹预测位置保持一致
	if (pObjAglListsLong->nCnt <= 2)
	{
		fAglSpeedLong = fAglSpeedNear;
		fAglPredictLong = pObjAglListsNear->arfPredict.afAngle;
		pObjAglListsLong->arfPredict.afAngle = fAglPredictLong;
	}
	else
	{
		//计算当前帧与上一次轨迹记录的间隔帧数
		nEndLong = pObjAglListsLong->nEnd;
		unFrmIdPreLong = parHistoryListLong[nEndLong].unFrmID;
		if (unFrmId > unFrmIdPreLong)
		{
			unFrmStepLong = unFrmId - unFrmIdPreLong;
		}
		else
		{
			unFrmStepLong = unFrmIdPreLong - unFrmId;
		}

		//获取上一次轨迹记录的目标角度、角速度、距离、距离变化速度
		fAglPreLong = parHistoryListLong[nEndLong].afAngle;
		fAglSpeedLong = pObjAglListsLong->arfSpeed.afAngle;
		fRPreLong = parHistoryListLong[nEndLong].fDist;
		fRSpeedLong = pObjAglListsLong->arfSpeed.fDist;

		//估算当前帧预测的目标角度、距离
		fAglPredictLong.fAz = DEGLIM360(fAglPreLong.fAz + fAglSpeedLong.fAz * unFrmStepLong);
		fAglPredictLong.fPt = DEGLIM(fAglPreLong.fPt + fAglSpeedLong.fPt * unFrmStepLong);
		fRPredictLong = fRPreLong + fRSpeedLong * unFrmStepLong;
		pObjAglListsLong->arfPredict.afAngle = fAglPredictLong;

		pObjAglListsLong->arfPredict.fDist = fRPredictLong;
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//估算当前帧搜索区域矩形
	//搜索区域中心

	// 稳定系到图像坐标系
	targetCarNUEPole.alpha = fAglPredictLong.fPt;
	targetCarNUEPole.beta = fAglPredictLong.fAz;
	targetCarNUEPole.distance = 0;

	////getEOServoPoleFromCarNUEPole(targetCarNUEPole, &targetEOCamPole,
	////	g_GLB_stInput.afPlatformRPY.fYaw, g_GLB_stInput.afPlatformRPY.fPitch, g_GLB_stInput.afPlatformRPY.fRoll,
	////	g_SERVO_stInput.fPitching, g_SERVO_stInput.fAzimuth);

	////crnSrRectLong.cx = SERVO_CalcObjCoordinateX((FLOAT32)targetEOCamPole.beta, g_GLB_stInput.nImageWidth,
	////	0, g_SERVO_stInput.fResolAz);

	////crnSrRectLong.cy = SERVO_CalcObjCoordinateY((FLOAT32)targetEOCamPole.alpha, g_GLB_stInput.nImageHeight,
	////	0, g_SERVO_stInput.fResolPt);

	imgPos = getImagePosFromStablePole(targetCarNUEPole, p_GLB_Input->stCamera, p_GLB_Input->servoInfo, p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr);
	crnSrRectLong.cx = round(imgPos.x);
	crnSrRectLong.cy = round(imgPos.y);



	//搜索区域半径
	fSpeedLong.vx = fAglSpeedLong.fAz / p_GLB_Input->stCamera.fAglReso;
	fSpeedLong.vy = fAglSpeedLong.fPt / p_GLB_Input->stCamera.fAglReso;
	snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), 30); 
	snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), 30); 
	crnSrRectLong.w = 2 * snSrRadiusLong.w + 1;
	crnSrRectLong.h = 2 * snSrRadiusLong.h + 1;
	crnSrRectLong.s = crnSrRectLong.w * crnSrRectLong.h;
	pFilter->crnObjPrediRtLong = crnSrRectLong;        //========20171023zy:长时轨迹的预测位置======
	IMGO_CenRect16S2MMRect32S(crnSrRectLong, &pFilter->mrnObjPrediRtLong,nWidth, nHeight);

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//判断长短轨迹预测位置是否一致
	DIST32S dnNLDist, dnNLDistPre;
	dnNLDistPre = pFilter->dnObjPredictDist;
	dnNLDist.dx = ABS(crnSrRectNear.cx - crnSrRectLong.cx);
	dnNLDist.dy = ABS(crnSrRectNear.cy - crnSrRectLong.cy);
	SINT32 fNLDist2 = dnNLDist.dx * dnNLDist.dx + dnNLDist.dy * dnNLDist.dy;
	SINT32 fNLDist2Pre = dnNLDistPre.dx * dnNLDistPre.dx + dnNLDistPre.dy * dnNLDistPre.dy;
	//SINT32 nNLDist2Thres = g_DST_stPara.nPipeRadiusTrack * g_DST_stPara.nPipeRadiusTrack;
	// SINT32   nNLDist2Thres = 20*20;
	//SINT32 nNLDist2Thres = (g_GLB_stOutput.ObjectStatus.sfSize.h * g_GLB_stOutput.ObjectStatus.sfSize.w) *  25;
	SINT32 nNLDist2Thres = 10 * 25;
    nNLDist2Thres = MAX(nNLDist2Thres,100);

	if (fNLDist2 - fNLDist2Pre > 2 
		|| fNLDist2 > nNLDist2Thres)
	{
		pFilter->nObjPredictFarCnt++;
	}
	else
	{
		pFilter->nObjPredictFarCnt = 0;
		pFilter->bObjPredictAbnormal = FALSE;
	}
	if (pFilter->nObjPredictFarCnt > 10
		&& fNLDist2 > nNLDist2Thres)
	{
		pFilter->bObjPredictAbnormal = TRUE;
	}
	pFilter->dnObjPredictDist = dnNLDist;

	////利用长短时轨迹判断是否开启惯性预测,采用多帧判断          by wcw04046 @ 2022/01/17
	//if (g_GLB_stOutput.nObjPredictFarCnt == 0)
	//{
	//	g_GLB_stPara.nServoTrackSta++;
	//}
	//else
	//{
	//	g_GLB_stPara.nServoTrackSta = 0;//一旦出现预测错误，直接关闭。
	//}
	//if(g_GLB_stPara.nServoTrackSta >= 50)//连续50帧稳定预测，表明目标基本匀速运动，可以开启角度预测
	//{
	//	g_GLB_stPara.bEnableServoTrackPredi = true;
	//}
	//else
	//{
	//	g_GLB_stPara.bEnableServoTrackPredi = false;
	//}
}


/**********************************************************
* 函数名称：TO_ObjAglTrackFilter()
* 功能描述：目标轨迹滤波及速度估算
* 输入参数：OBJ_ANGLE_R *parObjTrackList   --   目标短时间轨迹数组
*            SINT32 nFilterCntThres          --   管道的滤波帧数
*            OBJECTSTATUS *pObjStatus        --   跟踪目标信息结构体
* 输出参数：OBJ_ANGLE_R *parObjTrackList   --   目标短时间轨迹数组
* 返 回 值：无
* 调用关系：无
* 其它说明：短时轨迹用于目标实时位置滤波，长时轨迹用于目标航迹判断。【待添加】
**********************************************************/
void Predict_ObjAglTrackFilter(FilterMeanNL* pFilter,OBJ_ANGLE_R *parObjTrackList, SINT32 nFilterCntThres, TARGET_OBJECT* pTarget,GLB_INPUT* p_GLB_Input)
{
	ANGLE_R *parHistoryList = (ANGLE_R*)parObjTrackList->parHistoryList;
	SINT32  i, m;
	SINT32  nFilterCnt;//滤波帧数
	SINT32  nSpeedStep;//速度间隔帧数
	SINT32  nListSize = parObjTrackList->nListSize;  //目标角度数组长度
	FLOAT32 fAz, fPt;
	FLOAT32 fAzSum, fPtSum, fRSum;
	FLOAT32 fAzSpeed, fPtSpeed, fRSpeed;
	FLOAT32 fAzPre, fPtPre, fRPre;
	FLOAT32 fAzCurr, fPtCurr, fRCurr;
	SINT32  nEnd, nPre;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//初始化
	m       = parObjTrackList->nEnd;
	fAzSum  = 0.0f;
	fPtSum  = 0.0f;
	fRSum   = 0.0f;
	fAzSpeed= 0.0f;
	fPtSpeed= 0.0f;
	fRSpeed = 0.0f;
	parObjTrackList->arfFilter.unFrmID = p_GLB_Input->unFrmId;
	parObjTrackList->arfSpeed.unFrmID = p_GLB_Input->unFrmId;

	//防错处理
	if (nFilterCntThres <= 1)
	{
		parObjTrackList->nFilterCnt = 1;
		parObjTrackList->arfFilter.afAngle.fAz = parHistoryList[m].afAngle.fAz;
		parObjTrackList->arfFilter.afAngle.fPt = parHistoryList[m].afAngle.fPt;
		parObjTrackList->arfFilter.fDist = parHistoryList[m].fDist;
		//parObjTrackList->arfSpeed.afAngle.fAz = pTarget->sfAglSpeed.vx;
		//parObjTrackList->arfSpeed.afAngle.fPt = pTarget->sfAglSpeed.vy;          //20180626zy：目标角速度长短时
		parObjTrackList->arfSpeed.fDist = 1.0f;
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//方位角滤波
	//判断角度是否跨越0/360度边界
	FLOAT32 fObjSpeedX = parObjTrackList->arfSpeed.afAngle.fAz / p_GLB_Input->stCamera.fAglReso;
	if (parObjTrackList->nCnt > 1)
	{
		//MSSu, 20150617: 测试发现若某帧目标速度>=20，nFilterCnt=1后，
		//  目标角速度一直未更新，即使目标速度已下降。
		//  因此，此处将目标短时预测角速度与相邻帧角速度取均值。
		//MSSu, 20150808: 计算速度时，增加帧间隔，因长短轨迹记录的帧间隔不一致。
		nEnd = parObjTrackList->nEnd;
		nPre = (parObjTrackList->nEnd - 1 + nListSize) % nListSize;
		fAzCurr = parHistoryList[nEnd].afAngle.fAz;
		fAzPre = parHistoryList[nPre].afAngle.fAz;
		FLOAT32 fObjSpeedXNear = DEGLIM(fAzCurr - fAzPre);
		fObjSpeedXNear /= parObjTrackList->nStep;
		fObjSpeedXNear /= p_GLB_Input->stCamera.fAglReso;
		fObjSpeedX = (fObjSpeedX + fObjSpeedXNear) / 2;
	}
	fObjSpeedX = MAX(ABS(fObjSpeedX), 1.0f);
	nFilterCnt = MAX((SINT32)(nFilterCntThres / fObjSpeedX * 1.5f), 1);
	nFilterCnt = MIN(nFilterCnt, parObjTrackList->nCnt);
	m = parObjTrackList->nEnd;
	fAzSum = parHistoryList[m].afAngle.fAz;
	FLOAT32 fAzMin = 360.0f;
	FLOAT32 fAzMax = 0.0f;
	BBOOL bAzCorrect = FALSE;
	for (i = 0; i < nFilterCnt; i++)
	{
		fAzMin = MIN(fAzMin, parHistoryList[m].afAngle.fAz);
		fAzMax = MAX(fAzMax, parHistoryList[m].afAngle.fAz);
		m = (m + nListSize - 1) % nListSize;
	}
	if (fabs(fAzMin - fAzMax) > 180.0f)
	{
		bAzCorrect = TRUE;
	}
	m = parObjTrackList->nEnd;
	for (i = 1; i < nFilterCnt; i++)
	{
		m = (m + nListSize - 1) % nListSize;
		fAz = parHistoryList[m].afAngle.fAz;
		if (bAzCorrect && fAz < 180.0f)
		{
			fAzSum += fAz + 360.f;
		}
		else
		{
			fAzSum += fAz;
		}
	}
	parObjTrackList->arfFilter.afAngle.fAz = DEGLIM360(fAzSum / nFilterCnt);

	//方位角速度估算
	nFilterCnt  = MIN(MAX(2, nFilterCnt), parObjTrackList->nCnt);
	nEnd        = parObjTrackList->nEnd;
	nPre        = (parObjTrackList->nEnd - (nFilterCnt - 1) + nListSize) % nListSize;
	fAzCurr     = parHistoryList[nEnd].afAngle.fAz;
	fAzPre      = parHistoryList[nPre].afAngle.fAz;
	if (parHistoryList[nEnd].unFrmID > parHistoryList[nPre].unFrmID)
	{
		nSpeedStep = (SINT32)(parHistoryList[nEnd].unFrmID - parHistoryList[nPre].unFrmID);
	}
	else
	{
		nSpeedStep = (SINT32)(parHistoryList[nPre].unFrmID - parHistoryList[nEnd].unFrmID);
	}
	if (1 == parObjTrackList->nCnt || 0 == nSpeedStep)
	{
		//fAzSpeed = pObjStatus->sfAglSpeed.vx;
	}
	else
	{
		//=======201800626zy？？fAzPre=0.0f没有值，所以计算的角速度很大======
		fAzSpeed = DEGLIM(fAzCurr - fAzPre) / nSpeedStep;
	}
	parObjTrackList->arfSpeed.afAngle.fAz = fAzSpeed;        //20180626zy:目标角速度长短时              

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//俯仰角滤波
	//判断角度是否跨越0/360度边界
	FLOAT32 fObjSpeedY = parObjTrackList->arfSpeed.afAngle.fPt / p_GLB_Input->stCamera.fAglReso;
	if (parObjTrackList->nCnt > 1)
	{
		//MSSu, 20150617: 测试发现若某帧目标速度>=20，nFilterCnt=1后，
		//  目标角速度一直未更新，即使目标速度已下降。
		//  因此，此处将目标短时预测角速度与相邻帧角速度取均值。
		//MSSu, 20150808: 计算速度时，增加帧间隔，因长短轨迹记录的帧间隔不一致。
		nEnd = parObjTrackList->nEnd;
		nPre = (parObjTrackList->nEnd - 1 + nListSize) % nListSize;
		fPtCurr = parHistoryList[nEnd].afAngle.fPt;
		fPtPre = parHistoryList[nPre].afAngle.fPt;
		FLOAT32 fObjSpeedYNear = DEGLIM(fPtCurr - fPtPre);
		fObjSpeedYNear /= parObjTrackList->nStep;
		fObjSpeedYNear /= p_GLB_Input->stCamera.fAglReso;
		fObjSpeedY = (fObjSpeedY + fObjSpeedYNear) / 2;
	}
	fObjSpeedY = MAX(ABS(fObjSpeedY), 1.0f);
	nFilterCnt = MAX((SINT32)(nFilterCntThres / fObjSpeedY), 1);
	nFilterCnt = MIN(nFilterCnt, parObjTrackList->nCnt);
	m = parObjTrackList->nEnd;
	fPtSum = parHistoryList[m].afAngle.fPt;
	FLOAT32 fPtMin = 360.0f;
	FLOAT32 fPtMax = 0.0f;
	BBOOL bPtCorrect = FALSE;
	for (i = 0; i < nFilterCnt; i++)
	{
		fPtMin = MIN(fPtMin, parHistoryList[m].afAngle.fPt);
		fPtMax = MAX(fPtMax, parHistoryList[m].afAngle.fPt);
		m = (m + nListSize - 1) % nListSize;
	}
	if (fabs(fPtMin - fPtMax) > 180.0f)
	{
		bPtCorrect = TRUE;
	}
	m = parObjTrackList->nEnd;
	for (i = 1; i < nFilterCnt; i++)
	{
		m = (m + nListSize - 1) % nListSize;
		fPt = parHistoryList[m].afAngle.fPt;
		if (bPtCorrect && fPt < 180.0f)
		{
			fPtSum += fPt + 360.0f;
		}
		else
		{
			fPtSum += fPt;
		}
	}
	parObjTrackList->arfFilter.afAngle.fPt = DEGLIM(fPtSum / nFilterCnt);

	//俯仰角速度估算
	nFilterCnt  = MIN(MAX(2, nFilterCnt), parObjTrackList->nCnt);
	nEnd        = parObjTrackList->nEnd;
	nPre        = (parObjTrackList->nEnd - (nFilterCnt - 1) + nListSize) % nListSize;
	fPtCurr     = parHistoryList[nEnd].afAngle.fPt;
	fPtPre      = parHistoryList[nPre].afAngle.fPt;
	if (parHistoryList[nEnd].unFrmID > parHistoryList[nPre].unFrmID)
	{
		nSpeedStep = (SINT32)(parHistoryList[nEnd].unFrmID - parHistoryList[nPre].unFrmID);
	}
	else
	{
		nSpeedStep = (SINT32)(parHistoryList[nPre].unFrmID - parHistoryList[nEnd].unFrmID);
	}
	if (1 == parObjTrackList->nCnt || 0 == nSpeedStep)
	{
		//fPtSpeed = pObjStatus->sfAglSpeed.vy;
	}
	else
	{
		fPtSpeed = DEGLIM(fPtCurr - fPtPre) / nSpeedStep;
	}
	parObjTrackList->arfSpeed.afAngle.fPt = fPtSpeed;              //20180626zy：目标角速度长短时

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//距离滤波
	nFilterCnt = MIN(nFilterCntThres, parObjTrackList->nCnt);
	m = parObjTrackList->nEnd;
	fRSum = 0.0f;
	for (i = 0; i < nFilterCnt; i++)
	{
		fRSum = fRSum + parHistoryList[m].fDist;
		m = (m + nListSize - 1) % nListSize;
	}
	parObjTrackList->arfFilter.fDist = fRSum / nFilterCnt;

	//距离变化速度估算
	nEnd        = parObjTrackList->nEnd;
	nPre        = (parObjTrackList->nEnd - (nFilterCnt - 1) + nListSize) % nListSize;
	fRCurr      = parHistoryList[nEnd].fDist;
	fRPre       = parHistoryList[nPre].fDist;
	if (parHistoryList[nEnd].unFrmID > parHistoryList[nPre].unFrmID)
	{
		nSpeedStep = (SINT32)(parHistoryList[nEnd].unFrmID - parHistoryList[nPre].unFrmID);
	}
	else
	{
		nSpeedStep = (SINT32)(parHistoryList[nPre].unFrmID - parHistoryList[nEnd].unFrmID);
	}
	nSpeedStep  = MAX(nSpeedStep, 1);
	fRSpeed     = (fRCurr - fRPre) / nSpeedStep;
	parObjTrackList->arfSpeed.fDist = fRSpeed;
}


/**********************************************************
* 函数名称：Predict_CleanUpObjAglTrackInfo()
* 功能描述：清空目标跟踪轨迹信息
* 输入参数：无
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：无
**********************************************************/
void Predict_CleanUpObjAglTrackInfo(FilterMeanNL* pFilter)
{
	//初始化目标短时间轨迹数组（每帧记录一次）
	pFilter->ObjAglListsNear.nCnt  = 0;
	pFilter->ObjAglListsNear.nEnd  = 0;
	pFilter->ObjAglListsNear.nStep = 1;
	pFilter->ObjAglListsNear.nListSize = GLB_OBJTRACK_LEN;
	memset(pFilter->ObjAglListsNear.parHistoryList, 0, sizeof(ANGLE_R) * GLB_OBJTRACK_LEN);
	memset(&pFilter->ObjAglListsNear.arfFilter, 0, sizeof(ANGLE_R));

	//初始化目标长时间轨迹数组（每25帧记录一次）
	pFilter->ObjAglListsLong.nCnt  = 0;
	pFilter->ObjAglListsLong.nEnd  = 0;
	pFilter->ObjAglListsLong.nStep = GLB_FRM_FREQ / 4;
	pFilter->ObjAglListsLong.nListSize = GLB_OBJTRACK_LEN;
	memset(pFilter->ObjAglListsLong.parHistoryList, 0, sizeof(ANGLE_R) * GLB_OBJTRACK_LEN);
	memset(&pFilter->ObjAglListsLong.arfFilter, 0, sizeof(ANGLE_R));

	//初始化长短轨迹差异标志
	pFilter->dnObjPredictDist.dx = 0;
	pFilter->dnObjPredictDist.dy = 0;
	pFilter->nObjPredictFarCnt   = 0;
	pFilter->bObjPredictAbnormal = FALSE;
	pFilter->bObjTrackRefoundSucc = FALSE;
}


/**********************************************************
* 函数名称：TO_RecordObjAglTrackInfo()
* 功能描述：记录目标跟踪轨迹信息
* 输入参数：无
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：短时轨迹用于目标实时位置滤波，长时轨迹用于目标航迹判断。【待添加】
**********************************************************/
void Predict_RecordObjAglTrackInfo(FilterMeanNL* pFilter, TARGET_OBJECT* pTarget, GLB_INPUT* p_GLB_Input)
{
	//OBJECTSTATUS *pObjStatus = &pObjStatus;
	OBJ_ANGLE_R *pObjAglListsNear = &pFilter->ObjAglListsNear;
	OBJ_ANGLE_R *pObjAglListsLong = &pFilter->ObjAglListsLong;
	ANGLE_R *parHistoryListNear = (ANGLE_R*)pObjAglListsNear->parHistoryList;
	ANGLE_R *parHistoryListLong = (ANGLE_R*)pObjAglListsLong->parHistoryList;
	SINT32 nEndNear, nEndLong;
	UINT32 unFrmIdPreNear, unFrmIdPreLong;
	SINT32 nFrmStepNear, nFrmStepLong;

	// 上位机测试时，当跟踪过程中接受到解锁时，队列指针被清空，报错保护           by wcw04046 @ 2022/01/17
	// 指针使用前判空
	if(parHistoryListNear == NULL || parHistoryListLong == NULL)
	{
		return;
	}
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//记录目标短时间轨迹（每帧记录一次）
	if (0 == pObjAglListsNear->nCnt)
	{
		nEndNear = 0;
		parHistoryListNear[nEndNear].unFrmID = p_GLB_Input->unFrmId;
		parHistoryListNear[nEndNear].afAngle.fAz = pTarget->afAngle.fAz;
		parHistoryListNear[nEndNear].afAngle.fPt = pTarget->afAngle.fPt;
		pObjAglListsNear->nCnt++;
		pObjAglListsNear->nEnd = nEndNear;
	}
	else
	{
		//上一次记录的帧数
		nEndNear = pObjAglListsNear->nEnd;
		unFrmIdPreNear = parHistoryListNear[nEndNear].unFrmID;

		//若达到记录帧间隔
		if (p_GLB_Input->unFrmId > unFrmIdPreNear)
		{
			nFrmStepNear = (SINT32)(p_GLB_Input->unFrmId - unFrmIdPreNear);
		}
		else
		{
			nFrmStepNear = (SINT32)(unFrmIdPreNear - p_GLB_Input->unFrmId);
		}

		if ((nFrmStepNear >= pObjAglListsNear->nStep) /*&& (pObjStatus->unContiLostCnt <= 0)*/)
		{
			nEndNear = (pObjAglListsNear->nEnd + 1) % pObjAglListsNear->nListSize;
			parHistoryListNear[nEndNear].unFrmID = p_GLB_Input->unFrmId;
			parHistoryListNear[nEndNear].afAngle.fAz = pTarget->afAngle.fAz;
			parHistoryListNear[nEndNear].afAngle.fPt = pTarget->afAngle.fPt;
			pObjAglListsNear->nCnt++;
			pObjAglListsNear->nEnd = nEndNear;
		}
	}

	//目标短时轨迹滤波及速度估算
	Predict_ObjAglTrackFilter(pFilter,pObjAglListsNear, 20, pTarget, p_GLB_Input);


	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//记录目标长时间轨迹（每25帧记录一次）
	if (0 == pObjAglListsLong->nCnt)
	{
		nEndLong = 0;
		parHistoryListLong[nEndLong].unFrmID = p_GLB_Input->unFrmId;
		parHistoryListLong[nEndLong].afAngle.fAz = pTarget->afAngle.fAz;
		parHistoryListLong[nEndLong].afAngle.fPt = pTarget->afAngle.fPt;
		pObjAglListsLong->nCnt++;
		pObjAglListsLong->nEnd = nEndLong;
	}
	else
	{
		//上一次记录的帧数
		nEndLong = pObjAglListsLong->nEnd;
		unFrmIdPreLong = parHistoryListLong[nEndLong].unFrmID;

		//若长短轨迹预测异常，则不更新长时轨迹
		BBOOL bUpdateLong = true;
		if (pFilter->bObjPredictAbnormal 
			&& !pFilter->bObjTrackRefoundSucc)
		{
			bUpdateLong = false;
		}

		//若达到记录帧间隔
		if (p_GLB_Input->unFrmId > unFrmIdPreLong)
		{
			nFrmStepLong = (SINT32)(p_GLB_Input->unFrmId - unFrmIdPreLong);
		}
		else
		{
			nFrmStepLong = (SINT32)(unFrmIdPreLong - p_GLB_Input->unFrmId);
		}
		if (nFrmStepLong >= pObjAglListsLong->nStep 
			&& bUpdateLong
			/*&& (pObjStatus->unContiLostCnt <= 0)*/)
		{
			nEndLong = (pObjAglListsLong->nEnd + 1) % pObjAglListsLong->nListSize;
			parHistoryListLong[nEndLong].unFrmID = p_GLB_Input->unFrmId;
			parHistoryListLong[nEndLong].afAngle.fAz = pTarget->afAngle.fAz;
			parHistoryListLong[nEndLong].afAngle.fPt = pTarget->afAngle.fPt;
			pObjAglListsLong->nCnt++;
			pObjAglListsLong->nEnd = nEndLong;
		}
	}

	//目标长时轨迹滤波及速度估算
	if (pObjAglListsLong->nCnt <= 1)
	{
		pObjAglListsLong->arfFilter = pObjAglListsNear->arfFilter;
		pObjAglListsLong->arfSpeed = pObjAglListsNear->arfSpeed;
	}
	else
	{
		Predict_ObjAglTrackFilter(pFilter,pObjAglListsLong, 10, pTarget, p_GLB_Input);
	}


	// 长时轨迹异常处理
		//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//MSSu, 20150513: 记录长短时轨迹预测失败帧数
	POINT32F ptObjPos = pTarget->pfCenPos;
	MINMAXRECT32S mrnObjPrediNear = pFilter->mrnObjPrediRtNear;
	MINMAXRECT32S mrnObjPrediLong = pFilter->mrnObjPrediRtLong;
	if (mrnObjPrediNear.minX <= ptObjPos.x && ptObjPos.x <= mrnObjPrediNear.maxX
		&& mrnObjPrediNear.minY <= ptObjPos.y && ptObjPos.y <= mrnObjPrediNear.maxY)
	{
		pFilter->nObjTrackLostCntNear = 0;
	}
	else
	{
		pFilter->nObjTrackLostCntNear++;
	}
	if (mrnObjPrediLong.minX <= ptObjPos.x && ptObjPos.x <= mrnObjPrediLong.maxX
		&& mrnObjPrediLong.minY <= ptObjPos.y && ptObjPos.y <= mrnObjPrediLong.maxY)
	{
		pFilter->nObjTrackLostCntLong = 0;
	}
	else
	{
		pFilter->nObjTrackLostCntLong++;
	}

	//若长时轨迹预测失败次数过多，而短时轨迹预测成功，则可能为机动目标，
	//此时，重置长时轨迹
	if (pFilter->bObjPredictAbnormal
		&& pFilter->nObjTrackLostCntLong > 5//25  改小，更快的调整预测点位
		&& pFilter->nObjTrackLostCntNear <= 0)
	{
		pFilter->ObjAglListsLong.nCnt = 0;
		pFilter->ObjAglListsLong.nEnd = 0;
		pFilter->ObjAglListsLong.nStep = GLB_FRM_FREQ / 4;
		//g_GLB_stOutput.ObjAglListsLong.nListSize = GLB_OBJTRACK_LONG;
		//g_GLB_stOutput.ObjAglListsLong.parHistoryList = (ANGLE_R*)DSP_GLB_pObjAglListsLong;
		memset(pFilter->ObjAglListsLong.parHistoryList, 0, sizeof(ANGLE_R) * GLB_OBJTRACK_LEN);
		memset(&pFilter->ObjAglListsLong.arfFilter, 0, sizeof(ANGLE_R));
	}

}

//**********************************************************
//* 函数名称：TO_ObjAglTrackPredictNext()
//* 功能描述：目标轨迹预测：利用长短时目标方位、俯仰航迹，预测目标在下一帧的角度位置
//* 输入参数：SINT32 nWidth   --  图像宽度
//*           SINT32 nHeight  --  图像高度
//* 输出参数：无
//* 返 回 值：无
//* 调用关系：无
//* 其它说明：
//*   MSSu, 20171204: 将轨迹预测TO_ObjAglTrackPredict()拆分成两个部分执行：
//*   1）TO_ObjAglTrackPredictNext()“下一帧长短时角度预测” ：
//*       在当前帧TrackObject()结束时执行，
//*       因DSP中核A面目标检测用的搜索区域，是核B在前一帧算法结束后传递过来的。
//*   2）TO_ObjAglTrackPredictCurrent()“当前帧长短时预测的搜索区域”：
//*       在当前帧TrackObject()跟踪算法执行前执行，利用当前帧伺服角度计算图像预测区域。
//**********************************************************/
//void Predict_ObjAglTrackPredictNext(FilterMeanNL* pFilter,SINT32 nWidth, SINT32 nHeight, GLB_INPUT* g_GLB_stInput)
//{
//	//OBJECTSTATUS *pObjStatus = &g_GLB_stOutput.ObjectStatus;
//	OBJ_ANGLE_R *pObjAglListsNear = &pFilter->ObjAglListsNear;
//	OBJ_ANGLE_R *pObjAglListsLong = &pFilter->ObjAglListsLong;
//	ANGLE_R *parHistoryListNear = (ANGLE_R*)pObjAglListsNear->parHistoryList;
//	ANGLE_R *parHistoryListLong = (ANGLE_R*)pObjAglListsLong->parHistoryList;
//	SINT32  nEndNear, nEndLong;
//	UINT32  unFrmIdPreNear, unFrmIdPreLong;
//	UINT32  unFrmId, unFrmStepNear, unFrmStepLong;
//	ANGLE32F fAglPreNear, fAglPreLong;
//	ANGLE32F fAglSpeedNear, fAglSpeedLong;
//	ANGLE32F fAglPredictNear, fAglPredictLong;
//	FLOAT32 fRPreNear, fRSpeedNear, fRPredictNear;
//	FLOAT32 fRPreLong, fRSpeedLong, fRPredictLong;
//	CENTERRECT crnSrRectNear, crnSrRectLong;
//	SPEED32F fSpeedNear, fSpeedLong;
//	SIZE32S snSrRadiusNear, snSrRadiusLong;
//
//	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
//	//初始化
//	unFrmId = g_GLB_stInput->unFrmId;
//	pObjAglListsNear->arfPredict.unFrmID = unFrmId;
//	pObjAglListsLong->arfPredict.unFrmID = unFrmId;
//
//	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
//	//目标短时间轨迹预测
//	//计算当前帧与上一次轨迹记录的间隔帧数
//	nEndNear = pObjAglListsNear->nEnd;
//	unFrmIdPreNear = parHistoryListNear[nEndNear].unFrmID;
//	if (unFrmId > unFrmIdPreNear)
//	{
//		unFrmStepNear = unFrmId - unFrmIdPreNear;
//	}
//	else
//	{
//		unFrmStepNear = unFrmIdPreNear - unFrmId;
//	}
//
//	//获取上一次轨迹记录的目标角度、角速度、距离、距离变化速度
//	fAglPreNear = parHistoryListNear[nEndNear].afAngle;
//	fAglSpeedNear = pObjAglListsNear->arfSpeed.afAngle;
//	fRPreNear = parHistoryListNear[nEndNear].fDist;
//	fRSpeedNear = pObjAglListsNear->arfSpeed.fDist;
//
//	//估算当前帧预测的目标角度、距离
//	fAglPredictNear.fAz = DEGLIM360(fAglPreNear.fAz + fAglSpeedNear.fAz * unFrmStepNear);
//	fAglPredictNear.fPt = DEGLIM(fAglPreNear.fPt + fAglSpeedNear.fPt * unFrmStepNear);
//	fRPredictNear = fRPreNear + fRSpeedNear * unFrmStepNear;
//	pObjAglListsNear->arfPredict.afAngle = fAglPredictNear;
//	pObjAglListsNear->arfPredict.fDist = fRPredictNear;
//
//	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
//	//估算当前帧搜索区域矩形
//	//搜索区域中心
//	// 
//	//搜索区域半径
//	fSpeedNear.vx = fAglSpeedNear.fAz / g_SERVO_stInput.fResolAz;
//	fSpeedNear.vy = fAglSpeedNear.fPt / g_SERVO_stInput.fResolPt;
//// 	snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), g_DST_stPara.nPipeRadiusTrack);
//// 	snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), g_DST_stPara.nPipeRadiusTrack);
//	snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), g_PIPE_stPara.nPipeRadiusTrack);
//	snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), g_PIPE_stPara.nPipeRadiusTrack);
//
//	crnSrRectNear.w = 2 * snSrRadiusNear.w + 1;
//	crnSrRectNear.h = 2 * snSrRadiusNear.h + 1;
//	crnSrRectNear.s = crnSrRectNear.w * crnSrRectNear.h;
//	g_GLB_stOutput.crnObjPrediRtNear = crnSrRectNear;
//	IMGO_CenRect16S2MMRect32S(crnSrRectNear, &g_GLB_stOutput.mrnObjPrediRtNear,nWidth, nHeight);
//
//	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
//	//记录目标长时间轨迹（每秒记录一次）
//	//在长时间轨迹更新第2个位置之前，一直跟短时间轨迹预测位置保持一致
//	//if (pObjAglListsLong->nCnt <= 1 || g_GLB_stOutput.bObjTrackRefoundSucc)
//	if (pObjAglListsLong->nCnt <= 2)
//	{
//		fAglSpeedLong = fAglSpeedNear;
//		fAglPredictLong = pObjAglListsNear->arfPredict.afAngle;
//		pObjAglListsLong->arfPredict.afAngle = fAglPredictLong;
//	}
//	else
//	{
//		//计算当前帧与上一次轨迹记录的间隔帧数
//		nEndLong = pObjAglListsLong->nEnd;
//		unFrmIdPreLong = parHistoryListLong[nEndLong].unFrmID;
//		if (unFrmId > unFrmIdPreLong)
//		{
//			unFrmStepLong = unFrmId - unFrmIdPreLong;
//		}
//		else
//		{
//			unFrmStepLong = unFrmIdPreLong - unFrmId;
//		}
//
//		//获取上一次轨迹记录的目标角度、角速度、距离、距离变化速度
//		fAglPreLong = parHistoryListLong[nEndLong].afAngle;
//		fAglSpeedLong = pObjAglListsLong->arfSpeed.afAngle;
//		fRPreLong = parHistoryListLong[nEndLong].fDist;
//		fRSpeedLong = pObjAglListsLong->arfSpeed.fDist;
//
//		//估算当前帧预测的目标角度、距离
//		fAglPredictLong.fAz = SERVO_AddAzimuthScale360(fAglPreLong.fAz, fAglSpeedLong.fAz * unFrmStepLong);
//		fAglPredictLong.fPt = SERVO_AddPitchingScale360(fAglPreLong.fPt, fAglSpeedLong.fPt * unFrmStepLong);
//		fRPredictLong = fRPreLong + fRSpeedLong * unFrmStepLong;
//		pObjAglListsLong->arfPredict.afAngle = fAglPredictLong;
//		pObjAglListsLong->arfPredict.fDist = fRPredictLong;
//	}
//
//	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
//	//估算当前帧搜索区域矩形
//	//搜索区域中心
//	crnSrRectLong.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictLong.fAz, 
//		nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching));
//	crnSrRectLong.cy = (SINT16)(SERVO_CalcObjCoordinateY(fAglPredictLong.fPt, 
//		nHeight, g_SERVO_stInput.fPitching, g_SERVO_stInput.fResolPt));
//
//	//搜索区域半径
//	fSpeedLong.vx = fAglSpeedLong.fAz / g_SERVO_stInput.fResolAz;
//	fSpeedLong.vy = fAglSpeedLong.fPt / g_SERVO_stInput.fResolPt;
//	//snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), g_DST_stPara.nPipeRadiusTrack);
//	//snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), g_DST_stPara.nPipeRadiusTrack);
//	snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), g_PIPE_stPara.nPipeRadiusTrack);
//	snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), g_PIPE_stPara.nPipeRadiusTrack);
//
//	crnSrRectLong.w = 2 * snSrRadiusLong.w + 1;
//	crnSrRectLong.h = 2 * snSrRadiusLong.h + 1;
//	crnSrRectLong.s = crnSrRectLong.w * crnSrRectLong.h;
//	g_GLB_stOutput.crnObjPrediRtLong = crnSrRectLong;
//	IMGO_CenRect16S2MMRect32S(crnSrRectLong, &g_GLB_stOutput.mrnObjPrediRtLong,nWidth, nHeight);
//
//	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
//	//判断长短轨迹预测位置是否一致
//	DIST32S dnNLDist, dnNLDistPre;
//	dnNLDistPre = g_GLB_stOutput.dnObjPredictDist;
//
//	// 20180224 取绝对值 长短时预测异常时，修正搜索区域使用，防止搜索区域变小 
//	dnNLDist.dx = ABS(SERVO_CalcAzimuthXOffset(fAglPredictNear.fAz, fAglPredictLong.fAz, g_SERVO_stInput.fResolAz ));
//	dnNLDist.dy = ABS(SERVO_CalcPitchingYOffset(fAglPredictNear.fPt, fAglPredictLong.fPt, g_SERVO_stInput.fResolPt));
//
//	SINT32 fNLDist2 = dnNLDist.dx * dnNLDist.dx + dnNLDist.dy * dnNLDist.dy;
//	SINT32 fNLDist2Pre = dnNLDistPre.dx * dnNLDistPre.dx + dnNLDistPre.dy * dnNLDistPre.dy;
//	//SINT32 nNLDist2Thres = g_DST_stPara.nPipeRadiusTrack * g_DST_stPara.nPipeRadiusTrack;
//	SINT32 nNLDist2Thres = g_PIPE_stPara.nPipeRadiusTrack * g_PIPE_stPara.nPipeRadiusTrack;
//	if (fNLDist2 - fNLDist2Pre > 2 || fNLDist2 > nNLDist2Thres)
//	{
//		g_GLB_stOutput.nObjPredictFarCnt++;
//	}
//	else
//	{
//		g_GLB_stOutput.nObjPredictFarCnt = 0;
//		g_GLB_stOutput.bObjPredictAbnormal = false;
//	}
//	if (g_GLB_stOutput.nObjPredictFarCnt > 10 && fNLDist2 > nNLDist2Thres)
//	{
//		g_GLB_stOutput.bObjPredictAbnormal = true;
//	}
//	g_GLB_stOutput.dnObjPredictDist = dnNLDist;
//
//	////利用长短时轨迹判断是否开启惯性预测           by wcw04046 @ 2022/01/17
//	//if (g_GLB_stOutput.nObjPredictFarCnt == 0)
//	//{
//	//	g_GLB_stPara.bEnableServoTrackPredi = true;
//	//}
//	//else
//	//{
//	//	g_GLB_stPara.bEnableServoTrackPredi = false;
//	//}
//}
//

///**********************************************************
//* 函数名称：TO_ObjAglTrackPredictCurrent()
//* 功能描述：目标轨迹预测：利用长短时目标方位、俯仰航迹
//* 输入参数：SINT32 nWidth   --  图像宽度
//*            SINT32 nHeight  --  图像高度
//* 输出参数：无
//* 返 回 值：无
//* 调用关系：无
//* 其它说明：
//*   MSSu, 20171204: 将轨迹预测TO_ObjAglTrackPredict()拆分成两个部分执行：
//*   1）TO_ObjAglTrackPredictNext()“下一帧长短时角度预测” ：
//*       在当前帧TrackObject()结束时执行，
//*       因DSP中核A面目标检测用的搜索区域，是核B在前一帧算法结束后传递过来的。
//*   2）TO_ObjAglTrackPredictCurrent()“当前帧长短时预测的搜索区域”：
//*       在当前帧TrackObject()跟踪算法执行前执行，利用当前帧伺服角度计算图像预测区域。
//**********************************************************/
//void Predict_ObjAglResolvePredictInfo(FilterMeanNL* pFilter,SINT32 nWidth, SINT32 nHeight)
//{
//	OBJ_ANGLE_R *pObjAglListsNear = &g_GLB_stOutput.ObjAglListsNear;
//	OBJ_ANGLE_R *pObjAglListsLong = &g_GLB_stOutput.ObjAglListsLong;
//	ANGLE32F fAglPredictNear, fAglPredictLong;
//	CENTERRECT crnSrRectNear, crnSrRectLong;
//	SPEED32F fSpeedNear, fSpeedLong;
//	SIZE32S snSrRadiusNear, snSrRadiusLong;
//
//	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
//	//估算当前帧短时间搜索区域矩形
//	//搜索区域中心
//	fAglPredictNear = pObjAglListsNear->arfPredict.afAngle;
//	crnSrRectNear.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictNear.fAz, 
//	    nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching));
///*	crnSrRectNear.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictNear.fAz, 
//		nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching))*/;
//	crnSrRectNear.cy = (SINT16)(SERVO_CalcObjCoordinateY(fAglPredictNear.fPt, 
//		nHeight, g_SERVO_stInput.fPitching, g_SERVO_stInput.fResolPt));
//
//	//搜索区域半径
//	fSpeedNear.vx = pObjAglListsNear->arfSpeed.afAngle.fAz / g_SERVO_stInput.fResolAz;
//	fSpeedNear.vy = pObjAglListsNear->arfSpeed.afAngle.fPt / g_SERVO_stInput.fResolPt;
//// 	snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), g_DST_stPara.nPipeRadiusTrack);
//// 	snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), g_DST_stPara.nPipeRadiusTrack); 
//	snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), g_PIPE_stPara.nPipeRadiusTrack);
//	snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), g_PIPE_stPara.nPipeRadiusTrack);
//	crnSrRectNear.w = 2 * snSrRadiusNear.w + 1;
//	crnSrRectNear.h = 2 * snSrRadiusNear.h + 1;
//	crnSrRectNear.s = crnSrRectNear.w * crnSrRectNear.h;
//	g_GLB_stOutput.crnObjPrediRtNear = crnSrRectNear;
//	IMGO_CenRect16S2MMRect32S(crnSrRectNear, &g_GLB_stOutput.mrnObjPrediRtNear,nWidth, nHeight);
//
//	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
//	//估算当前帧长时间搜索区域矩形
//	//20170702，wsa,搜索区域中心
//	fAglPredictLong = pObjAglListsLong->arfPredict.afAngle;
//	crnSrRectLong.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictLong.fAz, 
//	    nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching));
//	//crnSrRectLong.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictLong.fAz, 
//	//	nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching));
//	crnSrRectLong.cy = (SINT16)(SERVO_CalcObjCoordinateY(fAglPredictLong.fPt, 
//		nHeight, g_SERVO_stInput.fPitching, g_SERVO_stInput.fResolPt));
//
//	//搜索区域半径
//	fSpeedLong.vx = pObjAglListsLong->arfSpeed.afAngle.fAz / g_SERVO_stInput.fResolAz;
//	fSpeedLong.vy = pObjAglListsLong->arfSpeed.afAngle.fPt / g_SERVO_stInput.fResolPt;
//	//snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), g_DST_stPara.nPipeRadiusTrack);
//	//snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), g_DST_stPara.nPipeRadiusTrack);
//	snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), g_PIPE_stPara.nPipeRadiusTrack);
//	snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), g_PIPE_stPara.nPipeRadiusTrack);
//	crnSrRectLong.w = 2 * snSrRadiusLong.w + 1;
//	crnSrRectLong.h = 2 * snSrRadiusLong.h + 1;
//	crnSrRectLong.s = crnSrRectLong.w * crnSrRectLong.h;
//	g_GLB_stOutput.crnObjPrediRtLong = crnSrRectLong;
//	IMGO_CenRect16S2MMRect32S(crnSrRectLong, &g_GLB_stOutput.mrnObjPrediRtLong,nWidth, nHeight);
//}