Merge branch 'feature'

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>

data_process_class_dll/data_process.cpp

@@ -62,7 +62,23 @@ int Data_Process::data_preprocess(struct DataRev    Data_Input[150])
 			Data_buffer.push_back(Data_buffer_temp);
 		}
 
-		// 修改一周扫描完成的判断条件，原来采用判断波位号变小为一周扫描完成，但机扫模式下，可能存在转台反转的情况，导致波位号变小，但并未扫描一周，因此改为判断波位号为0时，表示扫描一周完成
+#ifdef PHASE_SCANNING
+		// 相扫模式下，判断前后两帧波位号的大小，来判断是否扫描一周完成
+		if(Data_Input[0].Beam_index_aiz <= last_beam_num)              //TWS
+		{
+			last_beam_num = Data_Input[0].Beam_index_aiz;
+			circle_id++;
+			// std::cout << Data_Input[0].CPI_time << std::endl;	
+			// RDP_LOG<<"CPI_time: "<<Data_Input[0].CPI_time<<std::endl;
+			return 1;
+		}
+		else
+		{
+			last_beam_num = Data_Input[0].Beam_index_aiz;
+			return 2;
+		}
+#else
+		// 机扫模式下，可能存在转台反转的情况，导致波位号变小，但并未扫描一周，因此改为判断波位号为0时，表示扫描一周完成
 		if(Data_Input[0].Beam_index_aiz == 0)              //TWS
 		{
 			last_beam_num = Data_Input[0].Beam_index_aiz;
@@ -76,8 +92,8 @@ int Data_Process::data_preprocess(struct DataRev    Data_Input[150])
 			last_beam_num = Data_Input[0].Beam_index_aiz;
 			return 2;
 		}
+#endif
 	}
-
 	else if(Data_Input[0].point_type == 1)          //tas数据
 	{
 		//        std::cout << "TAS TARGET :" <<Data_Input[0].TAS_track_index;
@@ -160,6 +176,7 @@ int         model)
 		                              &trust_track,
 		                              Trust_Track_Output,
 		                              Trust_track_num_Output,
+									  latest_timestamp,
 		                              Work_Parameter
 		                              );
 

data_process_class_dll/data_process_class_dll.h

@@ -51,6 +51,7 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Track
 	float   Elevation;                              //俯仰
 	float   Range_V;                                //速度
 	float   Amplitude;                              //幅度
+	float   Quality;                                //航迹质量
 	int     Track_Index;                            //航迹号
 	int     Point_Sum;                              //本条航迹更新点的数目
 	int     Flag_Direction;                         //方向标志1靠近/0远离

data_process_class_dll/parameters.h

@@ -10,7 +10,7 @@
 /**********************************版本信息*************************************************/
 #define MAJOR_VERSION 1
 #define MINOR_VERSION 5
-#define PATCH_VERSION 6
+#define PATCH_VERSION 8
 #ifdef PHASE_SCANNING
 #define VERSION_INFO "PS"
 #else
@@ -49,8 +49,30 @@
 //#define MECHANICAL_SCANNING
 //#define PHASE_SCANNING
 
+/******************************通用数据处理参数**************************************/
+//航迹质量管理参数
+#define TRACK_QUALITY_MAX 100
+#define TRACK_QUALITY_MIN 0
+#define TRACK_QUALITY_D_WEIGHT 0.5
+#define TRACK_QUALITY_SNR_WEIGHT 0.3
+#define TRACK_QUALITY_LEN_WEIGHT 0.2
+#define TRACK_QUALITY_ITER_COEFF 0.8
+#define TRACK_QUALITY_DECREASE_COEFF_O2 -0.0008
+#define TRACK_QUALITY_DECREASE_COEFF_O1 -0.0025
+
+//航迹起批质量条件
+#define TRACK_START_QUALITY_L3 0.7
+#define TRACK_START_QUALITY_L4 0.6
+#define TRACK_START_QUALITY_L5 0.5
+#define TRACK_START_QUALITY_L6 0.4
+#define TRACK_START_QUALITY_L7 0.3
+
+//航迹消批质量条件
+#define TRACK_DIE_QUALITY 0.3
+#define TRACK_DIE_QUALITY_PRAMARY 0.3
+
 #ifdef MECHANICAL_SCANNING
