#include "track_asso.h"
#include "kalman.h"
#include "coor_trans.h"
#include "rdp_log.h"
#include <cmath>
#include <cstring>
#include <vector>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

// //////////////////////////// 用point_recv中点迹与trust_track中的点迹进行关联，                          /////////////
// //////////////////////////// 航迹更新结果通过Trust_Track_Output[MAX_TRACK_NUM][10]和 Trust_track_num_Output输出        /////////////

int Track_Asso:: track_asso_process(std::vector<PointRecv>               *point_recv,                           //输入点迹
									std::vector <Trust_Track>            *trust_track,                            //航迹文件
									struct  Track                    Trust_Track_Output[MAX_TRACK_NUM][10],   //更新航迹信息
									int                              *Trust_track_num_Output,                 //更新航迹数
									struct  RadarPara                Work_Parameter                           //雷达参数
									)
{

	//取出对应点迹区的点
	for (size_t i=0;i< point_recv->size();i++)
	{
		point_process.push_back((*point_recv)[i]);
		int n=point_process.size();
		point_process[n-1].point_section_asso = 1;
	}

	//IMM算法
	model_interaction(trust_track);

	model_filter(trust_track, Work_Parameter);

	model_output(trust_track);

	//删除关联上的点
	std::vector <PointRecv>::iterator Iter;
	for (Iter= point_process.begin(); Iter!=point_process.end();)
	{
		if((*Iter).Use_Flag==1 )
		{
			point_process.erase(Iter);
			Iter=point_process.begin();
		}
		else
		{
			Iter++;
		}
	}

	//剩余点重新存入点迹
	std::vector<PointRecv>().swap((*point_recv));
	for (size_t i=0;i<point_process.size();i++)
	{

	   (*point_recv).push_back(point_process[i]);

	}

	//point_process清空
	std::vector<PointRecv>().swap(point_process);

	//输出航迹
	for (size_t i=0;i<trust_track->size();i++)
	{                                                                                                                                                   // 输出更新航迹条件：
//        if((*trust_track)[i].manual_tracking_flag==0)
//        {

			*Trust_track_num_Output= *Trust_track_num_Output+1;

			Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
			double x,y;
			x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
			y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
			coor_trans Coor_trans;
			double r, azi;
			Coor_trans.cart2polar(x, y, &r, &azi);
			Trust_Track_Output[*Trust_track_num_Output-1][0].Range=static_cast<float>(r);
			Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=static_cast<float>(azi/PI*180);

			if((*trust_track)[i].Height<r)
				Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>(asin((*trust_track)[i].Height/r)/PI*180);
			else
				Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*trust_track)[i].elev_point);

			Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index;

			Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V =
			static_cast<float>(sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]));
			Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = static_cast<float>((*trust_track)[i].Amplitude);

			Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
			Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=static_cast<float>((*trust_track)[i].T_track/1000.0);
			Trust_Track_Output[*Trust_track_num_Output-1][0].GNSS_time=(*trust_track)[i].GNSS_time;
			Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track)[i].point_flag;
			Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);

			double Direction_Angle;
			Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
			if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
			Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);

			//关联点信息
			Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=static_cast<float>((*trust_track)[i].range_point);
			Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=static_cast<float>((*trust_track)[i].azi_point);
			Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=static_cast<float>((*trust_track)[i].elev_point);
			Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=static_cast<float>((*trust_track)[i].vr_point);
			Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=0;
			Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = static_cast<float>((*trust_track)[i].prf_point);
			Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);

			//直接输出点迹高度-20260605
			//Trust_Track_Output[*Trust_track_num_Output-1][0].z = (*trust_track)[i].range_point * sin((*trust_track)[i].elev_point/180.0*PI);
			//输出平滑后的高度
			Trust_Track_Output[*Trust_track_num_Output-1][0].z = static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);

			Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*trust_track)[i].elev_point);

			Trust_Track_Output[*Trust_track_num_Output-1][0].x=static_cast<float>((*trust_track)[i].X[0]);
			Trust_Track_Output[*Trust_track_num_Output-1][0].y=static_cast<float>((*trust_track)[i].X[3]);
			Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=static_cast<float>((*trust_track)[i].X[1]);
			Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=static_cast<float>((*trust_track)[i].X[4]);

			Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = static_cast<float>((*trust_track)[i].RCS);
			Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;

			std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim, (*trust_track)[i].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim));
			std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim, (*trust_track)[i].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim));
//        }
	}

	return 0;
}

