diff --git a/data_process_class_dll/data_process_class_dll.h b/data_process_class_dll/data_process_class_dll.h index 6847416..f8b3a4e 100644 --- a/data_process_class_dll/data_process_class_dll.h +++ b/data_process_class_dll/data_process_class_dll.h @@ -51,6 +51,7 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Track float Elevation; //俯仰 float Range_V; //速度 float Amplitude; //幅度 + float Quality; //航迹质量 int Track_Index; //航迹号 int Point_Sum; //本条航迹更新点的数目 int Flag_Direction; //方向标志1靠近/0远离 diff --git a/data_process_class_dll/parameters.h b/data_process_class_dll/parameters.h index b6e56a7..1dd035c 100644 --- a/data_process_class_dll/parameters.h +++ b/data_process_class_dll/parameters.h @@ -49,8 +49,30 @@ //#define MECHANICAL_SCANNING //#define PHASE_SCANNING +/******************************通用数据处理参数**************************************/ +//航迹质量管理参数 +#define TRACK_QUALITY_MAX 100 +#define TRACK_QUALITY_MIN 0 +#define TRACK_QUALITY_D_WEIGHT 0.5 +#define TRACK_QUALITY_SNR_WEIGHT 0.3 +#define TRACK_QUALITY_LEN_WEIGHT 0.2 +#define TRACK_QUALITY_ITER_COEFF 0.8 +#define TRACK_QUALITY_DECREASE_COEFF_O2 -0.0008 +#define TRACK_QUALITY_DECREASE_COEFF_O1 -0.0025 + +//航迹起批质量条件 +#define TRACK_START_QUALITY_L3 0.7 +#define TRACK_START_QUALITY_L4 0.6 +#define TRACK_START_QUALITY_L5 0.5 +#define TRACK_START_QUALITY_L6 0.4 +#define TRACK_START_QUALITY_L7 0.3 + +//航迹消批质量条件 +#define TRACK_DIE_QUALITY 0.3 +#define TRACK_DIE_QUALITY_PRAMARY 0.3 + #ifdef MECHANICAL_SCANNING -/******************************数据处理参数**************************************/ +/******************************机扫数据处理参数**************************************/ // #define DATA_RATE_TAS 0.0625 #define SIGMA_R 10.0 //测量误差 @@ -101,7 +123,7 @@ #ifdef PHASE_SCANNING -/******************************数据处理参数**************************************/ +/******************************相扫数据处理参数**************************************/ // #define DATA_RATE_TAS 0.3 #define SIGMA_R 10.0 //测量误差 diff --git a/data_process_class_dll/struct.h b/data_process_class_dll/struct.h index f1f91b6..3c47d04 100644 --- a/data_process_class_dll/struct.h +++ b/data_process_class_dll/struct.h @@ -46,17 +46,23 @@ struct Trust_Track int Track_Index; //航迹号 double Range; //距离 - double Azimuth; //方位 - double Elevation; //俯仰 + double Azimuth; //方位 + double Elevation; //俯仰 double Velocity; //航向速度 double Amplitude; //幅度 double Height; //高度 + double RCS; //RCS + double Quality; //航迹质量 - long long T_track; //航迹时间 + long long T_track; //航迹时间（CPI时间） long long GNSS_time; //GNSS时间 + long long Unupdated_time; //航迹未更新时间 int Track_Sum; //航迹总数 int Point_Index; //该航迹上的第几个点 + int Point_sum; //本条航迹更新点的数目 + int Valid_point_sum; //本条航迹有效点的数目 + int associate_point_number; //关联的点数 int Track_Mode; //跟踪模式 int Track_Update_Flag; //航迹更新标志 1已更新/0未更新 @@ -67,8 +73,6 @@ struct Trust_Track int manual_delete_flag; //手动航迹删除 int manual_tracking_flag; //手动TAS - int associate_point_number; //关联的点数 - int primary_target_flag; //重点目标标记 int Target_Type; //目标类型 搜索目标 低空目标 地面目标 @@ -87,8 +91,6 @@ struct Trust_Track float amp_asv_point; //关联上的点迹信息（归一化幅度） float prn_fftn_point; //点迹速度维FFT点数 - double RCS; - int pitch_num; // 俯仰波位号 float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 @@ -122,6 +124,7 @@ struct Temp_track double SNR; //信噪比 double RCS; //RCS double amplitude; //幅度 + double quality; //航迹质量 long long CPI_time; //时间戳 long long GNSS_time; //GNSS时间戳 diff --git a/data_process_class_dll/track_asso.cpp b/data_process_class_dll/track_asso.cpp index 2deab69..9f83f48 100644 --- a/data_process_class_dll/track_asso.cpp +++ b/data_process_class_dll/track_asso.cpp @@ -100,6 +100,7 @@ int Track_Asso:: track_asso_process(std::vector *point_ 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((*trust_track)[i].Height+Work_Parameter.Height); + Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast((*trust_track)[i].Quality * TRACK_QUALITY_MAX); double Direction_Angle; Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]); @@ -656,9 +657,18 @@ void Track_Asso:: model_filter(std::vector *trust_track,struct Ra (*trust_track)[track_index-1].P3[ii][jj]=P3_filter[ii][jj]; } + //计算航迹质量 + 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; + + (*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].T_track = point_process[point_index-1].CPI_time; (*trust_track)[track_index-1].GNSS_time = point_process[point_index-1].GNSS_time; + (*trust_track)[track_index-1].Unupdated_time = 0; //更新关联上的点迹信息 (*trust_track)[track_index-1].point_id = point_process[point_index-1].point_id; @@ -688,6 +698,8 @@ void Track_Asso:: model_filter(std::vector *trust_track,struct Ra (*trust_track)[track_index-1].point_flag=1; //实点 (*trust_track)[track_index-1].associate_point_number = (*trust_track)[track_index-1].associate_point_number+1; //关联点数+1 (*trust_track)[track_index-1].Track_Update_Flag = 1; //航迹更新标志 + (*trust_track)[track_index-1].Point_sum = (*trust_track)[track_index-1].Point_sum + 1; + (*trust_track)[track_index-1].Valid_point_sum = (*trust_track)[track_index-1].Valid_point_sum + 1; } //删除associated_info中关联上的航迹 点迹信息 @@ -726,31 +738,32 @@ void Track_Asso:: model_filter(std::vector *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; + } + //外推点数未到上限，则继续外推，否则不外推 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 = 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; - } 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); @@ -765,7 +778,6 @@ void Track_Asso:: model_filter(std::vector *trust_track,struct Ra memcpy(P3,(*trust_track)[i].P3,6*6*sizeof(double)); //外推 - double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6]; kalman Kalman; Kalman.kalman_pred(F, Q1, X1, P1, X1_pred, P1_pred); @@ -778,6 +790,7 @@ void Track_Asso:: model_filter(std::vector *trust_track,struct Ra memcpy((*trust_track)[i].P2,P2_pred,6*6*sizeof(double)); memcpy((*trust_track)[i].P3,P3_pred,6*6*sizeof(double)); + (*trust_track)[i].circle_id++; (*trust_track)[i].T_track = static_cast((*trust_track)[i].T_track+delta_T*1000.0); //更新航迹时间 (*trust_track)[i].GNSS_time = static_cast((*trust_track)[i].GNSS_time+delta_T*1000.0); //更新航迹时间 (*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间 @@ -788,9 +801,18 @@ void Track_Asso:: model_filter(std::vector *trust_track,struct Ra (*trust_track)[i].point_flag=0; //虚点 (*trust_track)[i].Track_Update_Flag = 0; //航迹更新标志 } + + //航迹质量递减 + 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].Unupdated_time += static_cast(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 = (*trust_track)[i].Quality < 0.0 ? 0.0 : (*trust_track)[i].Quality; + (*trust_track)[i].Point_sum = (*trust_track)[i].Point_sum + 1; + } } - } void Track_Asso:: model_output(std::vector *trust_track) diff --git a/data_process_class_dll/track_die.cpp b/data_process_class_dll/track_die.cpp index a68a1f7..26e33c1 100644 --- a/data_process_class_dll/track_die.cpp +++ b/data_process_class_dll/track_die.cpp @@ -16,16 +16,19 @@ void Track_Die::track_die_process( std::vector *trust_track, { bool die_flag = false; + //手动删除 if((*Iter).manual_delete_flag == 1) { die_flag = true; } - if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 0 && (*Iter).Extrapolate_round > TRACK_DIE_ROUND) + //非重点航迹自动消亡 + if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 0 && (*Iter).Quality < TRACK_DIE_QUALITY) { die_flag = true; } + //重点航迹自动消亡 if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 1 && (*Iter).Extrapolate_round > TRACK_DIE_ROUND_PRIMARY) { die_flag = true; diff --git a/data_process_class_dll/track_init.cpp b/data_process_class_dll/track_init.cpp index 9733374..1d0bb3f 100644 --- a/data_process_class_dll/track_init.cpp +++ b/data_process_class_dll/track_init.cpp @@ -254,6 +254,40 @@ void Track_Init::point_temp_track_asso(std::vector > 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); + + //计算航迹质量 + auto& temp_track_ref = (*temp_track)[(*temp_track).size()-1]; + L = static_cast(temp_track_ref.size()); + + double snr_tmp = 0; + double d_tmp = 0; + double quality_tmp = 0; + + d_tmp = exp(-temp_track_ref[L-1].d); + + if(L == 3){ + double average_snr = 0; + for(int i=0;i *trust_tra int L=static_cast((*Iter).size()); double range=(*Iter)[L-1].r; double azi=(*Iter)[L-1].azi/PI*180; + double quality = (*Iter)[L-1].quality; - //（已停用）1km以内，SNR小于20dB的临时航迹不转为可靠航迹 - // bool snr_flag = true; - // for(int p_idx = 0; p_idx < L; p_idx++){ - // if((*Iter)[p_idx].r < 1000 && (*Iter)[p_idx].snr < 20){ - // snr_flag = false; - // break; - // } + // //计算航迹初始质量 + // double average_snr = 0; + // for(int i=0;i=Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0) //按长度查找TRUST_TRACK_POINT + //航迹起批条件判断 + bool track_init_flag = true; + + //航迹长度大于10点，不起批 + if(L > 10){ + track_init_flag = false; + } + + //航迹长度小于起始点数，不起批 + if(L < Work_Parameter.track_start_point_num){ + track_init_flag = false; + } + + //航迹起批禁止区域判断 + if(track_init_prohibit(range,azi,Work_Parameter)!=0) + { + track_init_flag = false; + } + + //航迹质量起批条件判断 + switch(L){ + case 3: + if(quality < TRACK_START_QUALITY_L3){ + track_init_flag = false; + } + break; + case 4: + if(quality < TRACK_START_QUALITY_L4){ + track_init_flag = false; + } + break; + case 5: + if(quality < TRACK_START_QUALITY_L5){ + track_init_flag = false; + } + break; + case 6: + if(quality < TRACK_START_QUALITY_L6){ + track_init_flag = false; + } + break; + case 7: + if(quality < TRACK_START_QUALITY_L7){ + track_init_flag = false; + } + break; + default: + if(quality < TRACK_START_QUALITY_L7){ + track_init_flag = false; + } + break; + } + + //TODO: 杂波密度判断 + + if(track_init_flag) { //加入Track_to_start中 Track_to_start.push_back(std::vector ()); @@ -565,6 +663,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector *trust_tra std::memcpy(trust_track_tmp.range_dim, Track_to_start[i][L-1].range_dim, sizeof(trust_track_tmp.range_dim)); trust_track_tmp.T_track = Track_to_start[i][L-1].CPI_time; trust_track_tmp.GNSS_time = Track_to_start[i][L-1].GNSS_time; + trust_track_tmp.Unupdated_time = 0; trust_track_tmp.point_flag=1; //实点 trust_track_tmp.Track_Mode=0; //跟踪模式 TWS 0 trust_track_tmp.Target_Type=UNCONF_TARGET; @@ -576,6 +675,9 @@ void Track_Init::tmp_track_to_trust_track(std::vector *trust_tra trust_track_tmp.associate_point_number = 0; trust_track_tmp.manual_tracking_flag = 0; trust_track_tmp.point_type = 0; + trust_track_tmp.Quality = Track_to_start[i][L-1].quality; + trust_track_tmp.Point_sum = L; + trust_track_tmp.Valid_point_sum = L; trust_track_tmp.u[0]=0.3333; trust_track_tmp.u[1]=0.3333; trust_track_tmp.u[2]=0.3333; @@ -612,6 +714,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector *trust_tra Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast(Track_to_start[i][j].amplitude); Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast(Track_to_start[i][j].SNR); Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast(Track_to_start[i][j].RCS); + Trust_Track_Output[*Trust_track_num_Output-1][j].Quality = static_cast(Track_to_start[i][j].quality * TRACK_QUALITY_MAX); Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;