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

更新：1、修复kalman.cpp中部分参数命名冲突的问题；

Browse Source 
2、TWS点迹关联时增加点航距离门限，限制部分突然关联到很远的点迹的问题。

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-06-18 09:16:04 +08:00
parent 167fa793e9
commit 958ff6b11b
38 changed files with 8774 additions and 1775 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/tas_ctrl.cpp
+104 -109
View File
@@ -18,8 +18,7 @@ TAS_Ctrl::TAS_Ctrl()
tas_target_num=0;
}
void TAS_Ctrl::tas_ctrl_process(QVector <QVector<Trust_Track>> *trust_track,
void TAS_Ctrl::tas_ctrl_process(QVector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
@@ -43,27 +42,26 @@ void TAS_Ctrl::tas_ctrl_process(QVector <QVector<Trust_Track>> *trust_track,
};
void TAS_Ctrl::tas_target_add(QVector <QVector<Trust_Track>> *trust_track,
void TAS_Ctrl::tas_target_add(QVector<Trust_Track> *trust_track,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
{
for (int i=0;i<trust_track->size();i++)
for (int k=0;k<(*trust_track)[i].size();k++)
{
float r=sqrt(pow((*trust_track)[i][k].X[0],2)+pow((*trust_track)[i][k].X[3],2));
float v=sqrt(pow((*trust_track)[i][k].X[1],2)+pow((*trust_track)[i][k].X[4],2));
float h=(*trust_track)[i][k].Height+Work_Parameter.Height;
float azi = atan2((*trust_track)[i][k].X[3],(*trust_track)[i][k].X[0]);
double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
double h=(*trust_track)[i].Height+Work_Parameter.Height;
double azi = atan2((*trust_track)[i].X[3],(*trust_track)[i].X[0]);
if(azi<0)
azi=azi+2*PI;
azi=azi/PI*180;
//进入跟踪的条件： 1.手动跟踪的目标 或 满足速度、距离、高度条件满足 关联点数大于2 2.不在禁止跟踪区域
if( ( (*trust_track)[i][k].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, Work_Parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, Work_Parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
&& tas_target_num < MAX_TAS_NUM
&& (*trust_track)[i][k].Track_Mode == 0
&& (*trust_track)[i].Track_Mode == 0
&& tas_prohibite_area( r, azi, v, h, Work_Parameter) == 0)
{
for(int j=0;j<TAS_QUEUE_LENGTH;j++)
@@ -71,33 +69,33 @@ void TAS_Ctrl::tas_target_add(QVector <QVector<Trust_Track>> *trust_track,
if(tas_target_queue[j].empty_flag==0 && j%2==0)
{
//插入跟踪队列
tas_target_queue[j].Index=(*trust_track)[i][k].Track_Index;
tas_target_queue[j].Index=(*trust_track)[i].Track_Index;
tas_target_queue[j].empty_flag=1;
//跟踪目标数目加一
tas_target_num=tas_target_num+1;
//TAS跟踪标志置1
(*trust_track)[i][k].Track_Mode=1;
(*trust_track)[i].Track_Mode=1;
//目标跟踪状态发生改变 对外输出航迹更新信息
*Trust_track_num_Output= *Trust_track_num_Output+1;
float r_output,azmi_output;
double r_output,azmi_output;
coor_trans Coor_trans;
Coor_trans.cart2polar((*trust_track)[i][k].X[0],(*trust_track)[i][k].X[3],&r_output,&azmi_output);
Coor_trans.cart2polar((*trust_track)[i].X[0],(*trust_track)[i].X[3],&r_output,&azmi_output);
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i][k].Track_Index; //航迹号
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index; //航迹号
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=
sqrt((*trust_track)[i][k].X[1]*(*trust_track)[i][k].X[1]+(*trust_track)[i][k].X[4]*(*trust_track)[i][k].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i][k].Amplitude;
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i][k].point_flag;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i][k].Track_Mode;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i][k].Height/r_output)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i][k].Height+Work_Parameter.Height;
Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i][k].Target_Type;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i][k].snr_point;
sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i].Amplitude;
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i].point_flag;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i].Height/r_output)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height;
Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
break;
}
@@ -111,28 +109,28 @@ void TAS_Ctrl::tas_target_add(QVector <QVector<Trust_Track>> *trust_track,
int TAS_Ctrl:: tas_auto_start(float v, float r, float h, struct RadarPara Work_Parameter)
{
//int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter)
//{
// if(r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) )
// )
if( r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
&& ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) ) )
{
return 1;
}
else
{
return 0;
}
//// if(r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
//// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) )
//// )
// if( r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) ) )
// {
// return 1;
// }
// else
// {
// return 0;
// }
// return 0;
//// return 0;
};
//};
int TAS_Ctrl::tas_prohibite_area(float r,float azi, float v, float h,struct RadarPara Work_Parameter)
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter)
{
for (int i=0;i<Work_Parameter.TAS_prohibite_area_num;i++)
@@ -148,7 +146,7 @@ int TAS_Ctrl::tas_prohibite_area(float r,float azi, float v, float h,struct Rad
}
void TAS_Ctrl::tas_target_del(QVector <QVector<Trust_Track>> *trust_track,
void TAS_Ctrl::tas_target_del(QVector<Trust_Track> *trust_track,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
@@ -162,9 +160,8 @@ void TAS_Ctrl::tas_target_del(QVector <QVector<Trust_Track>> *trust_track,
//查找TAS队列里的目标是否存在
int flag=0;
for (int j=0;j<trust_track->size();j++)
for (int k=0;k<(*trust_track)[j].size();k++)
{
if(tas_target_queue[i].Index==(*trust_track)[j][k].Track_Index && (*trust_track)[j][k].Track_Mode == 1)
if(tas_target_queue[i].Index==(*trust_track)[j].Track_Index && (*trust_track)[j].Track_Mode == 1)
{
flag=1;
}
@@ -179,104 +176,102 @@ void TAS_Ctrl::tas_target_del(QVector <QVector<Trust_Track>> *trust_track,
}
//判断TAS目标是否满足自动跟踪条件 若不满足移除TAS队列 通知界面改变目标状态
for ( int i=0;i<trust_track->size();i++)
for (int j=0;j<(*trust_track)[i].size();j++)
{
float r=sqrt(pow((*trust_track)[i][j].X[0],2)+pow((*trust_track)[i][j].X[3],2));
float v=sqrt(pow((*trust_track)[i][j].X[1],2)+pow((*trust_track)[i][j].X[4],2));
float h=(*trust_track)[i][j].Height+Work_Parameter.Height;
// for ( int i=0;i<trust_track->size();i++)
// {
// double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
// double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
// double h=(*trust_track)[i].Height+Work_Parameter.Height;
if((*trust_track)[i][j].manual_tracking_flag == 0 && (*trust_track)[i][j].Track_Mode == 1 && tas_auto_end(v,r,h,Work_Parameter) == 1) //满足结束跟踪条件
{
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h,Work_Parameter) == 1) //满足结束跟踪条件
// {
int track_index=(*trust_track)[i][j].Track_Index;
// int track_index=(*trust_track)[i].Track_Index;
for (int ii=0;ii<TAS_QUEUE_LENGTH;ii++)
{
if(tas_target_queue[ii].Index == track_index)
{
//从队列中删除
memset(&tas_target_queue[ii],0,sizeof(Tracking_Target));
tas_target_num=tas_target_num-1;
// for (int ii=0;ii<TAS_QUEUE_LENGTH;ii++)
// {
// if(tas_target_queue[ii].Index == track_index)
// {
// //从队列中删除
// memset(&tas_target_queue[ii],0,sizeof(Tracking_Target));
// tas_target_num=tas_target_num-1;
//TAS跟踪标志置零
(*trust_track)[i][j].Track_Mode=0;
// //TAS跟踪标志置零
// (*trust_track)[i].Track_Mode=0;
//目标类型设为地面目标
(*trust_track)[i][j].Target_Type = 0;
// //目标类型设为地面目标
// (*trust_track)[i].Target_Type = 0;
//航迹状态发生改变 对外输出航迹更新信息
*Trust_track_num_Output= *Trust_track_num_Output+1;
float r_output,azmi_output;
coor_trans Coor_trans;
Coor_trans.cart2polar((*trust_track)[i][j].X[0],(*trust_track)[i][j].X[3],&r_output,&azmi_output);
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i][j].Track_Index; //航迹号
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=sqrt((*trust_track)[i][j].X[1]*(*trust_track)[i][j].X[1]+(*trust_track)[i][j].X[4]*(*trust_track)[i][j].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i][j].Amplitude;
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i][j].point_flag;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i][j].Track_Mode;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i][j].Height/r_output)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i][j].Height+Work_Parameter.Height;
Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i][j].Target_Type;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i][j].snr_point;
}
}
}
// //航迹状态发生改变 对外输出航迹更新信息
// *Trust_track_num_Output= *Trust_track_num_Output+1;
// double r_output,azmi_output;
// coor_trans Coor_trans;
// Coor_trans.cart2polar((*trust_track)[i].X[0],(*trust_track)[i].X[3],&r_output,&azmi_output);
// Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index; //航迹号
// Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
// Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i].Amplitude;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i].point_flag;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i].Height/r_output)/PI*180;
// Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
// Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
// }
// }
// }
}
// }
};
int TAS_Ctrl::tas_auto_end(float v, float r, float h, struct RadarPara Work_Parameter)
{
if((r<=1000 && h<Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h<Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h<Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h<Work_Parameter.TAS_Height_4) || (r>4000 && h<Work_Parameter.TAS_Height_5)
|| v<Work_Parameter.V_TAS_min || v>Work_Parameter.V_TAS_max
)
{
return 1;
//int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter)
//{
// if((r<=1000 && h<Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h<Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h<Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h<Work_Parameter.TAS_Height_4) || (r>4000 && h<Work_Parameter.TAS_Height_5)
// || v<Work_Parameter.V_TAS_min || v>Work_Parameter.V_TAS_max
// )
// {
// return 1;
}
else
{
// }
// else
// {
return 0;
}
// return 0;
// }
return 0;
// return 0;
};
//};
void TAS_Ctrl::tas_beam_output(QVector <QVector<Trust_Track>> *trust_track,
void TAS_Ctrl::tas_beam_output(QVector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam)
{
if(tas_target_queue[0].empty_flag==1)
{
float H_track;
float X_now[6];
double H_track;
double X_now[6];
for (int i=0;i<trust_track->size();i++ )
for (int j=0;j<(*trust_track)[i].size();j++)
{
if((*trust_track)[i][j].Track_Index == tas_target_queue[0].Index)
if((*trust_track)[i].Track_Index == tas_target_queue[0].Index)
{
H_track = (*trust_track)[i][j].Height;
H_track = (*trust_track)[i].Height;
for(int ii=0;ii<6;ii++)
X_now[ii]=(*trust_track)[i][j].X[ii];
X_now[ii]=(*trust_track)[i].X[ii];
}
}
//预测目标位置 计算跟踪波束波位号 俯仰角
float x_track=X_now[0]+X_now[1]*T_TAS_PRED;
float y_track=X_now[3]+X_now[4]*T_TAS_PRED;
float amzi,range;
double x_track=X_now[0]+X_now[1]*T_TAS_PRED;
double y_track=X_now[3]+X_now[4]*T_TAS_PRED;
double amzi,range;
coor_trans Coor_trans;
Coor_trans.cart2polar(x_track,y_track,&range,&amzi);
@@ -286,7 +281,7 @@ void TAS_Ctrl::tas_beam_output(QVector <QVector<Trust_Track>> *trust_track,
//目标方位
Tracking_beam->Azi=amzi/PI*180;
//目标俯仰角
float elev=asin(H_track/range)/PI*180;
double elev=asin(H_track/range)/PI*180;
if(elev<=0)