//遍历所有航迹  进行多模型交互
void Track_Asso:: model_interaction(std::vector <Trust_Track>   *trust_track)
{

	for (size_t loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
	{

//        if((*trust_track)[loop_of_track].manual_tracking_flag == 0)
//        {
			double u_last[3];
			u_last[0]=(*trust_track)[loop_of_track].u[0];
			u_last[1]=(*trust_track)[loop_of_track].u[1];
			u_last[2]=(*trust_track)[loop_of_track].u[2];

			double u_t[3][3];
			double c[3];
			c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
			c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
			c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];
			if (c[0] <= 1e-12) c[0] = 1e-12;
			if (c[1] <= 1e-12) c[1] = 1e-12;
			if (c[2] <= 1e-12) c[2] = 1e-12;

			u_t[0][0]=Pt[0][0]*u_last[0]/c[0];
			u_t[1][0]=Pt[1][0]*u_last[1]/c[0];
			u_t[2][0]=Pt[2][0]*u_last[2]/c[0];
			u_t[0][1]=Pt[0][1]*u_last[0]/c[1];
			u_t[1][1]=Pt[1][1]*u_last[1]/c[1];
			u_t[2][1]=Pt[2][1]*u_last[2]/c[1];
			u_t[0][2]=Pt[0][2]*u_last[0]/c[2];
			u_t[1][2]=Pt[1][2]*u_last[1]/c[2];
			u_t[2][2]=Pt[2][2]*u_last[2]/c[2];

			double Xo1_last[6],Xo2_last[6],Xo3_last[6],X1[6],X2[6],X3[6];
			for (int i=0;i<6;i++)
			{
				X1[i]=(*trust_track)[loop_of_track].X1[i];
				X2[i]=(*trust_track)[loop_of_track].X2[i];
				X3[i]=(*trust_track)[loop_of_track].X3[i];
			}

			for (int i=0;i<6;i++)
			{
				Xo1_last[i]=u_t[0][0]*X1[i]+u_t[1][0]*X2[i]+u_t[2][0]*X3[i];
				Xo2_last[i]=u_t[0][1]*X1[i]+u_t[1][1]*X2[i]+u_t[2][1]*X3[i];
				Xo3_last[i]=u_t[0][2]*X1[i]+u_t[1][2]*X2[i]+u_t[2][2]*X3[i];
			}

			double X1_sub_Xo1[6],X2_sub_Xo1[6],X3_sub_Xo1[6];
			double X1_sub_Xo2[6],X2_sub_Xo2[6],X3_sub_Xo2[6];
			double X1_sub_Xo3[6],X2_sub_Xo3[6],X3_sub_Xo3[6];
			double X11[6][6],X21[6][6],X31[6][6];
			double X12[6][6],X22[6][6],X32[6][6];
			double X13[6][6],X23[6][6],X33[6][6];

			for (int i=0;i<6;i++)
			{

				X1_sub_Xo1[i]=X1[i]-Xo1_last[i];
				X2_sub_Xo1[i]=X2[i]-Xo1_last[i];
				X3_sub_Xo1[i]=X3[i]-Xo1_last[i];
				X1_sub_Xo2[i]=X1[i]-Xo2_last[i];
				X2_sub_Xo2[i]=X2[i]-Xo2_last[i];
				X3_sub_Xo2[i]=X3[i]-Xo2_last[i];
				X1_sub_Xo3[i]=X1[i]-Xo3_last[i];
				X2_sub_Xo3[i]=X2[i]-Xo3_last[i];
				X3_sub_Xo3[i]=X3[i]-Xo3_last[i];

			}

			for (int i=0;i<6;i++)
				for(int j=0;j<6;j++)
				{
					X11[i][j]=X1_sub_Xo1[i]*X1_sub_Xo1[j];
					X21[i][j]=X2_sub_Xo1[i]*X2_sub_Xo1[j];
					X31[i][j]=X3_sub_Xo1[i]*X3_sub_Xo1[j];
					X12[i][j]=X1_sub_Xo2[i]*X1_sub_Xo2[j];
					X22[i][j]=X2_sub_Xo2[i]*X2_sub_Xo2[j];
					X32[i][j]=X3_sub_Xo2[i]*X3_sub_Xo2[j];
					X13[i][j]=X1_sub_Xo3[i]*X1_sub_Xo3[j];
					X23[i][j]=X2_sub_Xo3[i]*X2_sub_Xo3[j];
					X33[i][j]=X3_sub_Xo3[i]*X3_sub_Xo3[j];
				}

			double Po1_last[6][6],Po2_last[6][6],Po3_last[6][6];
			for (int i=0;i<6;i++)
				for(int j=0;j<6;j++)
				{
					Po1_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X11[i][j])*u_t[0][0]+((*trust_track)[loop_of_track].P2[i][j]+X21[i][j])*u_t[1][0]+((*trust_track)[loop_of_track].P3[i][j]+X31[i][j])*u_t[2][0];
					Po2_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X12[i][j])*u_t[0][1]+((*trust_track)[loop_of_track].P2[i][j]+X22[i][j])*u_t[1][1]+((*trust_track)[loop_of_track].P3[i][j]+X32[i][j])*u_t[2][1];
					Po3_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X13[i][j])*u_t[0][2]+((*trust_track)[loop_of_track].P2[i][j]+X23[i][j])*u_t[1][2]+((*trust_track)[loop_of_track].P3[i][j]+X33[i][j])*u_t[2][2];
				}

			for(int i=0;i<6;i++)
			{
				(*trust_track)[loop_of_track].X1[i]=Xo1_last[i];
				(*trust_track)[loop_of_track].X2[i]=Xo2_last[i];
				(*trust_track)[loop_of_track].X3[i]=Xo3_last[i];
			}
			for(int i=0;i<6;i++)
				for (int j=0;j<6;j++)
				{
					(*trust_track)[loop_of_track].P1[i][j]=Po1_last[i][j];
					(*trust_track)[loop_of_track].P2[i][j]=Po2_last[i][j];
					(*trust_track)[loop_of_track].P3[i][j]=Po3_last[i][j];
				}