-/******************************数据处理参数**************************************/
+/******************************机扫数据处理参数**************************************/
 // #define DATA_RATE_TAS 0.0625
 
 #define SIGMA_R    10.0         //测量误差
@@ -101,7 +123,7 @@
 
 #ifdef PHASE_SCANNING
 
-/******************************数据处理参数**************************************/
+/******************************相扫数据处理参数**************************************/
 // #define DATA_RATE_TAS 0.3
 
 #define SIGMA_R    10.0         //测量误差

data_process_class_dll/rdp_record.cpp

@@ -260,7 +260,7 @@ RDP_RECORD_DEFINE(RdpRecTrustTrack);
 //关联信息
 YLT_REFL(RdpRecAssoInfo, id,point_id,track_id,circle_id,d1,d2,d3,d_min,point_index,track_index,
 	delta_T,delta_h,R,VT,
-	thd_d,thd_h,thd_r,
+	tas_flag,thd_d,thd_h,thd_r,
 	min_d_flag);
 RDP_RECORD_DEFINE(RdpRecAssoInfo);
 

data_process_class_dll/rdp_record_entity.h

@@ -407,6 +407,8 @@ struct RdpRecAssoInfo
 	double R;
 	double VT;
 
+	bool tas_flag;  //TAS关联标志 1是 0否
+
 	bool thd_d;
 	bool thd_h;
 	bool thd_r;
@@ -432,6 +434,7 @@ struct RdpRecAssoInfo
 		delta_h = p.delta_h;
 		R = p.R;
 		VT = p.VT;
+		tas_flag = p.tas_flag;
 		thd_d = p.thd_d;
 		thd_h = p.thd_h;
 		thd_r = p.thd_r;

data_process_class_dll/struct.h

@@ -46,17 +46,23 @@ struct  Trust_Track
 	int     Track_Index;                        //航迹号
 
 	double  Range;						  //距离
-	double Azimuth;                        //方位
+	double  Azimuth;                        //方位
-	double Elevation;                      //俯仰
+	double  Elevation;                      //俯仰
 	double  Velocity;                          //航向速度
 	double  Amplitude;                          //幅度
 	double  Height;                             //高度
+	double  RCS;								//RCS
+	double  Quality;                            //航迹质量
 
-	long long  T_track;                         //航迹时间
+	long long	T_track;                        //航迹时间（CPI时间）
 	long long	GNSS_time;						//GNSS时间
+	long long 	Unupdated_time;					//航迹未更新时间
 
 	int     Track_Sum;                          //航迹总数
 	int     Point_Index;                        //该航迹上的第几个点
+	int		Point_sum;							//本条航迹更新点的数目
+	int		Valid_point_sum;					//本条航迹有效点的数目
+	int     associate_point_number;         	//关联的点数
 
 	int     Track_Mode;                         //跟踪模式
 	int     Track_Update_Flag;                  //航迹更新标志 1已更新/0未更新
@@ -67,8 +73,6 @@ struct  Trust_Track
 
 	int     manual_delete_flag;                 //手动航迹删除
 	int     manual_tracking_flag;             	//手动TAS
-	int     associate_point_number;         	//关联的点数
-
 	int		primary_target_flag;				//重点目标标记
 
 	int     Target_Type;                            //目标类型  搜索目标   低空目标  地面目标
@@ -87,8 +91,6 @@ struct  Trust_Track
 	float	amp_asv_point;							//关联上的点迹信息（归一化幅度）
 	float	prn_fftn_point;							//点迹速度维FFT点数
 
