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

更新：1、增加手动设置重点目标的功能；

Browse Source 
2、调整航迹关联时外推圈数限制，不过多外推；
3、调整航迹消批条件，重点航迹消批圈数提高；
4、更新文档；

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-09-23 09:35:13 +08:00
parent e0bf227be3
commit 50e0b5cb74
12 changed files with 247 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/track_asso.cpp
+60 -49
View File
@@ -67,7 +67,8 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
{ // 输出更新航迹条件：
// if((*trust_track)[i].manual_tracking_flag==0)
// {
if((*trust_track)[i].Track_Update_Flag==1){
(*trust_track)[i].Track_Update_Flag=0;
*Trust_track_num_Output= *Trust_track_num_Output+1;
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
@@ -129,7 +130,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
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;
@@ -651,6 +652,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
(*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
(*trust_track)[track_index-1].Track_Update_Flag = 1; //航迹更新标志
}
//删除associated_info中关联上的航迹 点迹信息
@@ -685,57 +687,66 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
//未关联上的航迹 进行外推 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)
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
//外推点数未到上限，则继续外推，否则不外推
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);
//航迹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; //虚点
(*trust_track)[i].Track_Update_Flag = 1; //航迹更新标志
}else{
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间
(*trust_track)[i].point_flag=0; //虚点
(*trust_track)[i].Track_Update_Flag = 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);
//航迹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; //虚点
}
}