//        }
	}
}

// 产生 F Q 矩阵
void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct  RadarPara   Work_Parameter)
{

	//F
	double alpha=0.2;
	memset(F,0,36*sizeof(double));
	F[0][0]=1;
	F[0][1]=delta_T;
	F[0][2]=(alpha*delta_T-1+exp(-alpha*delta_T))/pow(alpha,2);
	F[1][0]=0;
	F[1][1]=1;
	F[1][2]=(1-exp(-alpha*delta_T))/alpha;
	F[2][0]=0;
	F[2][1]=0;
	F[2][2]=exp(-alpha*delta_T);

	F[3][3]=1;
	F[3][4]=delta_T;
	F[3][5]=(alpha*delta_T-1+exp(-alpha*delta_T))/pow(alpha,2);
	F[4][3]=0;
	F[4][4]=1;
	F[4][5]=(1-exp(-alpha*delta_T))/alpha;
	F[5][3]=0;
	F[5][4]=0;
	F[5][5]=exp(-alpha*delta_T);

	//模型1
	//Q
	double q1=Work_Parameter.Model1_Q_slow;
	double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
	double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
	double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
	double q221=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
	double q231=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
	double q331=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
	memset(Q1,0,36*sizeof(double));
	Q1[0][0]=q1*q111; Q1[0][1]=q1*q121; Q1[0][2]=q1*q131;
	Q1[1][0]=q1*q121; Q1[1][1]=q1*q221; Q1[1][2]=q1*q231;
	Q1[2][0]=q1*q131; Q1[2][1]=q1*q231; Q1[2][2]=q1*q331;

	Q1[3][3]=q1*q111; Q1[3][4]=q1*q121; Q1[3][5]=q1*q131;
	Q1[4][3]=q1*q121; Q1[4][4]=q1*q221; Q1[4][5]=q1*q231;
	Q1[5][3]=q1*q131; Q1[5][4]=q1*q231; Q1[5][5]=q1*q331;

	//模型2
	//Q
	double q2;
	if(v_track<=5)
	{
		q2=Work_Parameter.Model2_Q_slow/20;
	}
	else if(v_track<=15&&v_track>5)
	{
		q2=Work_Parameter.Model2_Q_slow;
	}
	else if(v_track<=30&&v_track>15)
	{
		q2=Work_Parameter.Model2_Q_slow;
	}
	else if(v_track<=100&&v_track>30)
	{
		q2=Work_Parameter.Model2_Q_fast;

	}
	else
	{
		q2=Work_Parameter.Model2_Q_fast;
	}

	double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
	double q122=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
	double q132=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
	double q222=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
	double q232=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
	double q332=1/(2*alpha)*(1-exp(-2*alpha*delta_T));

	memset(Q2,0,36*sizeof(double));
	Q2[0][0]=q2*q112;Q2[0][1]=q2*q122; Q2[0][2]=q2*q132;
	Q2[1][0]=q2*q122;Q2[1][1]=q2*q222; Q2[1][2]=q2*q232;
	Q2[2][0]=q2*q132; Q2[2][1]=q2*q232; Q2[2][2]=q2*q332;

	Q2[3][3]=q2*q112; Q2[3][4]=q2*q122; Q2[3][5]=q2*q132;
	Q2[4][3]=q2*q122; Q2[4][4]=q2*q222; Q2[4][5]=q2*q232;
	Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332;

	//模型3
	//Q
	double q3;
	if(v_track<=5)
	{
		q3=Work_Parameter.Model3_Q_slow/20;
	}
	else if(v_track<=15&&v_track>5)
	{
		q3=Work_Parameter.Model3_Q_slow;
	}
	else if(v_track<=20&&v_track>15)
	{
		q3=Work_Parameter.Model3_Q_slow;
	}
	else if(v_track<=50&&v_track>20)
	{
		q3=Work_Parameter.Model3_Q_fast;

	}
	else if(v_track<=100&&v_track>50)
	{
		q3=2*Work_Parameter.Model3_Q_fast;

	}
	else
	{
		q3=10*Work_Parameter.Model3_Q_fast;
	}

	double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
	double q123=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
	double q133=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
	double q223=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
	double q233=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
	double q333=1/(2*alpha)*(1-exp(-2*alpha*delta_T));

	memset(Q3,0,36*sizeof(double));
	Q3[0][0]=q3*q113; Q3[0][1]=q3*q123; Q3[0][2]=q3*q133;
	Q3[1][0]=q3*q123; Q3[1][1]=q3*q223; Q3[1][2]=q3*q233;
	Q3[2][0]=q3*q133; Q3[2][1]=q3*q233; Q3[2][2]=q3*q333;

	Q3[3][3]=q3*q113; Q3[3][4]=q3*q123; Q3[3][5]=q3*q133;
	Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233;
	Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333;

}