-	double   RCS;
-
 	int     pitch_num;              // 俯仰波位号
 	float   speed_dim[129];         // 目标的十字星数据,目标点速度维129个点
 	float   range_dim[8];           // 目标的十字星数据,目标点距离维8个点
@@ -122,6 +124,7 @@ struct Temp_track
 	double SNR;          //信噪比
 	double RCS;          //RCS
 	double amplitude;         	//幅度
+	double quality;      //航迹质量
 	long long   CPI_time;       //时间戳
 	long long	GNSS_time;		//GNSS时间戳
 
@@ -157,6 +160,8 @@ struct Asso_Info
 	double R;
 	double VT;
 
+	bool tas_flag;  //TAS关联标志 1是 0否
+
 	bool thd_d;
 	bool thd_h;
 	bool thd_r;

data_process_class_dll/track_asso.cpp

@@ -20,6 +20,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv>               *point_
 									std::vector <Trust_Track>            *trust_track,                            //航迹文件
 									struct  Track                    Trust_Track_Output[MAX_TRACK_NUM][10],   //更新航迹信息
 									int                              *Trust_track_num_Output,                 //更新航迹数
+									long long 								latest_timestamp,                        //最新时间戳
 									struct  RadarPara                Work_Parameter                           //雷达参数
 									)
 {
@@ -35,7 +36,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv>               *point_
 	//IMM算法
 	model_interaction(trust_track);
 
-	model_filter(trust_track, Work_Parameter);
+	model_filter(trust_track, Work_Parameter, latest_timestamp);
 
 	model_output(trust_track);
 
@@ -99,6 +100,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv>               *point_
 			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);
+			Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast<float>((*trust_track)[i].Quality * TRACK_QUALITY_MAX);
 
 			double Direction_Angle;
 			Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
@@ -429,7 +431,7 @@ void Track_Asso::IMM_d_cal(double v_track,
 
 }
 
-void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  RadarPara Work_Parameter)
+void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  RadarPara Work_Parameter, long long latest_timestamp)
 {
 
 	
@@ -538,6 +540,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  Ra
 			associated_info_tmp.delta_h=delta_h;
 			associated_info_tmp.R=R;
 			associated_info_tmp.VT=VT;
+			associated_info_tmp.tas_flag=point_process[loop_of_point].point_type;
 			associated_info_tmp.thd_d=thd_d;
 			associated_info_tmp.thd_h=thd_h;
 			associated_info_tmp.thd_r=thd_r;
@@ -654,9 +657,18 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  Ra
 						(*trust_track)[track_index-1].P3[ii][jj]=P3_filter[ii][jj];
 					}
 
+				//计算航迹质量
+				double d_tmp = exp(-associated_info[min_index-1].d_min);
+				double snr_tmp = 1/(1+exp(-(point_process[point_index-1].SNR - 16) / 1.2));
+				double len_tmp = 1/(1+exp(-((*trust_track)[track_index-1].Valid_point_sum - 4) / 1.2)) * 0.5 + (1 - (*trust_track)[track_index-1].Valid_point_sum / (*trust_track)[track_index-1].Point_sum) * 0.5;
+				double tq_tmp = TRACK_QUALITY_D_WEIGHT * d_tmp + TRACK_QUALITY_SNR_WEIGHT * snr_tmp + TRACK_QUALITY_LEN_WEIGHT * len_tmp;
+				
+				(*trust_track)[track_index-1].Quality = (*trust_track)[track_index-1].Quality * TRACK_QUALITY_ITER_COEFF + tq_tmp * (1 - TRACK_QUALITY_ITER_COEFF);
+
 				//更新航迹时间
 				(*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].Unupdated_time = 0;
 
 				//更新关联上的点迹信息
 				(*trust_track)[track_index-1].point_id = point_process[point_index-1].point_id;
