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_init.cpp
+38 -38
View File
@@ -14,7 +14,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter
const RDP_parameter& rdp_parameter
)
{
@@ -35,10 +35,10 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
if(point_process.size()>0)
{
//点迹与临时航迹关联
point_temp_track_asso(temp_track,Work_Parameter);
point_temp_track_asso(temp_track,rdp_parameter);
//点迹与航迹头关联
point_track_head_asso(temp_track,Work_Parameter);
point_track_head_asso(temp_track,rdp_parameter);
//删除关联上的点迹
std::vector <PointRecv>::iterator Iter;
@@ -118,7 +118,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
#endif
//临时航迹满足起始长度 转为可靠航迹
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,rdp_parameter);
}
@@ -130,7 +130,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
}
void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
//关联信息
@@ -179,7 +179,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
{
//计算d
kalman Kalman;
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind,rdp_parameter);
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
//计算夹角
@@ -191,7 +191,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
double y2=Z[0]*sin(Z[1]);
double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2);
if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha<ALPHA_START && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
if(d*d<=rdp_parameter.track_init_threshold*rdp_parameter.track_init_threshold && alpha<rdp_parameter.track_init_angle && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*rdp_parameter.sigma_pitch&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
{
struct Asso_info asso_info_tmp;
asso_info_tmp.point_idx = static_cast<int>(i)+1;
@@ -251,7 +251,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
kalman Kalman;
double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].CPI_time)/1000.0;
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -278,8 +278,8 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
snr_tmp = 1/(1+exp(-(temp_track_ref.back().SNR - 16) / 1.2));
}
quality_tmp = d_tmp * TRACK_QUALITY_D_WEIGHT + snr_tmp * TRACK_QUALITY_SNR_WEIGHT;
quality_tmp /= (TRACK_QUALITY_D_WEIGHT + TRACK_QUALITY_SNR_WEIGHT);
quality_tmp = d_tmp * rdp_parameter.track_quality_d_weight + snr_tmp * rdp_parameter.track_quality_snr_weight;
quality_tmp /= (rdp_parameter.track_quality_d_weight + rdp_parameter.track_quality_snr_weight);
if(L == 3){
for(int i=0;i<L;i++)
@@ -287,7 +287,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
temp_track_ref[i].quality = quality_tmp;
}
}else{
temp_track_ref[L-1].quality = temp_track_ref[L-2].quality * TRACK_QUALITY_ITER_COEFF + quality_tmp * (1 - TRACK_QUALITY_ITER_COEFF);
temp_track_ref[L-1].quality = temp_track_ref[L-2].quality * rdp_parameter.track_quality_iter_coeff + quality_tmp * (1 - rdp_parameter.track_quality_iter_coeff);
}
}
@@ -315,7 +315,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
}
void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
//关联上的信息
@@ -357,26 +357,26 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
double vmax;
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
if(rdp_parameter.radar_work_mode == 0) //近程模式 最大速度减小一点
{
vmax = Work_Parameter.V_MAX;
vmax = rdp_parameter.track_init_velocity_max;
}
else //中远程模式 最大速度正常用
{
vmax = Work_Parameter.V_MAX;
vmax = rdp_parameter.track_init_velocity_max;
}
double delta_T = (T_point - T_track_head)/1000.0;
// if(
// ( (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
// || (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
// ( (rdp_parameter.radar_work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=rdp_parameter.track_init_velocity_min*delta_T/2.0) ))
// || (rdp_parameter.radar_work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=rdp_parameter.track_init_velocity_min*delta_T/2.0) ))
// )
// && vr_point*v_track_head>0 )
//点航关联条件
if( dis <= vmax* delta_T && dis >= Work_Parameter.V_MIN * delta_T && \
if( dis <= vmax* delta_T && dis >= rdp_parameter.track_init_velocity_min * delta_T && \
vr_point*v_track_head>0 && \
dis >= abs(vr_point) * delta_T && \
dis >= abs(v_track_head) * delta_T
@@ -434,7 +434,7 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
kalman Kalman;
double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].CPI_time)/1000.0;
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -474,7 +474,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
std::vector <std::vector<Temp_track>> Track_to_start;
@@ -498,8 +498,8 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
// double snr_tmp = 1/(1+exp(-(average_snr - 16) / 1.2));
// double &d = (*Iter)[L-1].d;
// double d_tmp = exp(-d);
// double quality_tmp = d_tmp * TRACK_QUALITY_D_WEIGHT + snr_tmp * TRACK_QUALITY_SNR_WEIGHT;
// quality_tmp /= (TRACK_QUALITY_D_WEIGHT + TRACK_QUALITY_SNR_WEIGHT);
// double quality_tmp = d_tmp * rdp_parameter.track_quality_d_weight + snr_tmp * rdp_parameter.track_quality_snr_weight;
// quality_tmp /= (rdp_parameter.track_quality_d_weight + rdp_parameter.track_quality_snr_weight);
// for(int i=0;i<L;i++)
// {
// (*Iter)[i].quality = quality_tmp;
@@ -514,12 +514,12 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
}
//航迹长度小于起始点数，不起批
if(L < Work_Parameter.track_start_point_num){
if(L < rdp_parameter.track_init_point_num_min){
track_init_flag = false;
}
//航迹起批禁止区域判断
if(track_init_prohibit(range,azi,Work_Parameter)!=0)
if(track_init_prohibit(range,azi,rdp_parameter)!=0)
{
track_init_flag = false;
}
@@ -527,32 +527,32 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
//航迹质量起批条件判断
switch(L){
case 3:
if(quality < TRACK_START_QUALITY_L3){
if(quality < rdp_parameter.track_init_quality_L3){
track_init_flag = false;
}
break;
case 4:
if(quality < TRACK_START_QUALITY_L4){
if(quality < rdp_parameter.track_init_quality_L4){
track_init_flag = false;
}
break;
case 5:
if(quality < TRACK_START_QUALITY_L5){
if(quality < rdp_parameter.track_init_quality_L5){
track_init_flag = false;
}
break;
case 6:
if(quality < TRACK_START_QUALITY_L6){
if(quality < rdp_parameter.track_init_quality_L6){
track_init_flag = false;
}
break;
case 7:
if(quality < TRACK_START_QUALITY_L7){
if(quality < rdp_parameter.track_init_quality_L7){
track_init_flag = false;
}
break;
default:
if(quality < TRACK_START_QUALITY_L7){
if(quality < rdp_parameter.track_init_quality_L7){
track_init_flag = false;
}
break;
@@ -581,7 +581,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
return;
//2.两两比较Track_to_start中的航迹信息，删除重复的航迹
int L = Work_Parameter.track_start_point_num;
int L = rdp_parameter.track_init_point_num_min;
for (size_t i=0; i+1<Track_to_start.size();i++)
{
if(Track_to_start[i][0].asso_flag!=2)
@@ -632,9 +632,9 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
for (size_t i=0; i<Track_to_start.size();i++)
{
if (Track_to_start[i].size() < 3) continue;
if(trust_track->size()<MAX_TRACK_INDEX)
if(trust_track->size() < static_cast<std::size_t>(rdp_parameter.track_index_max))
{
int index = track_index_mangement.track_ind_get(trust_track);
int index = track_index_mangement.track_ind_get(trust_track, rdp_parameter);
if(index>0)
{
@@ -647,7 +647,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
double T1=(Track_to_start[i][L-2].CPI_time-Track_to_start[i][L-3].CPI_time)/1000.0;
double T2=(Track_to_start[i][L-1].CPI_time-Track_to_start[i][L-2].CPI_time)/1000.0;
kalman Kalman;
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P,rdp_parameter);
Trust_Track trust_track_tmp = {};
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
@@ -715,7 +715,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>(Track_to_start[i][j].amplitude);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast<float>(Track_to_start[i][j].SNR);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast<float>(Track_to_start[i][j].RCS);
Trust_Track_Output[*Trust_track_num_Output-1][j].Quality = static_cast<float>(Track_to_start[i][j].quality * TRACK_QUALITY_MAX);
Trust_Track_Output[*Trust_track_num_Output-1][j].Quality = static_cast<float>(Track_to_start[i][j].quality * rdp_parameter.track_quality_max);
Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;
@@ -783,12 +783,12 @@ void Track_Init::tmp_track_die(std::vector <std::vector<Temp_track>> *temp_t
}
};
int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter)
int Track_Init::track_init_prohibit(double r, double azi, const RDP_parameter& rdp_parameter)
{
for (int i=0;i<Work_Parameter.track_prohibite_area_num;i++)
for (int i=0;i<rdp_parameter.track_init_prohibite_area_num;i++)
{
if(r<Work_Parameter.R_max_track_prohibited[i] && r>Work_Parameter.R_min_track_prohibited[i] && azi<Work_Parameter.Azimuth_max_track_prohibited[i] && azi>Work_Parameter.Azimuth_min_track_prohibited[i])
if(r<rdp_parameter.track_init_prohibite_area_range_max[i] && r>rdp_parameter.track_init_prohibite_area_range_min[i] && azi<rdp_parameter.track_init_prohibite_area_azimuth_max[i] && azi>rdp_parameter.track_init_prohibite_area_azimuth_min[i])
{
return 1;
}