From 56b5449b9f956ed242dac4bf15eb32003f5bc4b8 Mon Sep 17 00:00:00 2001 From: waiwaylee Date: Fri, 9 Oct 2026 14:04:22 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=EF=BC=9A1=E3=80=81=E4=BF=AE?= =?UTF-8?q?=E6=94=B9TWS=E5=A4=96=E6=8E=A8=E6=97=B6=EF=BC=8C=E6=97=B6?= =?UTF-8?q?=E9=97=B4=E5=B7=AE=E8=AE=A1=E7=AE=97=E6=96=B9=E6=B3=95=EF=BC=8C?= =?UTF-8?q?=E4=B8=8D=E5=86=8D=E4=BD=BF=E7=94=A8=E5=9B=BA=E5=AE=9A=E6=95=B0?= =?UTF-8?q?=E6=8D=AE=E7=8E=87=E8=AE=A1=E7=AE=97=E6=97=B6=E9=97=B4=E5=B7=AE?= =?UTF-8?q?=EF=BC=8C=E8=80=8C=E6=98=AF=E4=BD=BF=E7=94=A8=E6=9C=80=E6=96=B0?= =?UTF-8?q?=E6=97=B6=E9=97=B4=E6=88=B3=E6=9D=A5=E8=AE=A1=E7=AE=97=EF=BC=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: waiwaylee --- data_process_class_dll/data_process.cpp | 1 + data_process_class_dll/track_asso.cpp | 45 ++++++++++++++----------- data_process_class_dll/track_asso.h | 4 ++- 3 files changed, 30 insertions(+), 20 deletions(-) diff --git a/data_process_class_dll/data_process.cpp b/data_process_class_dll/data_process.cpp index 2a51c79..79c7d79 100644 --- a/data_process_class_dll/data_process.cpp +++ b/data_process_class_dll/data_process.cpp @@ -160,6 +160,7 @@ int model) &trust_track, Trust_Track_Output, Trust_track_num_Output, + latest_timestamp, Work_Parameter ); diff --git a/data_process_class_dll/track_asso.cpp b/data_process_class_dll/track_asso.cpp index 5bf9c08..84efb65 100644 --- a/data_process_class_dll/track_asso.cpp +++ b/data_process_class_dll/track_asso.cpp @@ -20,6 +20,7 @@ int Track_Asso:: track_asso_process(std::vector *point_ std::vector *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 *point_ //IMM算法 model_interaction(trust_track); - model_filter(trust_track, Work_Parameter); + model_filter(trust_track, Work_Parameter, latest_timestamp); model_output(trust_track); @@ -429,7 +430,7 @@ void Track_Asso::IMM_d_cal(double v_track, } -void Track_Asso:: model_filter(std::vector *trust_track,struct RadarPara Work_Parameter) +void Track_Asso:: model_filter(std::vector *trust_track,struct RadarPara Work_Parameter, long long latest_timestamp) { @@ -729,23 +730,29 @@ void Track_Asso:: model_filter(std::vector *trust_track,struct Ra 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 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); diff --git a/data_process_class_dll/track_asso.h b/data_process_class_dll/track_asso.h index 7ef623d..8bb2483 100644 --- a/data_process_class_dll/track_asso.h +++ b/data_process_class_dll/track_asso.h @@ -16,6 +16,7 @@ public: std::vector *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); //模型交互 void model_filter(std::vector *trust_track, //滤波 - struct RadarPara Work_Parameter + struct RadarPara Work_Parameter, + long long latest_timestamp ); void model_output(std::vector *trust_track ); //模型输出