@@ -686,6 +698,8 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  Ra
 				(*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
 				(*trust_track)[track_index-1].Track_Update_Flag = 1; 		//航迹更新标志
+				(*trust_track)[track_index-1].Point_sum = (*trust_track)[track_index-1].Point_sum + 1;
+				(*trust_track)[track_index-1].Valid_point_sum = (*trust_track)[track_index-1].Valid_point_sum + 1;
 			}
 			
 			//删除associated_info中关联上的航迹 点迹信息
@@ -724,27 +738,32 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  Ra
 	{
 		if((*trust_track)[i].point_flag == 0 && (*trust_track)[i].manual_tracking_flag == 0)  //未关联上点迹的航迹进行外推
 		{
+			double delta_T = 0;
+			// double delta_T = (latest_timestamp - (*trust_track)[i].T_track)/1000.0;
+			// if (delta_T <= 0)
+			// 	continue;
+			
+			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;
+			}
+
 			//外推点数未到上限，则继续外推，否则不外推
 			if((*trust_track)[i].Extrapolate_round <= MAX_EXTRAPOLATE_NUM)
 			{
 				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);
@@ -759,7 +778,6 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  Ra
 				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);
@@ -772,6 +790,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  Ra
 				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].circle_id++;
 				(*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;     //连续未用实点更新时间
@@ -782,9 +801,18 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  Ra
 				(*trust_track)[i].point_flag=0;                            						//虚点
 				(*trust_track)[i].Track_Update_Flag = 0;										//航迹更新标志
 			}
+
+			//航迹质量递减
+			double t = (*trust_track)[i].Unupdated_time / 1000.0;
+			(*trust_track)[i].Quality -= t * t * TRACK_QUALITY_DECREASE_COEFF_O2 + t * TRACK_QUALITY_DECREASE_COEFF_O1;
+			(*trust_track)[i].Unupdated_time += static_cast<long long>(delta_T*1000.0);
+			t = (*trust_track)[i].Unupdated_time / 1000.0;
+			(*trust_track)[i].Quality += t * t * TRACK_QUALITY_DECREASE_COEFF_O2 + t * TRACK_QUALITY_DECREASE_COEFF_O1;
+			(*trust_track)[i].Quality = (*trust_track)[i].Quality < 0.0 ? 0.0 : (*trust_track)[i].Quality;
+			(*trust_track)[i].Point_sum = (*trust_track)[i].Point_sum + 1;
+			
 		}
 	}
-
 }
 
 void Track_Asso:: model_output(std::vector <Trust_Track>    *trust_track)

data_process_class_dll/track_asso.h

@@ -16,6 +16,7 @@ public:
 							   std::vector <Trust_Track>            *trust_track,                            //航迹文件
 							   struct  Track                    Trust_Track_Output[MAX_TRACK_NUM][10],   //更新航迹信息
 							   int                              *Trust_track_num_Output,                 //更新航迹数
+							   long long 								latest_timestamp,                        //最新时间戳
 							   struct  RadarPara                Work_Parameter                             //工作参数
 							   );
 
@@ -30,7 +31,8 @@ private:
 	void model_interaction(std::vector <Trust_Track>    *trust_track);               //模型交互
 
 	void model_filter(std::vector <Trust_Track>          *trust_track,           //滤波
-					  struct  RadarPara              Work_Parameter
+					  struct  RadarPara              Work_Parameter,
+					  long long							latest_timestamp
 					  );
 
 	void model_output(std::vector <Trust_Track>         *trust_track );             //模型输出

data_process_class_dll/track_asso_tas.cpp

@@ -6,6 +6,10 @@
 #include <cstring>
 #include <vector>
 #include <Eigen/Dense>
+
+#include "rdp_record.h"
+#include "rdp_record_entity.h"
+
 using namespace Eigen;
 using namespace std;
 
@@ -109,6 +113,13 @@ long long								latest_timestamp		//最新时间戳
 
 			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));
+
+			(*trust_track)[i].Range = Trust_Track_Output[*Trust_track_num_Output-1][0].Range;
+			(*trust_track)[i].Azimuth = Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth;
+			(*trust_track)[i].Elevation = Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation;
+			(*trust_track)[i].Velocity = Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V;
+
+			RDP_RECORD(RdpRecTrustTrack((*trust_track)[i]));
 		}
 
 	}
