waiwaylee/radar_data_process
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

更新：1、增加临时航迹录入数据库；

Browse Source 
2、增加点迹信息的圈ID记录；

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-09-30 16:33:18 +08:00
parent a4f32e4036
commit c3c7e668d2
8 changed files with 212 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/track_init.cpp
+47 -68
View File
@@ -6,6 +6,8 @@
#include <vector>
#include <iostream>
#include<iomanip>
#include "rdp_record.h"
#include "rdp_record_entity.h"
using namespace std;
int Track_Init::track_init_process_logic( std::vector <PointRecv> *point_recv, //输入点迹
@@ -15,27 +17,12 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
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;i<temp_track->size();i++)
@@ -78,12 +65,15 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
for (unsigned int i=0;i<point_process.size();i++)
{
Temp_track temp_track_tmp = {};
temp_track_tmp.temp_track_id = temp_track_id++;
temp_track_tmp.point_id = point_process[i].point_id;
temp_track_tmp.circle_id = point_process[i].circle_id;
temp_track_tmp.r = point_process[i].range;
temp_track_tmp.azi = point_process[i].azimuth;
temp_track_tmp.height = point_process[i].height;
temp_track_tmp.Amp = point_process[i].amplitude;
temp_track_tmp.amplitude = point_process[i].amplitude;
temp_track_tmp.vr = point_process[i].velocity;
temp_track_tmp.snr = point_process[i].SNR;
temp_track_tmp.SNR = point_process[i].SNR;
temp_track_tmp.RCS = point_process[i].RCS;
temp_track_tmp.pitch_num = point_process[i].pitch_beam_index;
std::memcpy(temp_track_tmp.speed_dim, point_process[i].speed_dim, sizeof(temp_track_tmp.speed_dim));
@@ -93,7 +83,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
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 <PointRecv>
//消亡临时航迹
tmp_track_die(temp_track);
// std::cout << "-----------------------------------------------";
// for(int i=0;i<temp_track->size();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 <std::vector<Temp_track>>
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<double>((*temp_track)[j][L-1].T);
double T_track_head = static_cast<double>((*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 <std::vector<Temp_track>>
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 && 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
{
struct Asso_info asso_info_tmp;
@@ -245,18 +216,22 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
{
(*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 <std::vector<Temp_track>>
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 <std::vector<Temp_track>>
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<double>((*temp_track)[j][0].T);
double T_track_head = static_cast<double>((*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 <std::vector<Temp_track>>
//第一个点
(*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 <std::vector<Temp_track>>
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_Track> *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<Track_to_start[j][L-1].d)
@@ -565,8 +544,8 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *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_Track> *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_Track> *trust_tra
}
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast<float>(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<float>(Track_to_start[i][j].height);
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>(Track_to_start[i][j].Amp);
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].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].pitch_num = Track_to_start[i][j].pitch_num;
@@ -640,7 +619,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
Direction_Angle=Direction_Angle+2*PI;
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast<float>((Track_to_start[i][j].T)/1000.0);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast<float>((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<float>(Track_to_start[i][j].X[0]);