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

更新：1、修改TWS外推时，时间差计算方法，不再使用固定数据率计算时间差，而是使用最新时间戳来计算；

Browse Source 
Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-10-09 14:04:22 +08:00
parent 5cb67ccef8
commit 56b5449b9f
3 changed files with 30 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/data_process.cpp
+1
View File
@@ -160,6 +160,7 @@ int model)
&trust_track, &trust_track,
Trust_Track_Output, Trust_Track_Output,
Trust_track_num_Output, Trust_track_num_Output,
latest_timestamp,
Work_Parameter Work_Parameter
); );
data_process_class_dll/track_asso.cpp
+26 -19
View File
@@ -20,6 +20,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
std::vector <Trust_Track> *trust_track, //航迹文件 std::vector <Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数 int *Trust_track_num_Output, //更新航迹数
long long latest_timestamp, //最新时间戳
struct RadarPara Work_Parameter //雷达参数 struct RadarPara Work_Parameter //雷达参数
) )
{ {
@@ -35,7 +36,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
//IMM算法 //IMM算法
model_interaction(trust_track); model_interaction(trust_track);
model_filter(trust_track, Work_Parameter); model_filter(trust_track, Work_Parameter, latest_timestamp);
model_output(trust_track); 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> *trust_track,struct RadarPara Work_Parameter) void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct RadarPara Work_Parameter, long long latest_timestamp)
{ {
@@ -729,23 +730,29 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
if((*trust_track)[i].Extrapolate_round <= MAX_EXTRAPOLATE_NUM) 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 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) double delta_T = (latest_timestamp - (*trust_track)[i].T_track)/1000.0;
{ if (delta_T <= 0)
delta_T = Work_Parameter.DATA_RATE_SHORT; continue;
}
else if(Work_Parameter.work_mode == 1) //
{ //
delta_T = Work_Parameter.DATA_RATE_MIDDLE; // if(Work_Parameter.work_mode == 0)
} // {
else if(Work_Parameter.work_mode == 2) // delta_T = Work_Parameter.DATA_RATE_SHORT;
{ // }
delta_T = Work_Parameter.DATA_RATE_FAR; // else if(Work_Parameter.work_mode == 1)
} // {
else // delta_T = Work_Parameter.DATA_RATE_MIDDLE;
{ // }
delta_T = Work_Parameter.DATA_RATE_SHORT; // 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]; 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); IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,Work_Parameter);
data_process_class_dll/track_asso.h
+3 -1
View File
@@ -16,6 +16,7 @@ public:
std::vector <Trust_Track> *trust_track, //航迹文件 std::vector <Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数 int *Trust_track_num_Output, //更新航迹数
long long latest_timestamp, //最新时间戳
struct RadarPara Work_Parameter //工作参数 struct RadarPara Work_Parameter //工作参数
); );
@@ -30,7 +31,8 @@ private:
void model_interaction(std::vector <Trust_Track> *trust_track); //模型交互 void model_interaction(std::vector <Trust_Track> *trust_track); //模型交互
void model_filter(std::vector <Trust_Track> *trust_track, //滤波 void model_filter(std::vector <Trust_Track> *trust_track, //滤波
struct RadarPara Work_Parameter struct RadarPara Work_Parameter,
long long latest_timestamp
); );
void model_output(std::vector <Trust_Track> *trust_track ); //模型输出 void model_output(std::vector <Trust_Track> *trust_track ); //模型输出