621 lines
24 KiB
C++
621 lines
24 KiB
C++
#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 <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++)
|
|
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.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);
|
|
|
|
}
|
|
|
|
//清空点迹
|
|
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);
|
|
|
|
|
|
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;
|
|
};
|
|
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)
|
|
{
|
|
|
|
//点迹信息
|
|
// float Z[2]={ point_process[i].Range*cos(point_process[i].Azimuth),point_process[i].Range*sin(point_process[i].Azimuth)} ;
|
|
float Z[3]={point_process[i].Range,
|
|
point_process[i].Azimuth,
|
|
point_process[i].Velocity};
|
|
float vr_point=point_process[i].Velocity;
|
|
float r_point=point_process[i].Range;
|
|
float prt = point_process[i].PRF_index;
|
|
float freq_ind = point_process[i].Freq_index;
|
|
float h_point = point_process[i].Height;
|
|
float T_point = point_process[i].CPI_Time;
|
|
|
|
//航迹信息
|
|
float X[4];
|
|
float P[4][4];
|
|
memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(float));
|
|
memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(float));
|
|
float v_temp_track=(*temp_track)[j][L-1].vr;
|
|
float r_temp_track=(*temp_track)[j][L-1].r;
|
|
float h_temp_track=(*temp_track)[j][L-1].height;
|
|
float T_track_head = (*temp_track)[j][L-1].T;
|
|
|
|
|
|
float delta_T = (T_point - T_track_head)/1000.0;
|
|
|
|
if( fabs(r_point-r_temp_track)>=5 )
|
|
{
|
|
//计算d
|
|
kalman Kalman;
|
|
float d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
|
|
//float d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
|
|
//float d = Kalman.d_cal_track_init(Z,X,P,delta_T);
|
|
//计算夹角
|
|
float x0=(*temp_track)[j][L-2].X[0];
|
|
float y0=(*temp_track)[j][L-2].X[2];
|
|
float x1=(*temp_track)[j][L-1].X[0];
|
|
float y1=(*temp_track)[j][L-1].X[2];
|
|
float x2=Z[0]*cos(Z[1]);
|
|
float y2=Z[0]*sin(Z[1]);
|
|
double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2);
|
|
|
|
|
|
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.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.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;
|
|
float 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)};
|
|
float 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)};
|
|
float X[4];
|
|
float P[4][4];
|
|
kalman Kalman;
|
|
float 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(float));
|
|
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(float));
|
|
(*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)
|
|
{
|
|
|
|
|
|
//点迹信息
|
|
float 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;
|
|
float r_point=point_process[i].Range;
|
|
float h_point = point_process[i].Height;
|
|
float T_point = point_process[i].CPI_Time;
|
|
|
|
//航迹信息
|
|
float x_track_head, y_track_head;
|
|
x_track_head=(*temp_track)[j][0].X[0];
|
|
y_track_head=(*temp_track)[j][0].X[2];
|
|
float v_track_head=(*temp_track)[j][0].vr;
|
|
float h_track_head = (*temp_track)[j][0].height;
|
|
float T_track_head = (*temp_track)[j][0].T;
|
|
|
|
|
|
//距离差
|
|
float dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
|
|
|
|
float vmax;
|
|
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
|
|
{
|
|
vmax = V_MAX/5;
|
|
}
|
|
else //中远程模式 最大速度正常用
|
|
{
|
|
vmax = V_MAX;
|
|
}
|
|
|
|
float 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
|
|
{
|
|
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.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;;
|
|
float 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)};
|
|
float 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)};
|
|
float X[4];
|
|
float P[4][4];
|
|
kalman Kalman;
|
|
float 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(float));
|
|
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(float));
|
|
(*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(float x0,float y0,float x1,float y1,float x2,float 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 <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)
|
|
{
|
|
|
|
//temp_track中的临时航迹转为可靠航迹
|
|
QVector <QVector<Temp_track>>::iterator Iter;
|
|
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
|
|
{
|
|
int L=(*Iter).size();
|
|
float range=(*Iter)[L-1].r;
|
|
float 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
|
|
{
|
|
if(trust_track->size()<MAX_TRACK_INDEX)
|
|
{
|
|
|
|
int index = track_index_mangement.track_ind_get(trust_track);
|
|
if(index>0)
|
|
{
|
|
|
|
//加入本地航迹文件
|
|
float Z0[2]={((*Iter)[L-3].r)*cos((*Iter)[L-3].azi),((*Iter)[L-3].r)*sin((*Iter)[L-3].azi) };
|
|
float Z1[2]={((*Iter)[L-2].r)*cos((*Iter)[L-2].azi),((*Iter)[L-2].r)*sin((*Iter)[L-2].azi) };
|
|
float Z2[2]={((*Iter)[L-1].r)*cos((*Iter)[L-1].azi),((*Iter)[L-1].r)*sin((*Iter)[L-1].azi) };
|
|
float X[6];
|
|
float P[6][6];
|
|
float T1=((*Iter)[L-2].T-(*Iter)[L-3].T)/1000.0;
|
|
float T2=((*Iter)[L-1].T-(*Iter)[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(float));
|
|
memcpy(trust_track_tmp.P, P, 6*6*sizeof(float));
|
|
|
|
trust_track_tmp.Amplitude=(*Iter)[L-1].Amp;
|
|
trust_track_tmp.snr_point = (*Iter)[L-1].snr;
|
|
trust_track_tmp.RCS = (*Iter)[L-1].RCS;
|
|
trust_track_tmp.T_track = (*Iter)[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=(*Iter)[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(float));
|
|
memcpy(trust_track_tmp.P1, P, 6*6*sizeof(float));
|
|
memcpy(trust_track_tmp.X2, X, 6*sizeof(float));
|
|
memcpy(trust_track_tmp.P2, P, 6*6*sizeof(float));
|
|
memcpy(trust_track_tmp.X3, X, 6*sizeof(float));
|
|
memcpy(trust_track_tmp.P3, P, 6*6*sizeof(float));
|
|
|
|
float r, azi;
|
|
coor_trans Coor_trans;
|
|
Coor_trans.cart2polar(X[0], X[3], &r, &azi);
|
|
|
|
|
|
if(azi/PI*180>=TRACK_SECTION_1_START && azi/PI*180<TRACK_SECTION_2_START)
|
|
trust_track_tmp.Track_section_idx = 1;
|
|
|
|
if(azi/PI*180>=TRACK_SECTION_2_START && azi/PI*180<TRACK_SECTION_3_START)
|
|
trust_track_tmp.Track_section_idx = 2;
|
|
|
|
if(azi/PI*180>=TRACK_SECTION_3_START && azi/PI*180<TRACK_SECTION_4_START)
|
|
trust_track_tmp.Track_section_idx = 3;
|
|
|
|
if(azi/PI*180>=TRACK_SECTION_4_START && azi/PI*180<TRACK_SECTION_5_START)
|
|
trust_track_tmp.Track_section_idx = 4;
|
|
|
|
if(azi/PI*180>=TRACK_SECTION_5_START && azi/PI*180<TRACK_SECTION_6_START)
|
|
trust_track_tmp.Track_section_idx = 5;
|
|
|
|
if(azi/PI*180>=TRACK_SECTION_6_START && azi/PI*180<TRACK_SECTION_7_START)
|
|
trust_track_tmp.Track_section_idx = 6;
|
|
|
|
if(azi/PI*180>=TRACK_SECTION_7_START && azi/PI*180<TRACK_SECTION_8_START)
|
|
trust_track_tmp.Track_section_idx = 7;
|
|
|
|
if(azi/PI*180>=TRACK_SECTION_8_START && azi/PI*180<TRACK_SECTION_9_START)
|
|
trust_track_tmp.Track_section_idx = 8;
|
|
|
|
if(azi/PI*180>=TRACK_SECTION_9_START || azi/PI*180<TRACK_SECTION_1_START)
|
|
trust_track_tmp.Track_section_idx = 9;
|
|
|
|
(*trust_track)[trust_track_tmp.Track_section_idx-1].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=(*Iter)[j].r;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=(*Iter)[j].azi/PI*180;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=sqrt(pow((*Iter)[j].X[1],2)+pow((*Iter)[j].X[3],2));
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].z=(*Iter)[j].height;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=(*Iter)[j].Amp;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = (*Iter)[j].snr;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = (*Iter)[j].RCS;
|
|
float Direction_Angle;
|
|
Direction_Angle=atan2((*Iter)[j].X[3],(*Iter)[j].X[1]);
|
|
if(Direction_Angle<0)
|
|
Direction_Angle=Direction_Angle+2*PI;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][L-1].Direction_Angle=Direction_Angle/PI*180;
|
|
|
|
|
|
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=((*Iter)[j].T)/1000.0;
|
|
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].x=(*Iter)[j].X[0];
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=(*Iter)[j].X[1];
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].y=(*Iter)[j].X[2];
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=(*Iter)[j].X[3];
|
|
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=(*Iter)[j].r;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=(*Iter)[j].azi/PI*180;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=(*Iter)[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
|
|
}
|
|
|
|
temp_track->erase(Iter);
|
|
Iter=temp_track->begin();
|
|
}
|
|
else
|
|
{
|
|
Iter++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Iter++;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
Iter++;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
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 >1 || n>=10)
|
|
{
|
|
temp_track->erase(Iter);
|
|
Iter=temp_track->begin();
|
|
}
|
|
else
|
|
{
|
|
Iter++;
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
|
|
|
|
int Track_Init::track_init_prohibit(float r, float 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()
|
|
{
|
|
|
|
|
|
}
|