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;

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 7
+#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/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时间戳
 

data_process_class_dll/track_asso.cpp

@@ -100,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]);
@@ -656,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;
@@ -688,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中关联上的航迹 点迹信息
@@ -726,31 +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 = 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;
-				}
 
 				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);
@@ -765,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);
@@ -778,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;     //连续未用实点更新时间
@@ -788,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_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

@@ -254,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中删除关联上的临时航迹
@@ -451,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> ());
@@ -565,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;
@@ -576,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;
@@ -612,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;