// 计算三个 d
void Track_Asso::IMM_d_cal(double v_track,
						   double X1[6], double P1[6][6],
						   double X2[6], double P2[6][6],
						   double X3[6], double P3[6][6],
						   double Z[3], double prt,double freq_ind,
						   double delta_T,
						   double *d1,double *d2,double *d3,
							struct  RadarPara   Work_Parameter)
{
	double F[6][6],  Q1[6][6],  Q2[6][6],  Q3[6][6];
	IMM_F_Q_gen( v_track, delta_T,F, Q1,  Q2,  Q3,Work_Parameter);

	kalman Kalman;
	*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
	*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
	*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);

}

void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  RadarPara Work_Parameter)
{

	//存储关联信息
	struct asso_info
	{
		double d1;
		double d2;
		double d3;
		double d_min;
		int point_index;
		int track_index;
	};
	std::vector <asso_info> associated_info;

	//遍历所有点迹航迹  计算点迹和航迹的统计距离
	for (size_t loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
	{
//        if((*trust_track)[loop_of_track].manual_tracking_flag==0)
//        {
			(*trust_track)[loop_of_track].point_flag = 0;  //航迹的point_flag置为0 关联上点后再置为1

			for (size_t loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
			{

				//时间差
				double delta_T = (point_process[loop_of_point].CPI_Time - (*trust_track)[loop_of_track].T_track)/1000.0;
				//航迹X P
				double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
				memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
				memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
				memcpy(X3,(*trust_track)[loop_of_track].X3,6*sizeof(double));
				memcpy(P1,(*trust_track)[loop_of_track].P1,6*6*sizeof(double));
				memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
				memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
				double v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
				double h_track = (*trust_track)[loop_of_track].Height;
				double r_track =  sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));

				//量测信息
				double Z[3] = {point_process[loop_of_point].Range,
							  point_process[loop_of_point].Azimuth,
							  point_process[loop_of_point].Velocity};
				double prt = point_process[loop_of_point].PRF_index;
				double freq_ind =  point_process[loop_of_point].Freq_index;
				double h_point = point_process[loop_of_point].Height;

				//计算统计距离d
				double d1,d2,d3;
				IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);

				//计算高度门限
//      double d_h;
//                if (r_track<1000)
//                {
//                    if(abs(h_track-h_point)<=150)
//                    {
//                       d_h = 1;
//                    }
//                    else
//                    {
//                        d_h = 0;
//                    }

//                }
//                else if (r_track<2000 && r_track>=1000)
//                {
//                    if(abs(h_track-h_point)<=200)
//                    {
//                       d_h = 1;
//                    }
//                    else
//                    {
//                        d_h = 0;
//                    }

//                }
//                else if(r_track>=2000 && r_track<3000 )
//                {
//                    if(abs(h_track-h_point)<=200)
//                    {
//                       d_h = 1;
//                    }
//                    else
//                    {
//                        d_h = 0;
//                    }
//                }
//                else if(r_track>=3000 && r_track<5000)
//                {
//                    if(abs(h_track-h_point)<=300)
//                    {
//                       d_h = 1;
//                    }
//                    else
//                    {
//                        d_h = 0;
//                    }
//                }
//                else
//                {
//                   d_h = 1;
//                }

//                std::cout << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
//                std::cout << h_track<<" "<<h_point<<" "<<d_h;
//                d_h = 1;

		//计算俯仰门限
		bool d_p = true;

//		double p_track = atan2(h_track, r_track);           //航迹俯仰
//		double p_point = asin(h_point/point_process[loop_of_point].Range);          //点迹俯仰

//		if(	(r_track > 3000 && abs(p_track - p_point)*180/PI <= 7) || \
//		    (r_track > 1000 && r_track <= 3000 && abs(p_track - p_point)*180/PI <= 5) || \
//		    (r_track <= 1000 && abs(p_track - p_point)*180/PI <= 10)){
//			d_p = true;
//		}

				//计算距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差
				double& t_x0 = (*trust_track)[loop_of_track].X[0];
				double& t_y0 = (*trust_track)[loop_of_track].X[3];
				double a_track = atan2(t_y0, t_x0);                             //航迹方位角
				double& r_point = point_process[loop_of_point].Range;           //点迹距离
				double a_delta = abs(a_track - point_process[loop_of_point].Azimuth);       //方位差
				if(a_delta > PI){
					a_delta = 2*PI - a_delta;
				}

				double R = sqrt(r_track * r_track + r_point*r_point - 2*r_track*r_point*cos(a_delta));
				double VT = v_track*delta_T;

				bool d_r = true;

				if(VT < 100 && R > 300){
					//VT<100m，使用固定门限300m
					d_r = false;
				}else if((VT>=100 && VT<=300 && R/VT>3)){
					d_r = false;
				}else if(VT>300 && R > 1200){
					d_r = false;
				}

				//小于关联门限  保存关联信息
				if(
			(  r_track<=500               && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000.0) && d_p && d_r)//
			||( (r_track>500&&r_track<1000) && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/100.0) && d_p && d_r)//
			||(  r_track>=1000              && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD) && d_p && d_r)//
				)
				{
					 //std::cout << "asooooooooooo";

					struct asso_info associated_info_tmp;
					associated_info_tmp.d1=d1;
					associated_info_tmp.d2=d2;
					associated_info_tmp.d3=d3;
					associated_info_tmp.point_index=static_cast<int>(loop_of_point)+1;
					associated_info_tmp.track_index=static_cast<int>(loop_of_track)+1;
					if(d1<=d2&&d1<=d3)
						associated_info_tmp.d_min=d1;
					else if(d2<=d1&& d2<=d3)
						associated_info_tmp.d_min=d2;
					else
						associated_info_tmp.d_min=d3;

					associated_info.push_back(associated_info_tmp);
				}
			}
