diff --git a/data_process_class_dll/rdp_record.cpp b/data_process_class_dll/rdp_record.cpp index e16b201..226b83b 100644 --- a/data_process_class_dll/rdp_record.cpp +++ b/data_process_class_dll/rdp_record.cpp @@ -260,7 +260,7 @@ RDP_RECORD_DEFINE(RdpRecTrustTrack); //关联信息 YLT_REFL(RdpRecAssoInfo, id,point_id,track_id,circle_id,d1,d2,d3,d_min,point_index,track_index, delta_T,delta_h,R,VT, - thd_d,thd_h,thd_r, + tas_flag,thd_d,thd_h,thd_r, min_d_flag); RDP_RECORD_DEFINE(RdpRecAssoInfo); diff --git a/data_process_class_dll/rdp_record_entity.h b/data_process_class_dll/rdp_record_entity.h index 4b0d9f7..6180699 100644 --- a/data_process_class_dll/rdp_record_entity.h +++ b/data_process_class_dll/rdp_record_entity.h @@ -407,6 +407,8 @@ struct RdpRecAssoInfo double R; double VT; + bool tas_flag; //TAS关联标志 1是 0否 + bool thd_d; bool thd_h; bool thd_r; @@ -432,6 +434,7 @@ struct RdpRecAssoInfo delta_h = p.delta_h; R = p.R; VT = p.VT; + tas_flag = p.tas_flag; thd_d = p.thd_d; thd_h = p.thd_h; thd_r = p.thd_r; diff --git a/data_process_class_dll/struct.h b/data_process_class_dll/struct.h index 155e1ba..f1f91b6 100644 --- a/data_process_class_dll/struct.h +++ b/data_process_class_dll/struct.h @@ -157,6 +157,8 @@ struct Asso_Info double R; double VT; + bool tas_flag; //TAS关联标志 1是 0否 + bool thd_d; bool thd_h; bool thd_r; diff --git a/data_process_class_dll/track_asso.cpp b/data_process_class_dll/track_asso.cpp index 17495d0..5bf9c08 100644 --- a/data_process_class_dll/track_asso.cpp +++ b/data_process_class_dll/track_asso.cpp @@ -538,6 +538,7 @@ void Track_Asso:: model_filter(std::vector *trust_track,struct Ra associated_info_tmp.delta_h=delta_h; associated_info_tmp.R=R; associated_info_tmp.VT=VT; + associated_info_tmp.tas_flag=point_process[loop_of_point].point_type; associated_info_tmp.thd_d=thd_d; associated_info_tmp.thd_h=thd_h; associated_info_tmp.thd_r=thd_r; diff --git a/data_process_class_dll/track_asso_tas.cpp b/data_process_class_dll/track_asso_tas.cpp index f60f5a1..128812f 100644 --- a/data_process_class_dll/track_asso_tas.cpp +++ b/data_process_class_dll/track_asso_tas.cpp @@ -6,6 +6,10 @@ #include #include #include + +#include "rdp_record.h" +#include "rdp_record_entity.h" + using namespace Eigen; using namespace std; @@ -109,6 +113,13 @@ long long latest_timestamp //最新时间戳 std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim, (*trust_track)[i].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim)); std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim, (*trust_track)[i].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim)); + + (*trust_track)[i].Range = Trust_Track_Output[*Trust_track_num_Output-1][0].Range; + (*trust_track)[i].Azimuth = Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth; + (*trust_track)[i].Elevation = Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation; + (*trust_track)[i].Velocity = Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V; + + RDP_RECORD(RdpRecTrustTrack((*trust_track)[i])); } } @@ -240,16 +251,7 @@ void Track_Asso_Tas::model_filter( std::vector *t long long latest_timestamp //最新时间戳 ) { - //存储关联信息 - struct asso_info - { - double d1; - double d2; - double d3; - double d_min; - int point_index; - }; - std::vector associated_info; + std::vector associated_info; //航迹信息 double X1[6] = {0},X2[6] = {0},X3[6] = {0},P1[6][6] = {{0}},P2[6][6] = {{0}},P3[6][6] = {{0}}; @@ -327,24 +329,38 @@ void Track_Asso_Tas::model_filter( std::vector *t thd_h = true; } + struct Asso_Info associated_info_tmp; + associated_info_tmp.d1=d1; + associated_info_tmp.d2=d2; + associated_info_tmp.d3=d3; + associated_info_tmp.point_index=static_cast(loop_of_point)+1; + associated_info_tmp.track_index=tas_track_idx; + if(d1<=d2&&d1<=d3) + associated_info_tmp.d_min=d1; + else if(d2<=d1&& d2<=d3) + associated_info_tmp.d_min=d2; + else + associated_info_tmp.d_min=d3; + associated_info_tmp.point_id=point_process[loop_of_point].point_id; + associated_info_tmp.track_id=(*trust_track)[tas_track_idx].track_id; + associated_info_tmp.circle_id=point_process[loop_of_point].circle_id; + associated_info_tmp.delta_T=delta_T; + associated_info_tmp.delta_h=delta_h; + associated_info_tmp.R=0; + associated_info_tmp.VT=0; + associated_info_tmp.tas_flag=point_process[loop_of_point].point_type; + associated_info_tmp.thd_d=thd_d; + associated_info_tmp.thd_h=thd_h; + associated_info_tmp.thd_r=true; + associated_info_tmp.min_d_flag=false; + + //小于关联门限 保存关联信息 if(thd_d && thd_h) { - - // std::cout << "TAS asoooooo111" ; - struct asso_info associated_info_tmp; - associated_info_tmp.d1=d1; - associated_info_tmp.d2=d2; - associated_info_tmp.d3=d3; - associated_info_tmp.point_index=static_cast(loop_of_point)+1; - if(d1<=d2&&d1<=d3) - associated_info_tmp.d_min=d1; - else if(d2<=d1&& d2<=d3) - associated_info_tmp.d_min=d2; - else - associated_info_tmp.d_min=d3; - associated_info.push_back(associated_info_tmp); + }else{ + RDP_RECORD(RdpRecAssoInfo(associated_info_tmp)); } } @@ -370,6 +386,8 @@ void Track_Asso_Tas::model_filter( std::vector *t double d2=associated_info[min_index-1].d2; double d3=associated_info[min_index-1].d3; + associated_info[min_index-1].min_d_flag=true; + //卡尔曼滤波 double Z[3]={point_process[point_index-1].range,point_process[point_index-1].azimuth,point_process[point_index-1].velocity}; double prt = point_process[point_index-1].PRF_index; @@ -406,7 +424,7 @@ void Track_Asso_Tas::model_filter( std::vector *t double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0; //更新航迹 - for(size_t i=0;isize();i++) + for(size_t i=0;isize();i++){ if((*trust_track)[i].Track_Index == tas_track_idx) { @@ -450,6 +468,8 @@ void Track_Asso_Tas::model_filter( std::vector *t (*trust_track)[i].GNSS_time = point_process[point_index-1].GNSS_time; //更新关联上的点迹信息 + (*trust_track)[i].point_id=point_process[point_index-1].point_id; + (*trust_track)[i].circle_id=point_process[point_index-1].circle_id; (*trust_track)[i].range_point=point_process[point_index-1].range; (*trust_track)[i].azi_point=point_process[point_index-1].azimuth/PI*180; (*trust_track)[i].elev_point= asin(point_process[point_index-1].height/point_process[point_index-1].range)/PI*180; @@ -472,7 +492,12 @@ void Track_Asso_Tas::model_filter( std::vector *t //更新高度 track_hight_update(tas_track_idx,point_index,trust_track); } + } + for(auto& info : associated_info) + { + RDP_RECORD(RdpRecAssoInfo(info)); + } } //未关联上，航迹外推 else