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 ); //模型输出