waiwaylee/radar_data_process
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
7c10ae919c247483cdac789585d6e3e7f1706a70
radar_data_process/data_process_class_dll/track_init.cpp
T
waiwaylee 958ff6b11b 更新：1、修复kalman.cpp中部分参数命名冲突的问题； 
2、TWS点迹关联时增加点航距离门限，限制部分突然关联到很远的点迹的问题。

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
2026-06-18 09:16:04 +08:00

775 lines
32 KiB
C++
Raw Blame History

#include "track_init.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <iostream>
#include<iomanip>
using namespace std;
int Track_Init::track_init_process_logic( QVector <PointRecv> *point_recv, //输入点迹
QVector<Trust_Track> *trust_track, //可靠航迹
QVector <QVector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
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)
// {
// qDebug() << "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 (int i=0;i<(*point_recv).size();i++)
point_process.push_back((*point_recv)[i]);
//临时航迹的buff_round+1
for (int i=0;i<temp_track->size();i++)
{
for (int j=0;j<(*temp_track)[i].size();j++)
{
(*temp_track)[i][j].buff_round = (*temp_track)[i][j].buff_round+1;
}
}
if(point_process.size()>0)
{
//点迹与临时航迹关联
point_temp_track_asso(temp_track,Work_Parameter);
//点迹与航迹头关联
point_track_head_asso(temp_track,Work_Parameter);
//删除关联上的点迹
QVector <PointRecv>::iterator Iter;
for (Iter=point_process.begin(); Iter!=point_process.end();)
{
if((*Iter).Use_Flag==1)
{
point_process.erase(Iter);
Iter=point_process.begin();
}
else
{
Iter++;
}
}
//剩余点重新存入点迹
QVector<PointRecv>().swap((*point_recv));
for (int i=0;i<point_process.size();i++)
(*point_recv).push_back(point_process[i]);
//剩余点转为航迹头
for (unsigned int i=0;i<point_process.size();i++)
{
Temp_track temp_track_tmp;
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.vr = point_process[i].Velocity;
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_num;
std::memcpy(temp_track_tmp.speed_dim, point_process[i].speed_dim, sizeof(temp_track_tmp.speed_dim));
std::memcpy(temp_track_tmp.range_dim, point_process[i].range_dim, sizeof(temp_track_tmp.range_dim));
temp_track_tmp.asso_flag = 0;
temp_track_tmp.X[0]=point_process[i].Range*cos(point_process[i].Azimuth);
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.buff_round = 1;
// temp_track_tmp.Temp_track_section_idx = floor(point_process[i].beam_index/BEAM_NUM_DOT_SECTION)+1;
temp_track->push_back( QVector <Temp_track> ());
int n=temp_track->size();
(*temp_track)[n-1].push_back(temp_track_tmp);
// if(point_process[i].CPI_Time == 30675948 || point_process[i].CPI_Time == 30682320)
// {
// qDebug() << "point_process_azi=" << point_process[i].Azimuth
// << "point_process_dis=" << point_process[i].Range
// << "point_process_h=" << point_process[i].Height
// << "point_process_v=" << point_process[i].Velocity
// << "point_process_wave=" << point_process[i].beam_index
// << "point_process_time=" << point_process[i].CPI_Time;
// }
}
//清空点迹
QVector<PointRecv>().swap(point_process);
QVector<PointRecv>().swap((*point_recv));
//临时航迹满足起始长度 转为可靠航迹
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
}
//消亡临时航迹
tmp_track_die(temp_track);
// qDebug() << "-----------------------------------------------";
// for(int i=0;i<temp_track->size();i++)
// {
// for(int j=0;j<(*temp_track)[i].size();j++)
// {
// qDebug() << "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;
// }
// }
return 0;
}
void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
{
//关联信息
struct Asso_info
{
int track_idx;
int point_idx;
double d;
};
QVector <Asso_info> asso_info;
for ( int i=0;i<point_process.size();i++)
{
for ( int j=0;j<(*temp_track).size();j++)
{
int L = (*temp_track)[j].size();
if( (*temp_track)[j].size() > 1 && (*temp_track)[j][L-1].buff_round >= 2)
{
//点迹信息
// double Z[2]={ point_process[i].Range*cos(point_process[i].Azimuth),point_process[i].Range*sin(point_process[i].Azimuth)} ;
double Z[3]={point_process[i].Range,
point_process[i].Azimuth,
point_process[i].Velocity};
double vr_point=point_process[i].Velocity;
double r_point=point_process[i].Range;
double prt = point_process[i].PRF_index;
double freq_ind = point_process[i].Freq_index;
double h_point = point_process[i].Height;
double T_point = point_process[i].CPI_Time;
//航迹信息
double X[4];
double P[4][4];
memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(double));
memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(double));
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 = (*temp_track)[j][L-1].T;
double delta_T = (T_point - T_track_head)/1000.0;
if( fabs(r_point-r_temp_track)>=5 )
{
//计算d
kalman Kalman;
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
//计算夹角
double x0=(*temp_track)[j][L-2].X[0];
double y0=(*temp_track)[j][L-2].X[2];
double x1=(*temp_track)[j][L-1].X[0];
double y1=(*temp_track)[j][L-1].X[2];
double x2=Z[0]*cos(Z[1]);
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 )
// {
// qDebug() << "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;
asso_info_tmp.point_idx = i+1;
asso_info_tmp.track_idx = j+1;
asso_info_tmp.d = d;
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][L-1].asso_flag = 1;
point_process[i].Use_Flag = 1;
}
}
}
}
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(QVector <Temp_track> ());
//前L个点
int L = (*temp_track)[asso_info[i].track_idx-1].size();
for (int j=0;j < L;j++ )
{
(*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 asso_track_info_tmp;
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.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.RCS = point_process[asso_info[i].point_idx-1].RCS;
asso_track_info_tmp.asso_flag=0;
asso_track_info_tmp.buff_round = 1;
asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num;
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));
std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim));
asso_track_info_tmp.d = asso_info[i].d;
// asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;
double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][L-1].r*cos((*temp_track)[asso_info[i].track_idx-1][L-1].azi),
(*temp_track)[asso_info[i].track_idx-1][L-1].r*sin((*temp_track)[asso_info[i].track_idx-1][L-1].azi)};
double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth),
point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)};
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;
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
}
//temp_track中删除关联上的临时航迹
QVector <QVector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
if((*Iter).size()>=2)
{
if((*Iter)[(*Iter).size()-1].asso_flag==1)
{
temp_track->erase(Iter);
Iter=temp_track->begin();
}
else
{
Iter++;
}
}
else
{
Iter++;
}
}
}
void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
{
//关联上的信息
struct Asso_info
{
int track_idx;
int point_idx;
};
QVector <Asso_info> asso_info;
//关联
for ( int i=0;i<point_process.size();i++)
{
for ( int j=0;j<temp_track->size();j++)
{
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
{
//点迹信息
double x_point, y_point, vr_point;
coor_trans Coor_trans;
Coor_trans.polar2cart(&x_point,&y_point,point_process[i].Range,point_process[i].Azimuth);
vr_point=point_process[i].Velocity;
double r_point=point_process[i].Range;
double h_point = point_process[i].Height;
double T_point = point_process[i].CPI_Time;
//航迹信息
double x_track_head, y_track_head;
x_track_head=(*temp_track)[j][0].X[0];
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 = (*temp_track)[j][0].T;
// if(T_track_head == 30675948)
// {
// qDebug() << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
// << "y_track_head=" << y_track_head
// << "v_track_head=" << v_track_head
// << "T_track_head=" << T_track_head;
// }
// if(T_point == 30682320)
// {
// qDebug() <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
// qDebug() << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
// << "y_track_head=" << y_track_head
// << "v_track_head=" << v_track_head
// << "T_track_head=" << T_track_head;
// }
//距离差
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
double vmax;
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
{
vmax = Work_Parameter.V_MAX;
}
else //中远程模式 最大速度正常用
{
vmax = Work_Parameter.V_MAX;
}
double delta_T = (T_point - T_track_head)/1000.0;
//满足关联条件的点航
// if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=V_MIN*delta_T/2.0) )
// && vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E
if(
( (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
|| (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
)
&& vr_point*v_track_head>0 )
{
// if(T_track_head == 30675948)
// {
// qDebug() << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
// << "y_track_head=" << y_track_head
// << "v_track_head=" << v_track_head
// << "T_track_head=" << T_track_head;
// }
struct Asso_info asso_info_tmp;
asso_info_tmp.point_idx = i+1;
asso_info_tmp.track_idx = j+1;
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][0].asso_flag = 1;
point_process[i].Use_Flag = 1;
}
}
}
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(QVector <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;
//第二个点
struct Temp_track asso_track_info_tmp;
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.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.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.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num;
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));
std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim));
asso_track_info_tmp.asso_flag = 0;
asso_track_info_tmp.buff_round = 1;
// asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;;
double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][0].r*cos((*temp_track)[asso_info[i].track_idx-1][0].azi),
(*temp_track)[asso_info[i].track_idx-1][0].r*sin((*temp_track)[asso_info[i].track_idx-1][0].azi)};
double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth),
point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)};
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;
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
}
//temp_track中删除关联上的航迹头
QVector <QVector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
if((*Iter)[0].asso_flag==1)
{
temp_track->erase(Iter);
Iter=temp_track->begin();
}
else
{
Iter++;
}
}
}
////////////////////////////////////////////计算三点间的夹角////////////////////////////////////////////////
double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2)
{
double D12[2]={x1-x0,y1-y0};
double D23[2]={x2-x1,y2-y1};
double alpha=acos((D12[0]*D23[0]+D12[1]*D23[1])/(sqrt(D12[0]*D12[0]+D12[1]*D12[1])*sqrt(D23[0]*D23[0]+D23[1]*D23[1])))/PI*180;
return alpha;
}
void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
QVector <QVector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
{
QVector <QVector<Temp_track>> Track_to_start;
// 1. 将temp_track中满足条件的航迹取出, 放到Track_to_start中
QVector <QVector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
int L=(*Iter).size();
double range=(*Iter)[L-1].r;
double azi=(*Iter)[L-1].azi/PI*180;
if( ( L==Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0) //按长度查找TRUST_TRACK_POINT
{
//加入Track_to_start中
Track_to_start.push_back(QVector <Temp_track> ());
for (int i = 0; i<L; i++)
{
Track_to_start[Track_to_start.size()-1].push_back((*Iter)[i]);
}
temp_track->erase(Iter);
Iter=temp_track->begin();
}
else
{
Iter++;
}
}
//2.两两比较Track_to_start中的航迹信息，删除重复的航迹
int L = Work_Parameter.track_start_point_num;
for (int i=0; i<Track_to_start.size()-1;i++)
{
if(Track_to_start[i][0].asso_flag!=2)
{
for (int j=i+1;j<Track_to_start.size();j++)
{
if(Track_to_start[j][0].asso_flag!=2)
{
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][L-1].d<Track_to_start[j][L-1].d)
{
Track_to_start[j][0].asso_flag=2;
}
else
{
Track_to_start[i][0].asso_flag=2;
}
}
}
}
}
}
QVector <QVector<Temp_track>>::iterator Iter1;
for (Iter1=Track_to_start.begin(); Iter1!=Track_to_start.end();)
{
if( (*Iter1)[0].asso_flag == 2 )
{
Track_to_start.erase(Iter1);
Iter1=Track_to_start.begin();
}
else
{
Iter1++;
}
}
//3.Track_to_start中剩余的航迹起始为可靠航迹
for (int i=0; i<Track_to_start.size();i++)
{
if(trust_track->size()<MAX_TRACK_INDEX)
{
int index = track_index_mangement.track_ind_get(trust_track);
if(index>0)
{
//加入本地航迹文件
double Z0[2]={(Track_to_start[i][L-3].r)*cos(Track_to_start[i][L-3].azi),(Track_to_start[i][L-3].r)*sin(Track_to_start[i][L-3].azi) };
double Z1[2]={(Track_to_start[i][L-2].r)*cos(Track_to_start[i][L-2].azi),(Track_to_start[i][L-2].r)*sin(Track_to_start[i][L-2].azi) };
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;
kalman Kalman;
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
Trust_Track trust_track_tmp;
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.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.point_flag=1; //实点
trust_track_tmp.Track_Mode=0; //跟踪模式 TWS 0
trust_track_tmp.Target_Type=UNCONF_TARGET;
trust_track_tmp.Track_Index=index;
trust_track_tmp.Height=Track_to_start[i][L-1].height;
trust_track_tmp.Extrapolate_round=0;
trust_track_tmp.approach_flag = -1;
trust_track_tmp.manual_delete_flag = 0;
trust_track_tmp.associate_point_number = 0;
trust_track_tmp.manual_tracking_flag = 0;
trust_track_tmp.point_type = 0;
trust_track_tmp.u[0]=0.3333;
trust_track_tmp.u[1]=0.3333;
trust_track_tmp.u[2]=0.3333;
memcpy(trust_track_tmp.X1, X, 6*sizeof(double));
memcpy(trust_track_tmp.P1, P, 6*6*sizeof(double));
memcpy(trust_track_tmp.X2, X, 6*sizeof(double));
memcpy(trust_track_tmp.P2, P, 6*6*sizeof(double));
memcpy(trust_track_tmp.X3, X, 6*sizeof(double));
memcpy(trust_track_tmp.P3, P, 6*6*sizeof(double));
double r, azi;
coor_trans Coor_trans;
Coor_trans.cart2polar(X[0], X[3], &r, &azi);
(*trust_track).push_back(trust_track_tmp);
//输出航迹更新信息
*Trust_track_num_Output=*Trust_track_num_Output+1;
for (int j=0;j<L;j++)
{
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Index=index;
Trust_Track_Output[*Trust_track_num_Output-1][j].Point_Sum=L;
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=Track_to_start[i][j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=Track_to_start[i][j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=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=Track_to_start[i][j].height;
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=Track_to_start[i][j].Amp;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = Track_to_start[i][j].snr;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = Track_to_start[i][j].RCS;
Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim, Track_to_start[i][j].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim));
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].range_dim, Track_to_start[i][j].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].range_dim));
double Direction_Angle;
Direction_Angle=atan2(Track_to_start[i][j].X[3],Track_to_start[i][j].X[1]);
if(Direction_Angle<0)
Direction_Angle=Direction_Angle+2*PI;
Trust_Track_Output[*Trust_track_num_Output-1][j-1].Direction_Angle=Direction_Angle/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=(Track_to_start[i][j].T)/1000.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].x=Track_to_start[i][j].X[0];
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=Track_to_start[i][j].X[1];
Trust_Track_Output[*Trust_track_num_Output-1][j].y=Track_to_start[i][j].X[2];
Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=Track_to_start[i][j].X[3];
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=Track_to_start[i][j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=Track_to_start[i][j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=Track_to_start[i][j].vr;
Trust_Track_Output[*Trust_track_num_Output-1][j].point_type=0;
Trust_Track_Output[*Trust_track_num_Output-1][j].Flag_Point=1;
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Mode=0;//跟踪模式 TWS 0
}
}
}
}
QVector <QVector<Temp_track>>().swap(Track_to_start);
}
void Track_Init::tmp_track_die(QVector <QVector<Temp_track>> *temp_track)
{
QVector <QVector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
int n=(*Iter).size();
if( (*Iter)[n-1].buff_round >2 || n>=10)
{
temp_track->erase(Iter);
Iter=temp_track->begin();
}
else
{
Iter++;
}
}
};
int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter)
{
for (int i=0;i<Work_Parameter.track_prohibite_area_num;i++)
{
if(r<Work_Parameter.R_max_track_prohibited[i] && r>Work_Parameter.R_min_track_prohibited[i] && azi<Work_Parameter.Azimuth_max_track_prohibited[i] && azi>Work_Parameter.Azimuth_min_track_prohibited[i])
{
return 1;
}
}
return 0;
}
Track_Init::Track_Init()
{
}
Reference in New Issue View Git Blame Copy Permalink