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 15:56:02 +08:00
parent 76b0b460a5
commit 7e4cb2922d
34 changed files with 202 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/track_asso.cpp
+17 -20
View File
@@ -19,8 +19,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, //最新时间戳
const RDP_parameter& rdp_parameter //工作参数
long long latest_timestamp //最新时间戳
)
{
@@ -35,7 +34,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
//IMM算法
model_interaction(trust_track);
model_filter(trust_track, rdp_parameter, latest_timestamp);
model_filter(trust_track, latest_timestamp);
model_output(trust_track);
@@ -288,7 +287,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],const RDP_parameter& rdp_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])
{
//F
@@ -428,20 +427,19 @@ void Track_Asso::IMM_d_cal(double v_track,
double X3[6], double P3[6][6],
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
const RDP_parameter& rdp_parameter)
double *d1,double *d2,double *d3)
{
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3);
kalman Kalman;
*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);
*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);
}
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_parameter& rdp_parameter, long long latest_timestamp)
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track, long long latest_timestamp)
{
@@ -492,7 +490,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
//计算统计距离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,rdp_parameter);
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3);
//判定统计距离门限，thd_d
bool thd_d = false;
@@ -623,7 +621,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3);
if (delta_T > 0)
{
@@ -632,9 +630,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
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, 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);
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);
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
@@ -721,7 +719,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
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,rdp_parameter);
track_hight_update(track_index,point_index,trust_track);
(*trust_track)[track_index-1].Extrapolate_round = 0; //连续未用实点更新时间
(*trust_track)[track_index-1].point_flag=1; //实点
@@ -789,7 +787,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3);
//航迹X P
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
@@ -947,8 +945,7 @@ 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, //航迹
const RDP_parameter& rdp_parameter //工作参数
std::vector <Trust_Track> *trust_track //航迹
)
{