waiwaylee/radar_data_process
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

更新：1、修改航迹质量下降策略，改为按点迹未更新时间计算；

Browse Source 
Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-10-20 09:55:17 +08:00
parent d9a7bd0ee1
commit 4b70b24515
5 changed files with 47 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/track_asso.cpp
+26 -23
View File
@@ -668,6 +668,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
//更新航迹时间
(*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;
@@ -737,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);
@@ -776,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);
@@ -792,17 +793,19 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
(*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].Unupdated_time += static_cast<long long>(delta_T*1000.0);
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间
(*trust_track)[i].point_flag=0; //虚点
(*trust_track)[i].Track_Update_Flag = 1; //航迹更新标志
}else{
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间
(*trust_track)[i].Unupdated_time += static_cast<long long>(delta_T*1000.0);
(*trust_track)[i].point_flag=0; //虚点
(*trust_track)[i].Track_Update_Flag = 0; //航迹更新标志
}
//航迹质量递减
(*trust_track)[i].Quality -= (*trust_track)[i].Extrapolate_round * TRACK_QUALITY_DECREASE_COEFF;
(*trust_track)[i].Quality -= (*trust_track)[i].Unupdated_time / 1000.0 * TRACK_QUALITY_DECREASE_COEFF;
(*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;
}