From c3c7e668d24d213c05494eda5aa608682242e3cc Mon Sep 17 00:00:00 2001 From: waiwaylee Date: Wed, 30 Sep 2026 16:33:18 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=EF=BC=9A1=E3=80=81=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E4=B8=B4=E6=97=B6=E8=88=AA=E8=BF=B9=E5=BD=95=E5=85=A5?= =?UTF-8?q?=E6=95=B0=E6=8D=AE=E5=BA=93=EF=BC=9B=202=E3=80=81=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E7=82=B9=E8=BF=B9=E4=BF=A1=E6=81=AF=E7=9A=84=E5=9C=88?= =?UTF-8?q?ID=E8=AE=B0=E5=BD=95=EF=BC=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: waiwaylee --- data_process_class_dll/data_process.cpp | 6 +- data_process_class_dll/data_process.h | 2 + data_process_class_dll/rdp_record.cpp | 10 +- data_process_class_dll/rdp_record_entity.h | 119 ++++++++++++++++++ data_process_class_dll/struct.h | 22 ++-- data_process_class_dll/track_init.cpp | 115 +++++++---------- data_process_class_dll/track_init.h | 2 + .../track_init_direct_tracking.cpp | 36 +++--- 8 files changed, 212 insertions(+), 100 deletions(-) diff --git a/data_process_class_dll/data_process.cpp b/data_process_class_dll/data_process.cpp index 0fd3df8..5fa4ece 100644 --- a/data_process_class_dll/data_process.cpp +++ b/data_process_class_dll/data_process.cpp @@ -31,6 +31,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150]) PointRecv Data_buffer_temp; Data_buffer_temp.point_id = point_id++; Data_buffer_temp.frame_id = frame_id; + Data_buffer_temp.circle_id = circle_id; Data_buffer_temp.amplitude=Data_Input[i].Amplitude; Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI; Data_buffer_temp.SNR = Data_Input[i].Snr; @@ -65,7 +66,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150]) if(Data_Input[0].Beam_index_aiz == 0) //TWS { last_beam_num = Data_Input[0].Beam_index_aiz; - + circle_id++; return 1; } else @@ -87,6 +88,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150]) PointRecv Data_buffer_temp; Data_buffer_temp.point_id = point_id++; Data_buffer_temp.frame_id = frame_id; + Data_buffer_temp.circle_id = circle_id; Data_buffer_temp.amplitude=Data_Input[i].Amplitude; Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI; Data_buffer_temp.SNR = Data_Input[i].Snr; @@ -120,7 +122,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150]) // << "data_num:" << data_num; } - + circle_id++; return 3; } diff --git a/data_process_class_dll/data_process.h b/data_process_class_dll/data_process.h index 7f08e97..cb521c9 100644 --- a/data_process_class_dll/data_process.h +++ b/data_process_class_dll/data_process.h @@ -102,6 +102,8 @@ private: long long frame_id = 0; //帧id + long long circle_id = 0; //圈id + struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入 Dot_Coh dot_coh; //点迹凝聚 diff --git a/data_process_class_dll/rdp_record.cpp b/data_process_class_dll/rdp_record.cpp index acebbdb..49bb200 100644 --- a/data_process_class_dll/rdp_record.cpp +++ b/data_process_class_dll/rdp_record.cpp @@ -231,20 +231,24 @@ namespace rdp_record // RDP_RECORD_DEFINE(结构体名); //输入点迹 -YLT_REFL(RdpRecPoint, id,point_id,frame_id,range,azimuth, velocity,amplitude ,height,SNR,RCS, +YLT_REFL(RdpRecPoint, id,point_id,frame_id,circle_id,range,azimuth, velocity,amplitude ,height,SNR,RCS, encoder_value,amp_asv_point,prn_fftn_point,CPI_time,GNSS_time,use_flag,azimuth_beam_index,pitch_beam_index,PRF_index,freq_index,point_type,point_section_asso); RDP_RECORD_DEFINE(RdpRecPoint); //TWS凝聚后点迹 -YLT_REFL(RdpRecPointCohesionTWS, id,point_id,frame_id,range,azimuth, velocity,amplitude ,height,SNR,RCS, +YLT_REFL(RdpRecPointCohesionTWS, id,point_id,frame_id,circle_id,range,azimuth, velocity,amplitude ,height,SNR,RCS, encoder_value,amp_asv_point,prn_fftn_point,CPI_time,GNSS_time,use_flag,azimuth_beam_index,pitch_beam_index,PRF_index,freq_index,point_type,point_section_asso); RDP_RECORD_DEFINE(RdpRecPointCohesionTWS); //TAS凝聚后点迹 -YLT_REFL(RdpRecPointCohesionTAS, id,point_id,frame_id,range,azimuth, velocity,amplitude ,height,SNR,RCS, +YLT_REFL(RdpRecPointCohesionTAS, id,point_id,frame_id,circle_id,range,azimuth, velocity,amplitude ,height,SNR,RCS, encoder_value,amp_asv_point,prn_fftn_point,CPI_time,GNSS_time,use_flag,azimuth_beam_index,pitch_beam_index,PRF_index,freq_index,point_type,point_section_asso); RDP_RECORD_DEFINE(RdpRecPointCohesionTAS); +//临时航迹 +YLT_REFL(RdpRecTempTrack, id, temp_track_id,point_id,circle_id,X,P,r,azi,vr,height,SNR,RCS,amplitude, CPI_time,GNSS_time,d,buff_round,temp_track_section_idx,asso_flag,pitch_num); +RDP_RECORD_DEFINE(RdpRecTempTrack); + // 示例②：本圈输出的航迹 YLT_REFL(RdpRecTrackOut, id, track_index, track_mode, point_sum, flag_point, flag_direction, gnss_time_ms, track_time_s, x, y, z, v_x, v_y, v_z, range_m, azi_deg, elev_deg, diff --git a/data_process_class_dll/rdp_record_entity.h b/data_process_class_dll/rdp_record_entity.h index 1a4a419..4891a4d 100644 --- a/data_process_class_dll/rdp_record_entity.h +++ b/data_process_class_dll/rdp_record_entity.h @@ -21,6 +21,11 @@ #include "rdp_record.h" #include #include +#include +#include +#include +#include +#include #include "struct.h" @@ -30,6 +35,18 @@ extern template void rdp_record::Write(const Entity&); \ extern template void rdp_record::Write(const std::vector&) +// 基本类型元素转字符串 +template +std::string elementToString(const T& value); + +// 数组版本 +template +std::string arrayToString(const T (&arr)[N]); + +// 非数组元素：直接转字符串 +template +std::string arrayToString(const T& value); + // ============================ 记录实体定义区（在此新增结构体） ============================ // 输入点迹 —— 输入的原始点迹数据 struct RdpRecPoint @@ -37,6 +54,7 @@ struct RdpRecPoint int64_t id; //自增主键（写入时固定填 0） long long point_id; //点迹ID long long frame_id; //帧ID + long long circle_id; //圈ID double range; //径向距离 double azimuth; //方位 (弧度) double velocity ; //速度 @@ -65,6 +83,7 @@ struct RdpRecPoint id = 0; point_id = p.point_id; frame_id = p.frame_id; + circle_id = p.circle_id; range = p.range; azimuth = p.azimuth; velocity = p.velocity; @@ -94,6 +113,7 @@ struct RdpRecPointCohesionTWS int64_t id; //自增主键（写入时固定填 0） long long point_id; //点迹ID long long frame_id; //帧ID + long long circle_id; //圈ID double range; //径向距离 double azimuth; //方位 (弧度) double velocity ; //速度 @@ -122,6 +142,7 @@ struct RdpRecPointCohesionTWS id = 0; point_id = p.point_id; frame_id = p.frame_id; + circle_id = p.circle_id; range = p.range; azimuth = p.azimuth; velocity = p.velocity; @@ -151,6 +172,7 @@ struct RdpRecPointCohesionTAS int64_t id; //自增主键（写入时固定填 0） long long point_id; //点迹ID long long frame_id; //帧ID + long long circle_id; //圈ID double range; //径向距离 double azimuth; //方位 (弧度) double velocity ; //速度 @@ -179,6 +201,7 @@ struct RdpRecPointCohesionTAS id = 0; point_id = p.point_id; frame_id = p.frame_id; + circle_id = p.circle_id; range = p.range; azimuth = p.azimuth; velocity = p.velocity; @@ -202,6 +225,61 @@ struct RdpRecPointCohesionTAS }; RDP_RECORD_DECLARE(RdpRecPointCohesionTAS); +// 零时航迹 +struct RdpRecTempTrack +{ + int64_t id; //自增主键（写入时固定填 0） + long long temp_track_id; //临时航迹ID + long long point_id; //关联上的点迹ID + long long circle_id; //处理圈数ID + + std::string X; //状态 + std::string P; //协方差 + + double r; //距离 + double azi; //方位 + double vr; //径向速度 + double height; //高度 + double SNR; //信噪比 + double RCS; //RCS + double amplitude; //幅度 + long long CPI_time; //时间戳 + long long GNSS_time; //GNSS时间戳 + double d; //关联上的点的d（马氏距离） + int buff_round; //已缓存圈数 + int temp_track_section_idx; //临时航迹区号 与点迹区的划分相同 + int asso_flag; //关联标志 已关联：1 未关联：0 + int pitch_num; // 俯仰波位号 + + RdpRecTempTrack(){ + id = 0; + } + + RdpRecTempTrack(Temp_track& p){ + id = 0; + temp_track_id = p.temp_track_id; + point_id = p.point_id; + circle_id = p.circle_id; + X = arrayToString(p.X); + P = arrayToString(p.P); + r = p.r; + azi = p.azi; + vr = p.vr; + height = p.height; + RCS = p.RCS; + SNR = p.SNR; + amplitude = p.amplitude; + CPI_time = p.CPI_time; + GNSS_time = p.GNSS_time; + d = p.d; + buff_round = p.buff_round; + temp_track_section_idx = p.temp_track_section_idx; + asso_flag = p.asso_flag; + pitch_num = p.pitch_num; + } +}; +RDP_RECORD_DECLARE(RdpRecTempTrack); + // 示例②：本圈输出的航迹 —— 记录点位于 data_process.cpp::track_process()「数据记录示例②」处， // TWS/TAS 共用一张表并用 track_mode 区分；每行取输出数组的 [i][0] struct RdpRecTrackOut @@ -229,4 +307,45 @@ struct RdpRecTrackOut }; RDP_RECORD_DECLARE(RdpRecTrackOut); + +// ============================ 数组转换函数 ============================ + +// 基本类型元素转字符串 +template +std::string elementToString(const T& value) { + std::ostringstream oss; + oss << value; + return oss.str(); +} + +// 前置声明数组版本 +template +std::string arrayToString(const T (&arr)[N]); + +// 非数组元素：直接转字符串 +template +std::string arrayToString(const T& value) { + return elementToString(value); +} + +// 数组版本 +template +std::string arrayToString(const T (&arr)[N]) { + std::string result; + + // 如果 T 仍然是数组，说明当前不是最内层，用分号； + // 否则当前就是最内层，用逗号。 + const char sep = std::is_array::value ? ';' : ','; + + for (std::size_t i = 0; i < N; ++i) { + if (i > 0) { + result += sep; + } + result += arrayToString(arr[i]); + } + + return result; +} + + #endif // RDP_RECORD_ENTITY_H diff --git a/data_process_class_dll/struct.h b/data_process_class_dll/struct.h index fbfdc0a..5b5adb2 100644 --- a/data_process_class_dll/struct.h +++ b/data_process_class_dll/struct.h @@ -11,6 +11,7 @@ struct PointRecv { long long point_id; //点迹ID long long frame_id; //帧ID + long long circle_id; //圈ID double range; //径向距离 double azimuth; //方位 (弧度) double velocity ; //速度 @@ -74,7 +75,7 @@ struct Trust_Track double vr_point; double prf_point; int point_type; //TWS TAS - double snr_point; //信噪比 + double snr_point; //信噪比 float amp_asv_point; //关联上的点迹信息（归一化幅度） float prn_fftn_point; //点迹速度维FFT点数 @@ -93,32 +94,35 @@ struct Trust_Track double P1[6][6]; double P2[6][6]; double P3[6][6]; - double u[3]; + double u[3]; }; //临时航迹结构体 struct Temp_track { + long long temp_track_id; //临时航迹ID + long long point_id; //关联上的点迹ID + long long circle_id; //处理圈数ID - double X[4]; //状态 + double X[4]; //状态 double P[4][4]; //协方差 double r; //距离 double azi; //方位 double vr; //径向速度 double height; //高度 - double snr; //信噪比 + double SNR; //信噪比 double RCS; //RCS - double Amp; //幅度 - long long T; //时间戳 - long long GNSS_time; //GNSS时间戳 + double amplitude; //幅度 + long long CPI_time; //时间戳 + long long GNSS_time; //GNSS时间戳 - double d; //关联上的点的d + double d; //关联上的点的d（马氏距离） int buff_round; //已缓存圈数 - int Temp_track_section_idx; //临时航迹区号 与点迹区的划分相同 + int temp_track_section_idx; //临时航迹区号 与点迹区的划分相同 int asso_flag; //关联标志 已关联：1 未关联：0 diff --git a/data_process_class_dll/track_init.cpp b/data_process_class_dll/track_init.cpp index 0a1972b..64169eb 100644 --- a/data_process_class_dll/track_init.cpp +++ b/data_process_class_dll/track_init.cpp @@ -6,6 +6,8 @@ #include #include #include +#include "rdp_record.h" +#include "rdp_record_entity.h" using namespace std; int Track_Init::track_init_process_logic( std::vector *point_recv, //输入点迹 @@ -15,27 +17,12 @@ int Track_Init::track_init_process_logic( std::vector int *Trust_track_num_Output, struct RadarPara Work_Parameter ) - { -// for(int i=0;i<(*point_recv).size();i++) -// { -// if((*point_recv)[i].CPI_Time == 30675948 || (*point_recv)[i].CPI_Time == 30682320 -// || (*point_recv)[i].CPI_Time == 30682132 || (*point_recv)[i].CPI_Time == 30688692 -// || (*point_recv)[i].CPI_Time == 30691692 || (*point_recv)[i].CPI_Time == 30691880) -// { -// std::cout << "azi=" << (*point_recv)[i].Azimuth -// << "dis=" << (*point_recv)[i].Range -// << "h=" << (*point_recv)[i].Height -// << "v=" << (*point_recv)[i].Velocity -// << "wave=" << (*point_recv)[i].beam_index -// << "time=" << (*point_recv)[i].CPI_Time; -// } -// } - //取出待起航的点迹数据 - for (size_t i=0;i<(*point_recv).size();i++) + for (size_t i=0;i<(*point_recv).size();i++){ point_process.push_back((*point_recv)[i]); + } //临时航迹的buff_round+1 for (size_t i=0;isize();i++) @@ -78,12 +65,15 @@ int Track_Init::track_init_process_logic( std::vector for (unsigned int i=0;i temp_track_tmp.X[1]=0; temp_track_tmp.X[2]=point_process[i].range*sin(point_process[i].azimuth); temp_track_tmp.X[3]=0; - temp_track_tmp.T = point_process[i].CPI_time; + temp_track_tmp.CPI_time = point_process[i].CPI_time; temp_track_tmp.GNSS_time = point_process[i].GNSS_time; temp_track_tmp.buff_round = 1; // temp_track_tmp.Temp_track_section_idx = floor(point_process[i].beam_index/BEAM_NUM_DOT_SECTION)+1; @@ -125,19 +115,14 @@ int Track_Init::track_init_process_logic( std::vector //消亡临时航迹 tmp_track_die(temp_track); -// std::cout << "-----------------------------------------------"; -// for(int i=0;isize();i++) -// { -// for(int j=0;j<(*temp_track)[i].size();j++) -// { -// std::cout << "temp_track_azi=" << (*temp_track)[i][j].azi -// << "temp_track_dis=" << (*temp_track)[i][j].r -// << "temp_track_h=" << (*temp_track)[i][j].height -// << "temp_track_v=" << (*temp_track)[i][j].vr -// << "temp_track_time=" << (*temp_track)[i][j].T; -// } -// } - +#ifdef RDP_RECORD_ENABLE + //记录临时航迹 + for(auto& vec_t : *temp_track){ + for(auto& t : vec_t){ + RDP_RECORD(RdpRecTempTrack(t)); + } + } +#endif return 0; } @@ -184,7 +169,7 @@ void Track_Init::point_temp_track_asso(std::vector > double v_temp_track=(*temp_track)[j][L-1].vr; double r_temp_track=(*temp_track)[j][L-1].r; double h_temp_track=(*temp_track)[j][L-1].height; - double T_track_head = static_cast((*temp_track)[j][L-1].T); + double T_track_head = static_cast((*temp_track)[j][L-1].CPI_time); double delta_T = (T_point - T_track_head)/1000.0; @@ -204,20 +189,6 @@ void Track_Init::point_temp_track_asso(std::vector > double y2=Z[0]*sin(Z[1]); double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2); -// if(T_track_head == 33855816 && T_point == 33859004 ) -// { -// std::cout << "r_point=" << r_point -// << "h_point=" << h_point -// << "T_point=" << T_point -// << "r_temp_track=" << r_temp_track -// << "v_temp_track=" << v_temp_track -// << "h_temp_track=" << h_temp_track -// << "T_track_head=" << T_track_head -// << "d=" << d -// << "alpha=" << alpha; - -// } - if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8 { struct Asso_info asso_info_tmp; @@ -245,18 +216,22 @@ void Track_Init::point_temp_track_asso(std::vector > { (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][j]); (*temp_track)[(*temp_track).size()-1][j].asso_flag = 0; + (*temp_track)[(*temp_track).size()-1][j].temp_track_id = temp_track_id; } //关联上的点 Temp_track asso_track_info_tmp = {}; + asso_track_info_tmp.temp_track_id = temp_track_id++; + asso_track_info_tmp.point_id = point_process[asso_info[i].point_idx-1].point_id; + asso_track_info_tmp.circle_id = point_process[asso_info[i].point_idx-1].circle_id; asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range; asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth; asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height; - asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude; - asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_time; + asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude; + asso_track_info_tmp.CPI_time = point_process[asso_info[i].point_idx-1].CPI_time; asso_track_info_tmp.GNSS_time = point_process[asso_info[i].point_idx-1].GNSS_time; asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity; - asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].SNR; + asso_track_info_tmp.SNR = point_process[asso_info[i].point_idx-1].SNR; asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; asso_track_info_tmp.asso_flag=0; asso_track_info_tmp.buff_round = 1; @@ -272,7 +247,7 @@ void Track_Init::point_temp_track_asso(std::vector > double X[4]; double P[4][4]; kalman Kalman; - double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].T)/1000.0; + 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); memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); @@ -340,7 +315,7 @@ void Track_Init::point_track_head_asso( std::vector > y_track_head=(*temp_track)[j][0].X[2]; double v_track_head=(*temp_track)[j][0].vr; double h_track_head = (*temp_track)[j][0].height; - double T_track_head = static_cast((*temp_track)[j][0].T); + double T_track_head = static_cast((*temp_track)[j][0].CPI_time); //距离差 double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); @@ -392,17 +367,21 @@ void Track_Init::point_track_head_asso( std::vector > //第一个点 (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]); (*temp_track)[(*temp_track).size()-1][0].asso_flag = 0; + (*temp_track)[(*temp_track).size()-1][0].temp_track_id = temp_track_id; //第二个点 struct Temp_track asso_track_info_tmp = {}; + asso_track_info_tmp.temp_track_id = temp_track_id++; + asso_track_info_tmp.point_id = point_process[asso_info[i].point_idx-1].point_id; + asso_track_info_tmp.circle_id = point_process[asso_info[i].point_idx-1].circle_id; asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range; asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth; asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height; - asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude; + asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude; asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity; - asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].SNR; + asso_track_info_tmp.SNR = point_process[asso_info[i].point_idx-1].SNR; asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; - asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_time; + asso_track_info_tmp.CPI_time = point_process[asso_info[i].point_idx-1].CPI_time; asso_track_info_tmp.GNSS_time = point_process[asso_info[i].point_idx-1].GNSS_time; asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_beam_index; std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); @@ -417,7 +396,7 @@ void Track_Init::point_track_head_asso( std::vector > double X[4]; double P[4][4]; kalman Kalman; - double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].T)/1000.0; + 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); memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); @@ -514,9 +493,9 @@ void Track_Init::tmp_track_to_trust_track(std::vector *trust_tra if (Track_to_start[i].size() < 3 || Track_to_start[j].size() < 3) continue; - if( (Track_to_start[i][0].T == Track_to_start[j][0].T && Track_to_start[i][0].r == Track_to_start[j][0].r) - || (Track_to_start[i][1].T == Track_to_start[j][1].T && Track_to_start[i][1].r == Track_to_start[j][1].r) - || (Track_to_start[i][2].T == Track_to_start[j][2].T && Track_to_start[i][2].r == Track_to_start[j][2].r) + if( (Track_to_start[i][0].CPI_time == Track_to_start[j][0].CPI_time && Track_to_start[i][0].r == Track_to_start[j][0].r) + || (Track_to_start[i][1].CPI_time == Track_to_start[j][1].CPI_time && Track_to_start[i][1].r == Track_to_start[j][1].r) + || (Track_to_start[i][2].CPI_time == Track_to_start[j][2].CPI_time && Track_to_start[i][2].r == Track_to_start[j][2].r) ) { if(Track_to_start[i][L-1].d *trust_tra double Z2[2]={(Track_to_start[i][L-1].r)*cos(Track_to_start[i][L-1].azi),(Track_to_start[i][L-1].r)*sin(Track_to_start[i][L-1].azi) }; double X[6]; double P[6][6]; - double T1=(Track_to_start[i][L-2].T-Track_to_start[i][L-3].T)/1000.0; - double T2=(Track_to_start[i][L-1].T-Track_to_start[i][L-2].T)/1000.0; + 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); @@ -574,13 +553,13 @@ void Track_Init::tmp_track_to_trust_track(std::vector *trust_tra memcpy(trust_track_tmp.X, X, 6*sizeof(double)); memcpy(trust_track_tmp.P, P, 6*6*sizeof(double)); - trust_track_tmp.Amplitude=Track_to_start[i][L-1].Amp; - trust_track_tmp.snr_point = Track_to_start[i][L-1].snr; + trust_track_tmp.Amplitude=Track_to_start[i][L-1].amplitude; + trust_track_tmp.snr_point = Track_to_start[i][L-1].SNR; trust_track_tmp.RCS = Track_to_start[i][L-1].RCS; trust_track_tmp.pitch_num = Track_to_start[i][L-1].pitch_num; std::memcpy(trust_track_tmp.speed_dim, Track_to_start[i][L-1].speed_dim, sizeof(trust_track_tmp.speed_dim)); 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].T; + 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.point_flag=1; //实点 trust_track_tmp.Track_Mode=0; //跟踪模式 TWS 0 @@ -626,8 +605,8 @@ void Track_Init::tmp_track_to_trust_track(std::vector *trust_tra } Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast(sqrt(pow(Track_to_start[i][j].X[1],2)+pow(Track_to_start[i][j].X[3],2))); Trust_Track_Output[*Trust_track_num_Output-1][j].z=static_cast(Track_to_start[i][j].height); - Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast(Track_to_start[i][j].Amp); - 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].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].pitch_num = Track_to_start[i][j].pitch_num; @@ -640,7 +619,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector *trust_tra Direction_Angle=Direction_Angle+2*PI; Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=static_cast(Direction_Angle/PI*180); - Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast((Track_to_start[i][j].T)/1000.0); + Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast((Track_to_start[i][j].CPI_time)/1000.0); Trust_Track_Output[*Trust_track_num_Output-1][j].GNSS_time=Track_to_start[i][j].GNSS_time; Trust_Track_Output[*Trust_track_num_Output-1][j].x=static_cast(Track_to_start[i][j].X[0]); diff --git a/data_process_class_dll/track_init.h b/data_process_class_dll/track_init.h index 7a7d496..44e3aa0 100644 --- a/data_process_class_dll/track_init.h +++ b/data_process_class_dll/track_init.h @@ -27,6 +27,8 @@ public: private: + long long temp_track_id = 0; //临时航迹id + std::vector point_process; //要处理的点迹 void point_temp_track_asso(std::vector > *temp_track, diff --git a/data_process_class_dll/track_init_direct_tracking.cpp b/data_process_class_dll/track_init_direct_tracking.cpp index 20874eb..9178482 100644 --- a/data_process_class_dll/track_init_direct_tracking.cpp +++ b/data_process_class_dll/track_init_direct_tracking.cpp @@ -62,7 +62,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector ((*temp_track)[j][L-1].T); + double T_track_head = static_cast((*temp_track)[j][L-1].CPI_time); double delta_T = (T_point - T_track_head)/1000.0; @@ -114,10 +114,10 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector ((*temp_track)[j][0].T); + double T_track_head = static_cast((*temp_track)[j][0].CPI_time); //距离差 double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); @@ -250,11 +250,11 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector (asin((*Iter)[j].height/(*Iter)[j].r)/PI*180); Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast(sqrt(pow((*Iter)[j].X[1],2)+pow((*Iter)[j].X[3],2))); Trust_Track_Output[*Trust_track_num_Output-1][j].z=static_cast((*Iter)[j].height); - Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast((*Iter)[j].Amp); - Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast((*Iter)[j].snr); + Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast((*Iter)[j].amplitude); + Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast((*Iter)[j].SNR); Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast((*Iter)[j].RCS); double Direction_Angle; Direction_Angle=atan2((*Iter)[j].X[3],(*Iter)[j].X[1]); @@ -384,7 +384,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector(Direction_Angle/PI*180); - Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast(((*Iter)[j].T)/1000.0); + Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast(((*Iter)[j].CPI_time)/1000.0); Trust_Track_Output[*Trust_track_num_Output-1][j].x=static_cast((*Iter)[j].X[0]); Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=static_cast((*Iter)[j].X[1]); Trust_Track_Output[*Trust_track_num_Output-1][j].y=static_cast((*Iter)[j].X[2]);