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

更新：1、航迹关联时增加高度门限判断，同时加大统计距离门限，提高机动目标关联概率；

Browse Source 
2、修复TAS波束控制时，数据率判断有误的问题；
3、更新文档；

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-09-22 09:20:12 +08:00
parent a76da34756
commit e0bf227be3
9 changed files with 237 additions and 5070 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/parameters.h
+2 -2
View File
@@ -52,7 +52,7 @@
#define TRACK_DIE_ROUND 4 // 航迹消亡时间
#define TRACK_DIE_ROUND_TAS 96 //
#define ASSO_THORD 3
#define ASSO_THORD 4
#define H_F_WIN_LEN 3
@@ -101,7 +101,7 @@
#define TRACK_DIE_ROUND 4 // 航迹消亡时间
#define TRACK_DIE_ROUND_TAS 5
#define ASSO_THORD 3
#define ASSO_THORD 4
#define H_F_WIN_LEN 3
data_process_class_dll/tas_ctrl.cpp
+6 -5
View File
@@ -263,6 +263,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
long long CPI_time = 0;
long long earliest_CPI_time = LLONG_MAX;
float delta_T = 0;
float delta_T_last_track = 0;
double H_track = 0;
int earliest_track_idx=-1;
int earliest_tas_queue_idx=-1;
@@ -281,13 +282,14 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
if((*trust_track)[track_idx].Track_Index == tas_target_queue[tas_idx].Index)
{
CPI_time = (*trust_track)[track_idx].T_track;
delta_T = (latest_timestamp - CPI_time) / 1000.0f;
//查找最早的CPI时间, 航迹时间与当前最新时间戳的差值大于 DATA_RATE_TAS 时才输出TAS跟踪波束
if(CPI_time < earliest_CPI_time && (latest_timestamp - tas_target_queue[tas_idx].last_track_time) / 1000.0f > Work_Parameter.DATA_RATE_TAS && delta_T > Work_Parameter.DATA_RATE_TAS)
delta_T_last_track = (latest_timestamp - tas_target_queue[tas_idx].last_track_time) / 1000.0f;
//查找CPI时间最早，且满足TAS数据率的目标
if(CPI_time < earliest_CPI_time && (delta_T_last_track > Work_Parameter.DATA_RATE_TAS))
{
earliest_CPI_time = CPI_time;
H_track = (*trust_track)[track_idx].Height;
delta_T = (latest_timestamp - CPI_time) / 1000.0f;
earliest_track_idx = track_idx;
earliest_tas_queue_idx = tas_idx;
@@ -295,7 +297,6 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
for(int ii=0;ii<6;ii++){
X_now[ii]=(*trust_track)[track_idx].X[ii];
}
}
break;
data_process_class_dll/track_asso.cpp
+95 -144
View File
@@ -417,157 +417,108 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
//遍历所有点迹航迹 计算点迹和航迹的统计距离
for (size_t loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
{
// if((*trust_track)[loop_of_track].manual_tracking_flag==0)
// {
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
for (size_t loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
{
for (size_t loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
{
//时间差
double delta_T = (point_process[loop_of_point].CPI_Time - (*trust_track)[loop_of_track].T_track)/1000.0;
//航迹X P
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
memcpy(X3,(*trust_track)[loop_of_track].X3,6*sizeof(double));
memcpy(P1,(*trust_track)[loop_of_track].P1,6*6*sizeof(double));
memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
double v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
double h_track = (*trust_track)[loop_of_track].Height;
double r_track = sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));
//时间差
double delta_T = (point_process[loop_of_point].CPI_Time - (*trust_track)[loop_of_track].T_track)/1000.0;
//航迹X P
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
memcpy(X3,(*trust_track)[loop_of_track].X3,6*sizeof(double));
memcpy(P1,(*trust_track)[loop_of_track].P1,6*6*sizeof(double));
memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
double v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
double h_track = (*trust_track)[loop_of_track].Height;
double r_track = sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));
//量测信息
double Z[3] = {point_process[loop_of_point].Range,
point_process[loop_of_point].Azimuth,
point_process[loop_of_point].Velocity};
double prt = point_process[loop_of_point].PRF_index;
double freq_ind = point_process[loop_of_point].Freq_index;
double h_point = point_process[loop_of_point].Height;
//量测信息
double Z[3] = {point_process[loop_of_point].Range,
point_process[loop_of_point].Azimuth,
point_process[loop_of_point].Velocity};
double prt = point_process[loop_of_point].PRF_index;
double freq_ind = point_process[loop_of_point].Freq_index;
double h_point = point_process[loop_of_point].Height;
//计算统计距离d
double d1,d2,d3;
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
//计算统计距离d
double d1,d2,d3;
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
//计算高度门限
// double d_h;
// if (r_track<1000)
// {
// if(abs(h_track-h_point)<=150)
// {
// d_h = 1;
// }
// else
// {
// d_h = 0;
// }
//判定统计距离门限，thd_d
bool thd_d = false;
// }
// else if (r_track<2000 && r_track>=1000)
// {
// if(abs(h_track-h_point)<=200)
// {
// d_h = 1;
// }
// else
// {
// d_h = 0;
// }
// }
// else if(r_track>=2000 && r_track<3000 )
// {
// if(abs(h_track-h_point)<=200)
// {
// d_h = 1;
// }
// else
// {
// d_h = 0;
// }
// }
// else if(r_track>=3000 && r_track<5000)
// {
// if(abs(h_track-h_point)<=300)
// {
// d_h = 1;
// }
// else
// {
// d_h = 0;
// }
// }
// else
// {
// d_h = 1;
// }
// std::cout << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// std::cout << h_track<<" "<<h_point<<" "<<d_h;
// d_h = 1;
//计算俯仰门限
bool d_p = true;
// double p_track = atan2(h_track, r_track); //航迹俯仰
// double p_point = asin(h_point/point_process[loop_of_point].Range); //点迹俯仰
// if( (r_track > 3000 && abs(p_track - p_point)*180/PI <= 7) || \
// (r_track > 1000 && r_track <= 3000 && abs(p_track - p_point)*180/PI <= 5) || \
// (r_track <= 1000 && abs(p_track - p_point)*180/PI <= 10)){
// d_p = true;
// }
//计算距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差
double& t_x0 = (*trust_track)[loop_of_track].X[0];
double& t_y0 = (*trust_track)[loop_of_track].X[3];
double a_track = atan2(t_y0, t_x0); //航迹方位角
double& r_point = point_process[loop_of_point].Range; //点迹距离
double a_delta = abs(a_track - point_process[loop_of_point].Azimuth); //方位差
if(a_delta > PI){
a_delta = 2*PI - a_delta;
}
double R = sqrt(r_track * r_track + r_point*r_point - 2*r_track*r_point*cos(a_delta));
double VT = v_track*delta_T;
bool d_r = true;
if(VT < 100 && R > 300){
//VT<100m，使用固定门限300m
d_r = false;
}else if((VT>=100 && VT<=300 && R/VT>3)){
d_r = false;
}else if(VT>300 && R > 1200){
d_r = false;
}
//小于关联门限 保存关联信息
if(
( r_track<=500 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000.0) && d_p && d_r)//
||( (r_track>500&&r_track<1000) && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/100.0) && d_p && d_r)//
||( r_track>=1000 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD) && d_p && d_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);
}
if( (r_track <= 500 && \
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 1000.0)) || \
((r_track > 500 && r_track < 1000) && \
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 100.0)) || \
(r_track >= 1000 && \
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD))\
){
thd_d = true;
}
//判定高度门限，thd_h
bool thd_h = false;
double delta_h = abs(h_track-h_point);
if( (r_track <= 1000 && delta_h <= 150) || \
((r_track > 1000 && r_track <= 2000) && delta_h <= 200) || \
((r_track > 2000 && r_track <= 3000) && delta_h <= 250) || \
((r_track > 3000 && r_track <= 5000) && delta_h <= 300) || \
r_track > 5000
){
thd_h = true;
}
//判定距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差
double& t_x0 = (*trust_track)[loop_of_track].X[0];
double& t_y0 = (*trust_track)[loop_of_track].X[3];
double a_track = atan2(t_y0, t_x0); //航迹方位角
double& r_point = point_process[loop_of_point].Range; //点迹距离
double a_delta = abs(a_track - point_process[loop_of_point].Azimuth); //方位差
if(a_delta > PI){
a_delta = 2*PI - a_delta;
}
double R = sqrt(r_track * r_track + r_point*r_point - 2*r_track*r_point*cos(a_delta));
double VT = v_track*delta_T;
bool thd_r = true;
if(VT < 100 && R > 300){
//VT<100m，使用固定门限300m
thd_r = false;
}else if((VT>=100 && VT<=300 && R/VT>3)){
thd_r = false;
}else if(VT>300 && R > 1200){
thd_r = 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);
}
}
// }
}
data_process_class_dll/track_asso_tas.cpp
+17 -54
View File
@@ -289,65 +289,28 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
double d1,d2,d3;
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
//计算高度门限
double d_h;
if (r_track<1000)
{
if(abs(h_track-h_point)<=150)
{
d_h = 1;
}
else
{
d_h = 0;
}
//判定统计距离门限，thd_d
bool thd_d = false;
}
else if (r_track<2000 && r_track>=1000)
{
if(abs(h_track-h_point)<=200)
{
d_h = 1;
}
else
{
d_h = 0;
}
}
else if(r_track>=2000 && r_track<3000 )
{
if(abs(h_track-h_point)<=200)
{
d_h = 1;
}
else
{
d_h = 0;
}
}
else if(r_track>=3000 && r_track<5000)
{
if(abs(h_track-h_point)<=300)
{
d_h = 1;
}
else
{
d_h = 0;
}
}
else
{
d_h = 1;
if(d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD){
thd_d = true;
}
// std::cout << "T d" <<point_process[loop_of_point].CPI_Time/1000<<" "<<T_track/1000<<" "<<delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// std::cout << "h_track" <<h_track<<" h_point"<<h_point<<" d_h"<<d_h;
//判定高度门限，thd_h
bool thd_h = false;
double delta_h = abs(h_track-h_point);
if( (r_track <= 1000 && delta_h <= 150) || \
((r_track > 1000 && r_track <= 2000) && delta_h <= 200) || \
((r_track > 2000 && r_track <= 3000) && delta_h <= 250) || \
((r_track > 3000 && r_track <= 5000) && delta_h <= 300) || \
r_track > 5000
){
thd_h = true;
}
//小于关联门限 保存关联信息
if((d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD)
&& d_h == 1)//
if(thd_d && thd_h)
{
// std::cout << "TAS asoooooo111" ;