From d9a7bd0ee14d2493031c1fb4aedcb505eb529069 Mon Sep 17 00:00:00 2001
From: waiwaylee <waiwaylee@foxmail.com>
Date: Mon, 19 Oct 2026 16:09:37 +0800
Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=EF=BC=9A1=E3=80=81=E4=BC=98?=
 =?UTF-8?q?=E5=8C=96=E8=B0=83=E6=95=B4=E4=B8=B4=E6=97=B6=E8=88=AA=E8=BF=B9?=
 =?UTF-8?q?=E8=B4=A8=E9=87=8F=E8=AF=84=E4=BC=B0=E9=80=BB=E8=BE=91=EF=BC=9B?=
 =?UTF-8?q?=202=E3=80=81=E8=88=AA=E8=BF=B9=E8=B5=B7=E5=A7=8B=E6=97=B6?=
 =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=88=AA=E8=BF=B9=E8=B4=A8=E9=87=8F=E9=97=A8?=
 =?UTF-8?q?=E9=99=90=EF=BC=9B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
---
 data_process_class_dll/parameters.h   |   6 +-
 data_process_class_dll/track_init.cpp | 102 +++++++++++++++++++++-----
 2 files changed, 89 insertions(+), 19 deletions(-)

diff --git a/data_process_class_dll/parameters.h b/data_process_class_dll/parameters.h
index 7251442..8f95bb1 100644
--- a/data_process_class_dll/parameters.h
+++ b/data_process_class_dll/parameters.h
@@ -60,7 +60,11 @@
 #define TRACK_QUALITY_DECREASE_COEFF 0.01
 
 //航迹起批质量条件
-#define TRACK_START_QUALITY 0.5
+#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
 
 #ifdef MECHANICAL_SCANNING
 /******************************机扫数据处理参数**************************************/
diff --git a/data_process_class_dll/track_init.cpp b/data_process_class_dll/track_init.cpp
index 59ca8d8..690dfe1 100644
--- a/data_process_class_dll/track_init.cpp
+++ b/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,23 +485,24 @@ 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;
 
-		//计算航迹初始质量
-		double average_snr = 0;
-		for(int i=0;i<L;i++)
-		{
-			average_snr += (*Iter)[i].SNR;
-		}
-		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;
-		}
+		// //计算航迹初始质量
+		// double average_snr = 0;
+		// for(int i=0;i<L;i++)
+		// {
+		// 	average_snr += (*Iter)[i].SNR;
+		// }
+		// 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;
+		// }
 
 		//航迹起批条件判断
 		bool track_init_flag = true;
@@ -488,8 +523,39 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track>    *trust_tra
 			track_init_flag = false;
 		}
 
-		//TODO: 航迹质量起批条件判断
-
+		//航迹质量起批条件判断
+		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: 杂波密度判断