@@ -240,16 +251,7 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
                                     long long									latest_timestamp		//最新时间戳
                                     )
 {
-	//存储关联信息
+	std::vector <Asso_Info> associated_info;
-	struct asso_info
-	{
-		double d1;
-		double d2;
-		double d3;
-		double d_min;
-		int point_index;
-	};
-	std::vector <asso_info> associated_info;
 
 	//航迹信息
 	double X1[6] = {0},X2[6] = {0},X3[6] = {0},P1[6][6] = {{0}},P2[6][6] = {{0}},P3[6][6] = {{0}};
@@ -327,24 +329,38 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 			thd_h = true;
 		}
 
+		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=tas_track_idx;
+		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_tmp.point_id=point_process[loop_of_point].point_id;
+		associated_info_tmp.track_id=(*trust_track)[tas_track_idx].track_id;
+		associated_info_tmp.circle_id=point_process[loop_of_point].circle_id;
+		associated_info_tmp.delta_T=delta_T;
+		associated_info_tmp.delta_h=delta_h;
+		associated_info_tmp.R=0;
+		associated_info_tmp.VT=0;
+		associated_info_tmp.tas_flag=point_process[loop_of_point].point_type;
+		associated_info_tmp.thd_d=thd_d;
+		associated_info_tmp.thd_h=thd_h;
+		associated_info_tmp.thd_r=true;
+		associated_info_tmp.min_d_flag=false;
+
+
 		//小于关联门限  保存关联信息
 		if(thd_d && thd_h)
 		{
-
-			//            std::cout << "TAS asoooooo111" ;
-			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;
-			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);
+		}else{
+			RDP_RECORD(RdpRecAssoInfo(associated_info_tmp));
 		}
 	}
 
@@ -370,6 +386,8 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 		double d2=associated_info[min_index-1].d2;
 		double d3=associated_info[min_index-1].d3;
 
+		associated_info[min_index-1].min_d_flag=true;
+
 		//卡尔曼滤波
 		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;
@@ -406,7 +424,7 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 		double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0;
 
 		//更新航迹
-		for(size_t i=0;i<trust_track->size();i++)
+		for(size_t i=0;i<trust_track->size();i++){
 
 			if((*trust_track)[i].Track_Index == tas_track_idx)
 			{
@@ -450,6 +468,8 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 				(*trust_track)[i].GNSS_time = point_process[point_index-1].GNSS_time;
 
 				//更新关联上的点迹信息
+				(*trust_track)[i].point_id=point_process[point_index-1].point_id;
+				(*trust_track)[i].circle_id=point_process[point_index-1].circle_id;
 				(*trust_track)[i].range_point=point_process[point_index-1].range;
 				(*trust_track)[i].azi_point=point_process[point_index-1].azimuth/PI*180;
 				(*trust_track)[i].elev_point= asin(point_process[point_index-1].height/point_process[point_index-1].range)/PI*180;
@@ -472,7 +492,12 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 				//更新高度
 				track_hight_update(tas_track_idx,point_index,trust_track);
 			}
+		}
 
+		for(auto& info : associated_info)
+		{
+			RDP_RECORD(RdpRecAssoInfo(info));
+		}
 	}
 	//未关联上，航迹外推
 	else

data_process_class_dll/track_die.cpp

@@ -16,16 +16,19 @@ void Track_Die::track_die_process( std::vector <Trust_Track> *trust_track,
 	{
 		bool die_flag = false;
 
+		//手动删除
 		if((*Iter).manual_delete_flag == 1)
 		{
 			die_flag = true;
 		}
 
-		if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 0 && (*Iter).Extrapolate_round > TRACK_DIE_ROUND)
+		//非重点航迹自动消亡
+		if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 0 && (*Iter).Quality < TRACK_DIE_QUALITY)
 		{
 			die_flag = true;
 		}
 
