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

更新：1、数据处理增加版本号输出；

Browse Source 
2、增加Debug模式下可靠航迹、关联信息等数据写入数据库；

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-10-08 16:01:53 +08:00
parent c3c7e668d2
commit b227ae1957
9 changed files with 290 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/track_asso.cpp
+46 -32
View File
@@ -6,6 +6,10 @@
#include <cstring>
#include <vector>
#include <Eigen/Dense>
#include "rdp_record.h"
#include "rdp_record_entity.h"
using namespace Eigen;
using namespace std;
@@ -101,6 +105,11 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
(*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;
//关联点信息
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=static_cast<float>((*trust_track)[i].range_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=static_cast<float>((*trust_track)[i].azi_point);
@@ -143,12 +152,13 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Direction=0;
}
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = static_cast<float>((*trust_track)[i].RCS);
Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;
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));
RDP_RECORD(RdpRecTrustTrack((*trust_track)[i]));
}
}
@@ -422,17 +432,8 @@ void Track_Asso::IMM_d_cal(double v_track,
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct RadarPara Work_Parameter)
{
//存储关联信息
struct asso_info
{
double d1;
double d2;
double d3;
double d_min;
int point_index;
int track_index;
};
std::vector <asso_info> associated_info;
std::vector <Asso_Info> associated_info;
//遍历所有点迹航迹 计算点迹和航迹的统计距离
for (size_t loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
@@ -518,28 +519,38 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
thd_r = false;
}
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<int>(loop_of_point)+1;
associated_info_tmp.track_index=static_cast<int>(loop_of_track)+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_tmp.point_id=point_process[loop_of_point].point_id;
associated_info_tmp.track_id=(*trust_track)[loop_of_track].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=R;
associated_info_tmp.VT=VT;
associated_info_tmp.thd_d=thd_d;
associated_info_tmp.thd_h=thd_h;
associated_info_tmp.thd_r=thd_r;
associated_info_tmp.min_d_flag=false;
//小于关联门限 保存关联信息
if(thd_d && thd_h && thd_r)
{
//std::cout << "asooooooooooo";
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<int>(loop_of_point)+1;
associated_info_tmp.track_index=static_cast<int>(loop_of_track)+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));
}
}
// }
}
//最近邻法关联
@@ -582,11 +593,10 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
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);
if (delta_T <= 0)
if (delta_T > 0)
{
// RDP_LOG<<"delta_T <= 0, track_index: "<<track_index<<", point_index: "<<point_index<<", delta_T: "<<delta_T<<std::endl;
// 乱序/重复时间戳：不作为有效关联，不标记点迹已使用、不刷新航迹新鲜度
}else{
associated_info[min_index-1].min_d_flag=true;
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
kalman Kalman;
@@ -649,6 +659,8 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
(*trust_track)[track_index-1].GNSS_time = point_process[point_index-1].GNSS_time;
//更新关联上的点迹信息
(*trust_track)[track_index-1].point_id = point_process[point_index-1].point_id;
(*trust_track)[track_index-1].circle_id = point_process[point_index-1].circle_id;
(*trust_track)[track_index-1].range_point=point_process[point_index-1].range;
(*trust_track)[track_index-1].azi_point=point_process[point_index-1].azimuth/PI*180;
(*trust_track)[track_index-1].elev_point= asin(point_process[point_index-1].height/point_process[point_index-1].range)/PI*180;
@@ -681,6 +693,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
{
if(associated_info[ii].track_index==track_index)
{
RDP_RECORD(RdpRecAssoInfo(associated_info[ii]));
associated_info.erase(associated_info.begin()+ii);
associated_num=static_cast<int>(associated_info.size());
}
@@ -692,6 +705,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
{
if(associated_info[ii].point_index==point_index)
{
RDP_RECORD(RdpRecAssoInfo(associated_info[ii]));
associated_info.erase(associated_info.begin()+ii);
associated_num=static_cast<int>(associated_info.size());
}