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/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, //航迹文件
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<PointRecv> *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> *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)
{
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);