//        }
	}

	//最近邻法关联
	int associated_num=associated_info.size();
	for (size_t i=0;i<trust_track->size();i++)
	{
		//寻找最小d
		if(associated_num>0)
		{
			int min_index=1;
			double min_d=associated_info[0].d_min;
			for (int ii=0;ii<associated_num;ii++)
			{
				if(associated_info[ii].d_min<min_d)
				{
					min_d=associated_info[ii].d_min;
					min_index=ii+1;
				}
			}
			int track_index=associated_info[min_index-1].track_index;
			int point_index=associated_info[min_index-1].point_index;

			double d1=associated_info[min_index-1].d1;
			double d2=associated_info[min_index-1].d2;
			double d3=associated_info[min_index-1].d3;

			//卡尔曼滤波
			double Z[3]={point_process[point_index-1].Range,point_process[point_index-1].Azimuth,point_process[point_index-1].Velocity};
			double prt = point_process[point_index-1].PRF_index;
			double freq_ind =  point_process[point_index-1].Freq_index;
			double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
			memcpy(X1,(*trust_track)[track_index-1].X1,6*sizeof(double));
			memcpy(X2,(*trust_track)[track_index-1].X2,6*sizeof(double));
			memcpy(X3,(*trust_track)[track_index-1].X3,6*sizeof(double));
			memcpy(P1,(*trust_track)[track_index-1].P1,6*6*sizeof(double));
			memcpy(P2,(*trust_track)[track_index-1].P2,6*6*sizeof(double));
			memcpy(P3,(*trust_track)[track_index-1].P3,6*6*sizeof(double));
			double v_track = sqrt(pow((*trust_track)[track_index-1].X[1],2)+pow((*trust_track)[track_index-1].X[4],2));
			double delta_T = (point_process[point_index-1].CPI_Time - (*trust_track)[track_index-1].T_track)/1000.0;
			double F[6][6],  Q1[6][6],  Q2[6][6],  Q3[6][6];
			IMM_F_Q_gen( v_track, delta_T,F, Q1,  Q2,  Q3,Work_Parameter);

			if (delta_T <= 0)
			{

				RDP_LOG<<"delta_T <= 0, track_index: "<<track_index<<", point_index: "<<point_index<<", delta_T: "<<delta_T<<std::endl;
				// 乱序/重复时间戳：不作为有效关联，不标记点迹已使用、不刷新航迹新鲜度
				for (int ii=0;ii<associated_num;)
				{
					if(associated_info[ii].track_index==track_index)
					{
						associated_info.erase(associated_info.begin()+ii);
						associated_num=associated_info.size();
					}
					else
						ii++;
				}
				associated_num=associated_info.size();
				for (int ii=0;ii<associated_num;)
				{
					if(associated_info[ii].point_index==point_index)
					{
						associated_info.erase(associated_info.begin()+ii);
						associated_num=associated_info.size();
					}
					else
						ii++;
				}
				associated_num=associated_info.size();
				continue;
			}

			if(delta_T>0)
			{
				double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
				double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
				kalman Kalman;
				Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind );
				Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind );
				Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind );

				Matrix3f S1_out,S2_out,S3_out;
				for (int ii=0;ii<3;ii++)
					for (int jj=0;jj<3;jj++)
					{
						S1_out(ii,jj)=static_cast<float>(S1[ii][jj]);
						S2_out(ii,jj)=static_cast<float>(S2[ii][jj]);
						S3_out(ii,jj)=static_cast<float>(S3[ii][jj]);
					}
				double det_S1=S1_out.determinant();
				double Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0;
				double det_S2=S2_out.determinant();
				double Possibility2=(det_S2>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S2)*exp(-0.5*d2)):0.0;
				double det_S3=S3_out.determinant();
				double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0;

				//更新模型概率
				double u_last[3];
				u_last[0]=(*trust_track)[track_index-1].u[0];
				u_last[1]=(*trust_track)[track_index-1].u[1];
				u_last[2]=(*trust_track)[track_index-1].u[2];

				double c[3];
				c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
				c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
				c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];

				double u_den = Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2];
				if (u_den > 1e-12)
				{
					(*trust_track)[track_index-1].u[0]=Possibility1*c[0]/u_den;
					(*trust_track)[track_index-1].u[1]=Possibility2*c[1]/u_den;
					(*trust_track)[track_index-1].u[2]=Possibility3*c[2]/u_den;
				}
				// 若分母异常，保持上一拍模型概率

				//更新 X1 X2 X3 P1 P2 P3
				for(int ii=0;ii<6;ii++)
				{
					(*trust_track)[track_index-1].X1[ii]=X1_filter[ii];
					(*trust_track)[track_index-1].X2[ii]=X2_filter[ii];
					(*trust_track)[track_index-1].X3[ii]=X3_filter[ii];
				}
				for(int ii=0;ii<6;ii++)
					for (int jj=0;jj<6;jj++)
					{
						(*trust_track)[track_index-1].P1[ii][jj]=P1_filter[ii][jj];
						(*trust_track)[track_index-1].P2[ii][jj]=P2_filter[ii][jj];
						(*trust_track)[track_index-1].P3[ii][jj]=P3_filter[ii][jj];
					}

				//更新航迹时间
				(*trust_track)[track_index-1].T_track = point_process[point_index-1].CPI_Time;
				(*trust_track)[track_index-1].GNSS_time = point_process[point_index-1].GNSS_time;

				//更新关联上的点迹信息
				(*trust_track)[track_index-1].range_point=point_process[point_index-1].Range;
				(*trust_track)[track_index-1].azi_point=point_process[point_index-1].Azimuth/PI*180;
				(*trust_track)[track_index-1].elev_point= asin(point_process[point_index-1].Height/point_process[point_index-1].Range)/PI*180;
				(*trust_track)[track_index-1].vr_point=point_process[point_index-1].Velocity;
				(*trust_track)[track_index-1].point_type = 0;
				(*trust_track)[track_index-1].prf_point = point_process[point_index-1].PRF_index;
				(*trust_track)[track_index-1].snr_point = point_process[point_index-1].snr;
				(*trust_track)[track_index-1].Amplitude= point_process[point_index-1].Amplitude;    //幅度

				(*trust_track)[track_index-1].RCS = point_process[point_index-1].RCS;

				(*trust_track)[track_index-1].pitch_num = point_process[point_index-1].pitch_num;

				std::memcpy((*trust_track)[track_index-1].speed_dim, point_process[point_index-1].speed_dim, sizeof((*trust_track)[track_index-1].speed_dim));
				std::memcpy((*trust_track)[track_index-1].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[track_index-1].range_dim));

				//更新高度
				track_hight_update(track_index,point_index,trust_track);

			}
			(*trust_track)[track_index-1].Extrapolate_round = 0;        //连续未用实点更新时间
			(*trust_track)[track_index-1].point_flag=1;                 //实点
			(*trust_track)[track_index-1].associate_point_number = (*trust_track)[track_index-1].associate_point_number+1; //关联点数+1

			//删除associated_info中关联上的航迹 点迹信息
			for (int ii=0;ii<associated_num;)
			{
				if(associated_info[ii].track_index==track_index)
				{
					associated_info.erase(associated_info.begin()+ii);
					associated_num=associated_info.size();
				}
				else
					ii++;
			}
			associated_num=associated_info.size();
			for (int ii=0;ii<associated_num;)
			{
				if(associated_info[ii].point_index==point_index)
				{
					associated_info.erase(associated_info.begin()+ii);
					associated_num=associated_info.size();
				}
				else
					ii++;
			}
			associated_num=associated_info.size();

			//在接收点迹中 将关联上的点迹的使用标记位置为1 等待删除
			point_process[point_index-1].Use_Flag = 1;
		}
	}

	//未关联上的航迹 进行外推 tws
	for (size_t i =0; i<(*trust_track).size();i++ )
	{
		if((*trust_track)[i].point_flag == 0 && (*trust_track)[i].manual_tracking_flag == 0)
		{
			double v_track = sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
			double delta_T;
			if(Work_Parameter.work_mode == 0)
			{
			   delta_T = Work_Parameter.DATA_RATE_SHORT;
			}
			else if(Work_Parameter.work_mode == 1)
			{
				delta_T = Work_Parameter.DATA_RATE_MIDDLE;
			}
			else if(Work_Parameter.work_mode == 2)
			{
				delta_T = Work_Parameter.DATA_RATE_FAR;
			}
			else
			{
				delta_T = Work_Parameter.DATA_RATE_SHORT;
			}

			double F[6][6],  Q1[6][6],  Q2[6][6],  Q3[6][6];
			IMM_F_Q_gen( v_track, delta_T, F, Q1,  Q2,  Q3,Work_Parameter);

			//航迹X P
			double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
			memcpy(X1,(*trust_track)[i].X1,6*sizeof(double));
			memcpy(X2,(*trust_track)[i].X2,6*sizeof(double));
			memcpy(X3,(*trust_track)[i].X3,6*sizeof(double));
			memcpy(P1,(*trust_track)[i].P1,6*6*sizeof(double));
			memcpy(P2,(*trust_track)[i].P2,6*6*sizeof(double));
			memcpy(P3,(*trust_track)[i].P3,6*6*sizeof(double));

			//外推

			double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
			kalman Kalman;
			Kalman.kalman_pred(F, Q1, X1, P1, X1_pred, P1_pred);
			Kalman.kalman_pred(F, Q2, X2, P2, X2_pred, P2_pred);
			Kalman.kalman_pred(F, Q3, X3, P3, X3_pred, P3_pred);
			memcpy((*trust_track)[i].X1,X1_pred,6*sizeof(double));
			memcpy((*trust_track)[i].X2,X2_pred,6*sizeof(double));
			memcpy((*trust_track)[i].X3,X3_pred,6*sizeof(double));
			memcpy((*trust_track)[i].P1,P1_pred,6*6*sizeof(double));
			memcpy((*trust_track)[i].P2,P2_pred,6*6*sizeof(double));
			memcpy((*trust_track)[i].P3,P3_pred,6*6*sizeof(double));

			(*trust_track)[i].T_track = static_cast<long long>((*trust_track)[i].T_track+delta_T*1000.0);            //更新航迹时间
			(*trust_track)[i].GNSS_time = static_cast<long long>((*trust_track)[i].GNSS_time+delta_T*1000.0);            //更新航迹时间
			(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1;     //连续未用实点更新时间
			(*trust_track)[i].point_flag=0;                                                 //虚点
		}
	}

}