+		//重点航迹自动消亡
 		if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 1 && (*Iter).Extrapolate_round > TRACK_DIE_ROUND_PRIMARY)
 		{
 			die_flag = true;

data_process_class_dll/track_init.cpp

@@ -107,6 +107,15 @@ int Track_Init::track_init_process_logic(   std::vector <PointRecv>
 		std::vector<PointRecv>().swap(point_process);
 		std::vector<PointRecv>().swap((*point_recv));
 
+#ifdef RDP_RECORD_ENABLE
+		//记录临时航迹
+		for(auto& vec_t : *temp_track){
+			for(auto& t : vec_t){
+				RDP_RECORD(RdpRecTempTrack(t));
+			}
+		}
+#endif	
+
 		//临时航迹满足起始长度 转为可靠航迹
 		tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
 
@@ -115,14 +124,6 @@ int Track_Init::track_init_process_logic(   std::vector <PointRecv>
 	//消亡临时航迹
 	tmp_track_die(temp_track);
 
-#ifdef RDP_RECORD_ENABLE
-	//记录临时航迹
-	for(auto& vec_t : *temp_track){
-		for(auto& t : vec_t){
-			RDP_RECORD(RdpRecTempTrack(t));
-		}
-	}
-#endif	
 	return 0;
 
 }
@@ -253,6 +254,40 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
 		memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
 		memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
 		(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
+
+		//计算航迹质量
+		auto& temp_track_ref = (*temp_track)[(*temp_track).size()-1];
+		L = static_cast<int>(temp_track_ref.size());
+		
+		double snr_tmp = 0;
+		double d_tmp = 0;
+		double quality_tmp = 0;
+
+		d_tmp = exp(-temp_track_ref[L-1].d);
+
+		if(L == 3){
+			double average_snr = 0;
+			for(int i=0;i<L;i++)
+			{
+				average_snr += temp_track_ref[i].SNR;
+			}
+			average_snr /= L;
+			snr_tmp = 1/(1+exp(-(average_snr - 16) / 1.2));
+		}else{
+			snr_tmp = 1/(1+exp(-(temp_track_ref.back().SNR - 16) / 1.2));
+		}
+
+		quality_tmp = d_tmp * TRACK_QUALITY_D_WEIGHT + snr_tmp * TRACK_QUALITY_SNR_WEIGHT;
+		quality_tmp /= (TRACK_QUALITY_D_WEIGHT + TRACK_QUALITY_SNR_WEIGHT);
+
+		if(L == 3){
+			for(int i=0;i<L;i++)
+			{
+				temp_track_ref[i].quality = quality_tmp;
+			}
+		}else{
+			temp_track_ref[L-1].quality = temp_track_ref[L-2].quality * TRACK_QUALITY_ITER_COEFF + quality_tmp * (1 - TRACK_QUALITY_ITER_COEFF);
+		}
 	}
 
 	//temp_track中删除关联上的临时航迹
@@ -450,17 +485,81 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track>    *trust_tra
 		int L=static_cast<int>((*Iter).size());
 		double range=(*Iter)[L-1].r;
 		double azi=(*Iter)[L-1].azi/PI*180;
+		double quality = (*Iter)[L-1].quality;
 
-		//（已停用）1km以内，SNR小于20dB的临时航迹不转为可靠航迹
+		// //计算航迹初始质量
-		// bool snr_flag = true;
+		// double average_snr = 0;
-		// for(int p_idx = 0; p_idx < L; p_idx++){
+		// for(int i=0;i<L;i++)
-		// 	if((*Iter)[p_idx].r < 1000 && (*Iter)[p_idx].snr < 20){
+		// {
-		// 		snr_flag = false;
+		// 	average_snr += (*Iter)[i].SNR;
-		// 		break;
+		// }
-		// 	}
+		// average_snr /= L;
+		// double snr_tmp = 1/(1+exp(-(average_snr - 16) / 1.2));
+		// double &d = (*Iter)[L-1].d;
+		// double d_tmp = exp(-d);
+		// double quality_tmp = d_tmp * TRACK_QUALITY_D_WEIGHT + snr_tmp * TRACK_QUALITY_SNR_WEIGHT;
+		// quality_tmp /= (TRACK_QUALITY_D_WEIGHT + TRACK_QUALITY_SNR_WEIGHT);
+		// for(int i=0;i<L;i++)
+		// {
+		// 	(*Iter)[i].quality = quality_tmp;
 		// }
 
-		if(  (L <=10 && L>=Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0) //按长度查找TRUST_TRACK_POINT
+		//航迹起批条件判断
+		bool track_init_flag = true;
+
+		//航迹长度大于10点，不起批
+		if(L > 10){
+			track_init_flag = false;
+		}
+
+		//航迹长度小于起始点数，不起批
+		if(L < Work_Parameter.track_start_point_num){
+			track_init_flag = false;
+		}
+		
+		//航迹起批禁止区域判断
+		if(track_init_prohibit(range,azi,Work_Parameter)!=0)
+		{
+			track_init_flag = false;
+		}
+
+		//航迹质量起批条件判断
+		switch(L){
+			case 3:
+				if(quality < TRACK_START_QUALITY_L3){
+					track_init_flag = false;
+				}
+				break;
+			case 4:
+				if(quality < TRACK_START_QUALITY_L4){
+					track_init_flag = false;
+				}
+				break;
+			case 5:
+				if(quality < TRACK_START_QUALITY_L5){
+					track_init_flag = false;
+				}
+				break;
+			case 6:
+				if(quality < TRACK_START_QUALITY_L6){
+					track_init_flag = false;
+				}
+				break;
+			case 7:
+				if(quality < TRACK_START_QUALITY_L7){
+					track_init_flag = false;
+				}
+				break;
+			default:
+				if(quality < TRACK_START_QUALITY_L7){
+					track_init_flag = false;
+				}
+				break;
+		}
+
+		//TODO: 杂波密度判断
+
+		if(track_init_flag)
 		{
 			//加入Track_to_start中
 			Track_to_start.push_back(std::vector <Temp_track> ());
@@ -564,6 +663,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track>    *trust_tra
 				std::memcpy(trust_track_tmp.range_dim, Track_to_start[i][L-1].range_dim, sizeof(trust_track_tmp.range_dim));
 				trust_track_tmp.T_track = Track_to_start[i][L-1].CPI_time;
 				trust_track_tmp.GNSS_time = Track_to_start[i][L-1].GNSS_time;
+				trust_track_tmp.Unupdated_time = 0;
 				trust_track_tmp.point_flag=1;       //实点
 				trust_track_tmp.Track_Mode=0;       //跟踪模式 TWS 0
 				trust_track_tmp.Target_Type=UNCONF_TARGET;
@@ -575,6 +675,9 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track>    *trust_tra
 				trust_track_tmp.associate_point_number = 0;
 				trust_track_tmp.manual_tracking_flag = 0;
 				trust_track_tmp.point_type = 0;
+				trust_track_tmp.Quality = Track_to_start[i][L-1].quality;
+				trust_track_tmp.Point_sum = L;
+				trust_track_tmp.Valid_point_sum = L;
 				trust_track_tmp.u[0]=0.3333;
 				trust_track_tmp.u[1]=0.3333;
 				trust_track_tmp.u[2]=0.3333;
@@ -611,6 +714,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track>    *trust_tra
 					Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>(Track_to_start[i][j].amplitude);
 					Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast<float>(Track_to_start[i][j].SNR);
 					Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast<float>(Track_to_start[i][j].RCS);
+					Trust_Track_Output[*Trust_track_num_Output-1][j].Quality = static_cast<float>(Track_to_start[i][j].quality * TRACK_QUALITY_MAX);
 
 					Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;