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

更新：1、修改参数配置方式，改为由配置文件输入；

Browse Source 
Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-10-27 10:36:14 +08:00
parent 0e325fb153
commit 423c200659
39 changed files with 19638 additions and 945 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/track_asso.cpp
+49 -66
View File
@@ -20,7 +20,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
long long latest_timestamp, //最新时间戳
struct RadarPara Work_Parameter //雷达参数
const RDP_parameter& rdp_parameter //工作参数
)
{
@@ -35,7 +35,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
//IMM算法
model_interaction(trust_track);
model_filter(trust_track, Work_Parameter, latest_timestamp);
model_filter(trust_track, rdp_parameter, latest_timestamp);
model_output(trust_track);
@@ -80,8 +80,8 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
double x,y;
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*rdp_parameter.radar_system_delay;
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*rdp_parameter.radar_system_delay;
coor_trans Coor_trans;
double r, azi;
Coor_trans.cart2polar(x, y, &r, &azi);
@@ -103,8 +103,8 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=static_cast<float>((*trust_track)[i].T_track/1000.0);
Trust_Track_Output[*Trust_track_num_Output-1][0].GNSS_time=(*trust_track)[i].GNSS_time;
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track)[i].point_flag;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast<float>((*trust_track)[i].Quality * TRACK_QUALITY_MAX);
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast<float>((*trust_track)[i].Quality * rdp_parameter.track_quality_max);
double Direction_Angle;
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
@@ -130,7 +130,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
//直接输出点迹高度-20260605
//Trust_Track_Output[*Trust_track_num_Output-1][0].z = (*trust_track)[i].range_point * sin((*trust_track)[i].elev_point/180.0*PI);
//输出平滑后的高度
Trust_Track_Output[*Trust_track_num_Output-1][0].z = static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
Trust_Track_Output[*Trust_track_num_Output-1][0].z = static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*trust_track)[i].elev_point);
@@ -288,7 +288,7 @@ void Track_Asso:: model_interaction(std::vector <Trust_Track> *trust_track)
}
// 产生 F Q 矩阵
void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter)
void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter)
{
//F
@@ -316,7 +316,7 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
//模型1
//Q
double q1=Work_Parameter.Model1_Q_slow;
double q1=rdp_parameter.model_1_Q_slow;
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
@@ -337,24 +337,24 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
double q2;
if(v_track<=5)
{
q2=Work_Parameter.Model2_Q_slow/20;
q2=rdp_parameter.model_2_Q_slow/20;
}
else if(v_track<=15&&v_track>5)
{
q2=Work_Parameter.Model2_Q_slow;
q2=rdp_parameter.model_2_Q_slow;
}
else if(v_track<=30&&v_track>15)
{
q2=Work_Parameter.Model2_Q_slow;
q2=rdp_parameter.model_2_Q_slow;
}
else if(v_track<=100&&v_track>30)
{
q2=Work_Parameter.Model2_Q_fast;
q2=rdp_parameter.model_2_Q_fast;
}
else
{
q2=Work_Parameter.Model2_Q_fast;
q2=rdp_parameter.model_2_Q_fast;
}
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
@@ -378,29 +378,29 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
double q3;
if(v_track<=5)
{
q3=Work_Parameter.Model3_Q_slow/20;
q3=rdp_parameter.model_3_Q_slow/20;
}
else if(v_track<=15&&v_track>5)
{
q3=Work_Parameter.Model3_Q_slow;
q3=rdp_parameter.model_3_Q_slow;
}
else if(v_track<=20&&v_track>15)
{
q3=Work_Parameter.Model3_Q_slow;
q3=rdp_parameter.model_3_Q_slow;
}
else if(v_track<=50&&v_track>20)
{
q3=Work_Parameter.Model3_Q_fast;
q3=rdp_parameter.model_3_Q_fast;
}
else if(v_track<=100&&v_track>50)
{
q3=2*Work_Parameter.Model3_Q_fast;
q3=2*rdp_parameter.model_3_Q_fast;
}
else
{
q3=10*Work_Parameter.Model3_Q_fast;
q3=10*rdp_parameter.model_3_Q_fast;
}
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
@@ -429,19 +429,19 @@ void Track_Asso::IMM_d_cal(double v_track,
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
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,rdp_parameter);
kalman Kalman;
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind, rdp_parameter);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind, rdp_parameter);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind, rdp_parameter);
}
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct RadarPara Work_Parameter, long long latest_timestamp)
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_parameter& rdp_parameter, long long latest_timestamp)
{
@@ -492,17 +492,17 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
//计算统计距离d
double d1,d2,d3;
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,rdp_parameter);
//判定统计距离门限，thd_d
bool thd_d = false;
if( (r_track <= 500 && \
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 1000.0)) || \
(d1*d1 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold / 1000.0)) || \
((r_track > 500 && r_track < 1000) && \
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 100.0)) || \
(d1*d1 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold / 100.0)) || \
(r_track >= 1000 && \
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD))\
(d1*d1 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold))\
){
thd_d = true;
}
@@ -623,7 +623,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
double v_track = sqrt(pow((*trust_track)[track_index-1].X[1],2)+pow((*trust_track)[track_index-1].X[4],2));
double delta_T = (point_process[point_index-1].CPI_time - (*trust_track)[track_index-1].T_track)/1000.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);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
if (delta_T > 0)
{
@@ -632,9 +632,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
kalman Kalman;
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind );
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind );
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind );
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind, rdp_parameter);
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind, rdp_parameter);
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind, rdp_parameter);
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
@@ -690,9 +690,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
double d_tmp = exp(-associated_info[min_index-1].d_min);
double snr_tmp = 1/(1+exp(-(point_process[point_index-1].SNR - 16) / 1.2));
double len_tmp = 1/(1+exp(-((*trust_track)[track_index-1].Valid_point_sum - 4) / 1.2)) * 0.5 + (1 - (*trust_track)[track_index-1].Valid_point_sum / (*trust_track)[track_index-1].Point_sum) * 0.5;
double tq_tmp = TRACK_QUALITY_D_WEIGHT * d_tmp + TRACK_QUALITY_SNR_WEIGHT * snr_tmp + TRACK_QUALITY_LEN_WEIGHT * len_tmp;
double tq_tmp = rdp_parameter.track_quality_d_weight * d_tmp + rdp_parameter.track_quality_snr_weight * snr_tmp + rdp_parameter.track_quality_len_weight * len_tmp;
(*trust_track)[track_index-1].Quality = (*trust_track)[track_index-1].Quality * TRACK_QUALITY_ITER_COEFF + tq_tmp * (1 - TRACK_QUALITY_ITER_COEFF);
(*trust_track)[track_index-1].Quality = (*trust_track)[track_index-1].Quality * rdp_parameter.track_quality_iter_coeff + tq_tmp * (1 - rdp_parameter.track_quality_iter_coeff);
//更新航迹时间
(*trust_track)[track_index-1].T_track = point_process[point_index-1].CPI_time;
@@ -721,7 +721,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
std::memcpy((*trust_track)[track_index-1].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[track_index-1].range_dim));
//更新高度
track_hight_update(track_index,point_index,trust_track);
track_hight_update(track_index,point_index,trust_track,rdp_parameter);
(*trust_track)[track_index-1].Extrapolate_round = 0; //连续未用实点更新时间
(*trust_track)[track_index-1].point_flag=1; //实点
@@ -780,34 +780,16 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
if((*trust_track)[i].point_flag == 0 && (*trust_track)[i].manual_tracking_flag == 0) //未关联上点迹的航迹进行外推
{
double delta_T = 0;
// 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;
}
delta_T = rdp_parameter.data_rate_tws;
//外推点数未到上限，则继续外推，否则不外推
if((*trust_track)[i].Extrapolate_round <= MAX_EXTRAPOLATE_NUM)
if((*trust_track)[i].Extrapolate_round <= rdp_parameter.extrapolate_max)
{
double v_track = sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
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,rdp_parameter);
//航迹X P
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
@@ -845,10 +827,10 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
//航迹质量递减
double t = (*trust_track)[i].Unupdated_time / 1000.0;
(*trust_track)[i].Quality -= t * t * TRACK_QUALITY_DECREASE_COEFF_O2 + t * TRACK_QUALITY_DECREASE_COEFF_O1;
(*trust_track)[i].Quality -= t * t * rdp_parameter.track_quality_decrease_coeff_o2 + t * rdp_parameter.track_quality_decrease_coeff_o1;
(*trust_track)[i].Unupdated_time += static_cast<long long>(delta_T*1000.0);
t = (*trust_track)[i].Unupdated_time / 1000.0;
(*trust_track)[i].Quality += t * t * TRACK_QUALITY_DECREASE_COEFF_O2 + t * TRACK_QUALITY_DECREASE_COEFF_O1;
(*trust_track)[i].Quality += t * t * rdp_parameter.track_quality_decrease_coeff_o2 + t * rdp_parameter.track_quality_decrease_coeff_o1;
(*trust_track)[i].Quality = (*trust_track)[i].Quality < 0.0 ? 0.0 : (*trust_track)[i].Quality;
(*trust_track)[i].Point_sum = (*trust_track)[i].Point_sum + 1;
@@ -965,7 +947,8 @@ void Track_Asso:: model_output(std::vector <Trust_Track> *trust_track)
//高度维更新
void Track_Asso:: track_hight_update(int updata_track_index, //更新的航迹号
int asso_point_index, //点迹号
std::vector <Trust_Track> *trust_track //航迹
std::vector <Trust_Track> *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
)
{
@@ -978,19 +961,19 @@ void Track_Asso:: track_hight_update(int updata_
if((*trust_track)[updata_track_index-1].range_point <= 1000)
{
height_win_length = H_F_WIN_LEN+2;
height_win_length = rdp_parameter.height_smooth_win+2;
}
else if((*trust_track)[updata_track_index-1].range_point <= 2000 && (*trust_track)[updata_track_index-1].range_point > 1000)
{
height_win_length = H_F_WIN_LEN+3;
height_win_length = rdp_parameter.height_smooth_win+3;
}
else if((*trust_track)[updata_track_index-1].range_point <= 4000 && (*trust_track)[updata_track_index-1].range_point > 2000)
{
height_win_length = H_F_WIN_LEN+4;
height_win_length = rdp_parameter.height_smooth_win+4;
}
else
{
height_win_length = H_F_WIN_LEN+6;
height_win_length = rdp_parameter.height_smooth_win+6;
}
double sum=0;