void Track_Asso:: model_output(std::vector <Trust_Track>    *trust_track)
{
	for(size_t loop_of_track=0; loop_of_track<trust_track->size(); loop_of_track++)
	{

//        if((*trust_track)[loop_of_track].manual_tracking_flag==0)
//        {
			double u_now[3];
			u_now[0]=(*trust_track)[loop_of_track].u[0];
			u_now[1]=(*trust_track)[loop_of_track].u[1];
			u_now[2]=(*trust_track)[loop_of_track].u[2];

			double X1_filter[6],X2_filter[6],X3_filter[6];
			double P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
			for (int ii=0;ii<6;ii++)
			{
					X1_filter[ii]=(*trust_track)[loop_of_track].X1[ii];
					X2_filter[ii]=(*trust_track)[loop_of_track].X2[ii];
					X3_filter[ii]=(*trust_track)[loop_of_track].X3[ii];
			}
			for (int ii=0;ii<6;ii++)
				for(int jj=0;jj<6;jj++)
				{
						P1_filter[ii][jj]=(*trust_track)[loop_of_track].P1[ii][jj];
						P2_filter[ii][jj]=(*trust_track)[loop_of_track].P2[ii][jj];
						P3_filter[ii][jj]=(*trust_track)[loop_of_track].P3[ii][jj];
				}

			double X_filter[6], P_filter[6][6];
			for (int i=0;i<6;i++)
				X_filter[i]=u_now[0]*X1_filter[i]+u_now[1]*X2_filter[i]+u_now[2]*X3_filter[i];

			double X1_sub_X[6], X2_sub_X[6], X3_sub_X[6];
			double X1X[6][6], X2X[6][6],X3X[6][6];
			for (int i=0;i<6;i++)
			{
				X1_sub_X[i]=X1_filter[i]-X_filter[i];
				X2_sub_X[i]=X2_filter[i]-X_filter[i];
				X3_sub_X[i]=X3_filter[i]-X_filter[i];
			}
			for(int i=0;i<6;i++)
				for (int j=0;j<6;j++)
				{
					X1X[i][j]=X1_sub_X[i]*X1_sub_X[j];
					X2X[i][j]=X2_sub_X[i]*X2_sub_X[j];
					X3X[i][j]=X3_sub_X[i]*X3_sub_X[j];
				}
			for(int i=0;i<6;i++)
				for (int j=0;j<6;j++)
					P_filter[i][j]=u_now[0]*(P1_filter[i][j]+X1X[i][j])+u_now[1]*(P2_filter[i][j]+X2X[i][j])+u_now[2]*(P3_filter[i][j]+X3X[i][j]);

			//本地航迹文件更新
			(*trust_track)[loop_of_track].X[0]=X_filter[0];  //位置 速度
			(*trust_track)[loop_of_track].X[1]=X_filter[1];
			(*trust_track)[loop_of_track].X[2]=X_filter[2];
			(*trust_track)[loop_of_track].X[3]=X_filter[3];
			(*trust_track)[loop_of_track].X[4]=X_filter[4];
			(*trust_track)[loop_of_track].X[5]=X_filter[5];
			for(int ii=0;ii<6;ii++)                         //协方差矩阵
				for (int jj=0;jj<6;jj++)
					(*trust_track)[loop_of_track].P[ii][jj]=P_filter[ii][jj];

	//        //航迹区更新
	//        double r, azi;
	//        coor_trans Coor_trans;
	//        Coor_trans.cart2polar((*trust_track)[loop_of_track].X[0], (*trust_track)[loop_of_track].X[3], &r, &azi);

	//        if(azi/PI*180>=TRACK_SECTION_1_START && azi/PI*180<TRACK_SECTION_2_START)
	//           (*trust_track)[loop_of_track].Track_section_idx = 1;

	//        if(azi/PI*180>=TRACK_SECTION_2_START && azi/PI*180<TRACK_SECTION_3_START)
	//           (*trust_track)[loop_of_track].Track_section_idx = 2;

	//        if(azi/PI*180>=TRACK_SECTION_3_START && azi/PI*180<TRACK_SECTION_4_START)
	//           (*trust_track)[loop_of_track].Track_section_idx = 3;

	//        if(azi/PI*180>=TRACK_SECTION_4_START && azi/PI*180<TRACK_SECTION_5_START)
	//           (*trust_track)[loop_of_track].Track_section_idx = 4;

	//        if(azi/PI*180>=TRACK_SECTION_5_START && azi/PI*180<TRACK_SECTION_6_START)
	//           (*trust_track)[loop_of_track].Track_section_idx = 5;

	//        if(azi/PI*180>=TRACK_SECTION_6_START && azi/PI*180<TRACK_SECTION_7_START)
	//           (*trust_track)[loop_of_track].Track_section_idx = 6;

	//        if(azi/PI*180>=TRACK_SECTION_7_START && azi/PI*180<TRACK_SECTION_8_START)
	//           (*trust_track)[loop_of_track].Track_section_idx = 7;

	//        if(azi/PI*180>=TRACK_SECTION_8_START && azi/PI*180<TRACK_SECTION_9_START)
	//           (*trust_track)[loop_of_track].Track_section_idx = 8;

	//        if(azi/PI*180>=TRACK_SECTION_9_START || azi/PI*180<TRACK_SECTION_1_START)
	//            (*trust_track)[loop_of_track].Track_section_idx = 9;

		}
//    }

}

//高度维更新
void Track_Asso:: track_hight_update(int                                 updata_track_index,                     //更新的航迹号
									 int                                 asso_point_index,                       //点迹号
									 std::vector <Trust_Track>               *trust_track                            //航迹
									 )
{

	if(updata_track_index<=static_cast<int>(trust_track->size()) &&  asso_point_index<=static_cast<int>(point_process.size()))
	{

		(*trust_track)[updata_track_index-1].Hight_smooth.push_back(point_process[asso_point_index-1].Height);

		int height_win_length;

		if((*trust_track)[updata_track_index-1].range_point <= 1000)
		{
			height_win_length = H_F_WIN_LEN+2;
		}
		else if((*trust_track)[updata_track_index-1].range_point <= 2000 && (*trust_track)[updata_track_index-1].range_point > 1000)
		{
			height_win_length = H_F_WIN_LEN+3;
		}
		else if((*trust_track)[updata_track_index-1].range_point <= 4000 && (*trust_track)[updata_track_index-1].range_point > 2000)
		{
			height_win_length = H_F_WIN_LEN+4;
		}
		else
		{
			height_win_length = H_F_WIN_LEN+6;
		}

		double sum=0;
		if((*trust_track)[updata_track_index-1].Hight_smooth.size()<static_cast<size_t>(height_win_length))
		{

			for (size_t i=0;i<(*trust_track)[updata_track_index-1].Hight_smooth.size();i++)
			{
				sum=sum+(*trust_track)[updata_track_index-1].Hight_smooth[i];

			}

			(*trust_track)[updata_track_index-1].Height=sum/(*trust_track)[updata_track_index-1].Hight_smooth.size();

		}
		else if((*trust_track)[updata_track_index-1].Hight_smooth.size()>=static_cast<size_t>(height_win_length))
		{

			int N=(*trust_track)[updata_track_index-1].Hight_smooth.size();
			for (int i=0;i<height_win_length;i++)
			{
				sum=sum+(*trust_track)[updata_track_index-1].Hight_smooth[N-1-i];
			}
			(*trust_track)[updata_track_index-1].Height=sum/height_win_length;
		}
	}
};

Track_Asso::Track_Asso()
{
	Pt[0][0]=0.8;Pt[0][1]=0.15;Pt[0][2]=0.05;       //IMM 转移概率
	Pt[1][0]=0.3;Pt[1][1]=0.4;Pt[1][2]=0.3;
	Pt[2][0]=0.05;Pt[2][1]=0.15;Pt[2][2]=0.8;

};

void Track_Asso::reset()
{
	point_process.clear();
}