waiwaylee
958ff6b11b
2、TWS点迹关联时增加点航距离门限，限制部分突然关联到很远的点迹的问题。 Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
966 lines
40 KiB
C++
966 lines
40 KiB
C++
#include "track_asso.h"
|
|
#include "kalman.h"
|
|
#include "coor_trans.h"
|
|
#include <qmath.h>
|
|
#include "memory.h"
|
|
#include <QVector>
|
|
#include <Eigen/Dense>
|
|
using namespace Eigen;
|
|
using namespace std;
|
|
|
|
|
|
// //////////////////////////// 用point_recv中点迹与trust_track中的点迹进行关联， /////////////
|
|
// //////////////////////////// 航迹更新结果通过Trust_Track_Output[MAX_TRACK_NUM][10]和 Trust_track_num_Output输出 /////////////
|
|
|
|
int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv, //输入点迹
|
|
QVector <Trust_Track> *trust_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]);
|
|
int n=point_process.size();
|
|
point_process[n-1].point_section_asso = 1;
|
|
}
|
|
|
|
|
|
//IMM算法
|
|
model_interaction(trust_track);
|
|
|
|
model_filter(trust_track, Work_Parameter);
|
|
|
|
model_output(trust_track);
|
|
|
|
|
|
//删除关联上的点
|
|
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]);
|
|
|
|
}
|
|
|
|
//point_process清空
|
|
QVector<PointRecv>().swap(point_process);
|
|
|
|
//输出航迹
|
|
for (int i=0;i<trust_track->size();i++)
|
|
{ // 输出更新航迹条件：
|
|
// if((*trust_track)[i].manual_tracking_flag==0)
|
|
// {
|
|
|
|
*Trust_track_num_Output= *Trust_track_num_Output+1;
|
|
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
|
|
double x,y;
|
|
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
|
|
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
|
|
coor_trans Coor_trans;
|
|
double r, azi;
|
|
Coor_trans.cart2polar(x, y, &r, &azi);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azi/PI*180;
|
|
|
|
if((*trust_track)[i].Height<r)
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin((*trust_track)[i].Height/r)/PI*180;
|
|
else
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = (*trust_track)[i].elev_point;
|
|
|
|
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].Track_Mode=(*trust_track)[i].Track_Mode;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track)[i].T_track/1000.0;
|
|
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].z=(*trust_track)[i].Height+Work_Parameter.Height;
|
|
|
|
|
|
double Direction_Angle;
|
|
Direction_Angle=atan((*trust_track)[i].X[4]/(*trust_track)[i].X[1]);
|
|
if((*trust_track)[i].X[1]<0){Direction_Angle=Direction_Angle+PI;}
|
|
if((*trust_track)[i].X[1]>0&&(*trust_track)[i].X[4]<0){Direction_Angle=Direction_Angle+2*PI;}
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180;
|
|
|
|
//关联点信息
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=(*trust_track)[i].range_point;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track)[i].azi_point;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track)[i].elev_point;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track)[i].vr_point;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=0;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = (*trust_track)[i].prf_point;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
|
|
|
|
//直接输出点迹高度-20260605
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].z = (*trust_track)[i].range_point * sin((*trust_track)[i].elev_point/180.0*PI);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = (*trust_track)[i].elev_point;
|
|
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track)[i].X[0];
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track)[i].X[3];
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track)[i].X[1];
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track)[i].X[4];
|
|
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*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));
|
|
// }
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
//遍历所有航迹 进行多模型交互
|
|
void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
|
|
{
|
|
|
|
|
|
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
|
|
{
|
|
|
|
// if((*trust_track)[loop_of_track].manual_tracking_flag == 0)
|
|
// {
|
|
double u_last[3];
|
|
u_last[0]=(*trust_track)[loop_of_track].u[0];
|
|
u_last[1]=(*trust_track)[loop_of_track].u[1];
|
|
u_last[2]=(*trust_track)[loop_of_track].u[2];
|
|
|
|
double u_t[3][3];
|
|
double c[3];
|
|
c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
|
|
c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
|
|
c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];
|
|
|
|
u_t[0][0]=Pt[0][0]*u_last[0]/c[0];
|
|
u_t[1][0]=Pt[1][0]*u_last[1]/c[0];
|
|
u_t[2][0]=Pt[2][0]*u_last[2]/c[0];
|
|
u_t[0][1]=Pt[0][1]*u_last[0]/c[1];
|
|
u_t[1][1]=Pt[1][1]*u_last[1]/c[1];
|
|
u_t[2][1]=Pt[2][1]*u_last[2]/c[1];
|
|
u_t[0][2]=Pt[0][2]*u_last[0]/c[2];
|
|
u_t[1][2]=Pt[1][2]*u_last[1]/c[2];
|
|
u_t[2][2]=Pt[2][2]*u_last[2]/c[2];
|
|
|
|
|
|
double Xo1_last[6],Xo2_last[6],Xo3_last[6],X1[6],X2[6],X3[6];
|
|
for (int i=0;i<6;i++)
|
|
{
|
|
X1[i]=(*trust_track)[loop_of_track].X1[i];
|
|
X2[i]=(*trust_track)[loop_of_track].X2[i];
|
|
X3[i]=(*trust_track)[loop_of_track].X3[i];
|
|
}
|
|
|
|
|
|
for (int i=0;i<6;i++)
|
|
{
|
|
Xo1_last[i]=u_t[0][0]*X1[i]+u_t[1][0]*X2[i]+u_t[2][0]*X3[i];
|
|
Xo2_last[i]=u_t[0][1]*X1[i]+u_t[1][1]*X2[i]+u_t[2][1]*X3[i];
|
|
Xo3_last[i]=u_t[0][2]*X1[i]+u_t[1][2]*X2[i]+u_t[2][2]*X3[i];
|
|
}
|
|
|
|
|
|
double X1_sub_Xo1[6],X2_sub_Xo1[6],X3_sub_Xo1[6];
|
|
double X1_sub_Xo2[6],X2_sub_Xo2[6],X3_sub_Xo2[6];
|
|
double X1_sub_Xo3[6],X2_sub_Xo3[6],X3_sub_Xo3[6];
|
|
double X11[6][6],X21[6][6],X31[6][6];
|
|
double X12[6][6],X22[6][6],X32[6][6];
|
|
double X13[6][6],X23[6][6],X33[6][6];
|
|
|
|
|
|
for (int i=0;i<6;i++)
|
|
{
|
|
|
|
X1_sub_Xo1[i]=X1[i]-Xo1_last[i];
|
|
X2_sub_Xo1[i]=X2[i]-Xo1_last[i];
|
|
X3_sub_Xo1[i]=X3[i]-Xo1_last[i];
|
|
X1_sub_Xo2[i]=X1[i]-Xo2_last[i];
|
|
X2_sub_Xo2[i]=X2[i]-Xo2_last[i];
|
|
X3_sub_Xo2[i]=X3[i]-Xo2_last[i];
|
|
X1_sub_Xo3[i]=X1[i]-Xo3_last[i];
|
|
X2_sub_Xo3[i]=X2[i]-Xo3_last[i];
|
|
X3_sub_Xo3[i]=X3[i]-Xo3_last[i];
|
|
|
|
}
|
|
|
|
|
|
for (int i=0;i<6;i++)
|
|
for(int j=0;j<6;j++)
|
|
{
|
|
X11[i][j]=X1_sub_Xo1[i]*X1_sub_Xo1[j];
|
|
X21[i][j]=X2_sub_Xo1[i]*X2_sub_Xo1[j];
|
|
X31[i][j]=X3_sub_Xo1[i]*X3_sub_Xo1[j];
|
|
X12[i][j]=X1_sub_Xo2[i]*X1_sub_Xo2[j];
|
|
X22[i][j]=X2_sub_Xo2[i]*X2_sub_Xo2[j];
|
|
X32[i][j]=X3_sub_Xo2[i]*X3_sub_Xo2[j];
|
|
X13[i][j]=X1_sub_Xo3[i]*X1_sub_Xo3[j];
|
|
X23[i][j]=X2_sub_Xo3[i]*X2_sub_Xo3[j];
|
|
X33[i][j]=X3_sub_Xo3[i]*X3_sub_Xo3[j];
|
|
}
|
|
|
|
|
|
double Po1_last[6][6],Po2_last[6][6],Po3_last[6][6];
|
|
for (int i=0;i<6;i++)
|
|
for(int j=0;j<6;j++)
|
|
{
|
|
Po1_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X11[i][j])*u_t[0][0]+((*trust_track)[loop_of_track].P2[i][j]+X21[i][j])*u_t[1][0]+((*trust_track)[loop_of_track].P3[i][j]+X31[i][j])*u_t[2][0];
|
|
Po2_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X12[i][j])*u_t[0][1]+((*trust_track)[loop_of_track].P2[i][j]+X22[i][j])*u_t[1][1]+((*trust_track)[loop_of_track].P3[i][j]+X32[i][j])*u_t[2][1];
|
|
Po3_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X13[i][j])*u_t[0][2]+((*trust_track)[loop_of_track].P2[i][j]+X23[i][j])*u_t[1][2]+((*trust_track)[loop_of_track].P3[i][j]+X33[i][j])*u_t[2][2];
|
|
}
|
|
|
|
for(int i=0;i<6;i++)
|
|
{
|
|
(*trust_track)[loop_of_track].X1[i]=Xo1_last[i];
|
|
(*trust_track)[loop_of_track].X2[i]=Xo2_last[i];
|
|
(*trust_track)[loop_of_track].X3[i]=Xo3_last[i];
|
|
}
|
|
for(int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
{
|
|
(*trust_track)[loop_of_track].P1[i][j]=Po1_last[i][j];
|
|
(*trust_track)[loop_of_track].P2[i][j]=Po2_last[i][j];
|
|
(*trust_track)[loop_of_track].P3[i][j]=Po3_last[i][j];
|
|
}
|
|
// }
|
|
}
|
|
}
|
|
|
|
|
|
// 产生 F Q 矩阵
|
|
void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter)
|
|
{
|
|
|
|
//F
|
|
double alpha=0.2;
|
|
memset(F,0,36*sizeof(double));
|
|
F[0][0]=1;
|
|
F[0][1]=delta_T;
|
|
F[0][2]=(alpha*delta_T-1+exp(-alpha*delta_T))/pow(alpha,2);
|
|
F[1][0]=0;
|
|
F[1][1]=1;
|
|
F[1][2]=(1-exp(-alpha*delta_T))/alpha;
|
|
F[2][0]=0;
|
|
F[2][1]=0;
|
|
F[2][2]=exp(-alpha*delta_T);
|
|
|
|
F[3][3]=1;
|
|
F[3][4]=delta_T;
|
|
F[3][5]=(alpha*delta_T-1+exp(-alpha*delta_T))/pow(alpha,2);
|
|
F[4][3]=0;
|
|
F[4][4]=1;
|
|
F[4][5]=(1-exp(-alpha*delta_T))/alpha;
|
|
F[5][3]=0;
|
|
F[5][4]=0;
|
|
F[5][5]=exp(-alpha*delta_T);
|
|
|
|
|
|
|
|
//模型1
|
|
//Q
|
|
double q1=Work_Parameter.Model1_Q_slow;
|
|
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
|
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
|
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
|
double q221=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
|
|
double q231=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
|
|
double q331=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
|
|
memset(Q1,0,36*sizeof(double));
|
|
Q1[0][0]=q1*q111; Q1[0][1]=q1*q121; Q1[0][2]=q1*q131;
|
|
Q1[1][0]=q1*q121; Q1[1][1]=q1*q221; Q1[1][2]=q1*q231;
|
|
Q1[2][0]=q1*q131; Q1[2][1]=q1*q231; Q1[2][2]=q1*q331;
|
|
|
|
Q1[3][3]=q1*q111; Q1[3][4]=q1*q121; Q1[3][5]=q1*q131;
|
|
Q1[4][3]=q1*q121; Q1[4][4]=q1*q221; Q1[4][5]=q1*q231;
|
|
Q1[5][3]=q1*q131; Q1[5][4]=q1*q231; Q1[5][5]=q1*q331;
|
|
|
|
|
|
//模型2
|
|
//Q
|
|
double q2;
|
|
if(v_track<=5)
|
|
{
|
|
q2=Work_Parameter.Model2_Q_slow/20;
|
|
}
|
|
else if(v_track<=15&&v_track>5)
|
|
{
|
|
q2=Work_Parameter.Model2_Q_slow;
|
|
}
|
|
else if(v_track<=30&&v_track>15)
|
|
{
|
|
q2=Work_Parameter.Model2_Q_slow;
|
|
}
|
|
else if(v_track<=100&&v_track>30)
|
|
{
|
|
q2=Work_Parameter.Model2_Q_fast;
|
|
|
|
}
|
|
else
|
|
{
|
|
q2=Work_Parameter.Model2_Q_fast;
|
|
}
|
|
|
|
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
|
double q122=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
|
double q132=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
|
double q222=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
|
|
double q232=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
|
|
double q332=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
|
|
|
|
memset(Q2,0,36*sizeof(double));
|
|
Q2[0][0]=q2*q112;Q2[0][1]=q2*q122; Q2[0][2]=q2*q132;
|
|
Q2[1][0]=q2*q122;Q2[1][1]=q2*q222; Q2[1][2]=q2*q232;
|
|
Q2[2][0]=q2*q132; Q2[2][1]=q2*q232; Q2[2][2]=q2*q332;
|
|
|
|
Q2[3][3]=q2*q112; Q2[3][4]=q2*q122; Q2[3][5]=q2*q132;
|
|
Q2[4][3]=q2*q122; Q2[4][4]=q2*q222; Q2[4][5]=q2*q232;
|
|
Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332;
|
|
|
|
|
|
//模型3
|
|
//Q
|
|
double q3;
|
|
if(v_track<=5)
|
|
{
|
|
q3=Work_Parameter.Model3_Q_slow/20;
|
|
}
|
|
else if(v_track<=15&&v_track>5)
|
|
{
|
|
q3=Work_Parameter.Model3_Q_slow;
|
|
}
|
|
else if(v_track<=20&&v_track>15)
|
|
{
|
|
q3=Work_Parameter.Model3_Q_slow;
|
|
}
|
|
else if(v_track<=50&&v_track>20)
|
|
{
|
|
q3=Work_Parameter.Model3_Q_fast;
|
|
|
|
}
|
|
else if(v_track<=100&&v_track>50)
|
|
{
|
|
q3=2*Work_Parameter.Model3_Q_fast;
|
|
|
|
}
|
|
else
|
|
{
|
|
q3=10*Work_Parameter.Model3_Q_fast;
|
|
}
|
|
|
|
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
|
double q123=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
|
double q133=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
|
double q223=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
|
|
double q233=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
|
|
double q333=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
|
|
|
|
|
|
memset(Q3,0,36*sizeof(double));
|
|
Q3[0][0]=q3*q113; Q3[0][1]=q3*q123; Q3[0][2]=q3*q133;
|
|
Q3[1][0]=q3*q123; Q3[1][1]=q3*q223; Q3[1][2]=q3*q233;
|
|
Q3[2][0]=q3*q133; Q3[2][1]=q3*q233; Q3[2][2]=q3*q333;
|
|
|
|
Q3[3][3]=q3*q113; Q3[3][4]=q3*q123; Q3[3][5]=q3*q133;
|
|
Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233;
|
|
Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333;
|
|
|
|
|
|
}
|
|
|
|
|
|
// 计算三个 d
|
|
void Track_Asso::IMM_d_cal(double v_track,
|
|
double X1[6], double P1[6][6],
|
|
double X2[6], double P2[6][6],
|
|
double X3[6], double P3[6][6],
|
|
double Z[3], double prt,double freq_ind,
|
|
double delta_T,
|
|
double *d1,double *d2,double *d3,
|
|
struct RadarPara Work_Parameter)
|
|
{
|
|
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);
|
|
|
|
kalman Kalman;
|
|
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
|
|
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
|
|
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Track_Asso:: model_filter(QVector <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;
|
|
};
|
|
QVector <asso_info> associated_info;
|
|
|
|
//遍历所有点迹航迹 计算点迹和航迹的统计距离
|
|
for (int 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
|
|
|
|
for (int 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 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);
|
|
|
|
//计算高度门限
|
|
double d_h;
|
|
if (r_track<1000)
|
|
{
|
|
if(abs(h_track-h_point)<=150)
|
|
{
|
|
d_h = 1;
|
|
}
|
|
else
|
|
{
|
|
d_h = 0;
|
|
}
|
|
|
|
}
|
|
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;
|
|
}
|
|
|
|
qDebug() << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
|
|
qDebug() << h_track<<" "<<h_point<<" "<<d_h;
|
|
d_h = 1;
|
|
|
|
//计算距离门限，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) && d_h == 1 && 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) && d_h == 1 && d_r)//
|
|
||( r_track>=1000 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD) && d_h == 1 && d_r)//
|
|
)
|
|
{
|
|
qDebug() << "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=loop_of_point+1;
|
|
associated_info_tmp.track_index=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);
|
|
}
|
|
}
|
|
// }
|
|
}
|
|
|
|
|
|
//最近邻法关联
|
|
int associated_num=associated_info.size();
|
|
for (int i=0;i<trust_track->size();i++)
|
|
{
|
|
//寻找最小d
|
|
if(associated_num>0)
|
|
{
|
|
int min_index=1;
|
|
double min_d=associated_info[0].d_min;
|
|
for (int ii=0;ii<associated_num;ii++)
|
|
{
|
|
if(associated_info[ii].d_min<min_d)
|
|
{
|
|
min_d=associated_info[ii].d_min;
|
|
min_index=ii+1;
|
|
}
|
|
}
|
|
int track_index=associated_info[min_index-1].track_index;
|
|
int point_index=associated_info[min_index-1].point_index;
|
|
|
|
double d1=associated_info[min_index-1].d1;
|
|
double d2=associated_info[min_index-1].d2;
|
|
double d3=associated_info[min_index-1].d3;
|
|
|
|
//卡尔曼滤波
|
|
double Z[3]={point_process[point_index-1].Range,point_process[point_index-1].Azimuth,point_process[point_index-1].Velocity};
|
|
double prt = point_process[point_index-1].PRF_index;
|
|
double freq_ind = point_process[point_index-1].Freq_index;
|
|
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
|
|
memcpy(X1,(*trust_track)[track_index-1].X1,6*sizeof(double));
|
|
memcpy(X2,(*trust_track)[track_index-1].X2,6*sizeof(double));
|
|
memcpy(X3,(*trust_track)[track_index-1].X3,6*sizeof(double));
|
|
memcpy(P1,(*trust_track)[track_index-1].P1,6*6*sizeof(double));
|
|
memcpy(P2,(*trust_track)[track_index-1].P2,6*6*sizeof(double));
|
|
memcpy(P3,(*trust_track)[track_index-1].P3,6*6*sizeof(double));
|
|
double v_track = sqrt(pow((*trust_track)[track_index-1].X[1],2)+pow((*trust_track)[track_index-1].X[4],2));
|
|
double delta_T = (point_process[point_index-1].CPI_Time - (*trust_track)[track_index-1].T_track)/1000.0;
|
|
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)
|
|
{
|
|
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
|
|
double S1[2][2],S2[2][2],S3[2][2];
|
|
kalman Kalman;
|
|
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind );
|
|
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind );
|
|
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind );
|
|
|
|
|
|
Matrix2f S1_out,S2_out,S3_out;
|
|
for (int ii=0;ii<2;ii++)
|
|
for (int jj=0;jj<2;jj++)
|
|
{
|
|
S1_out(ii,jj)=S1[ii][jj];
|
|
S2_out(ii,jj)=S2[ii][jj];
|
|
S3_out(ii,jj)=S3[ii][jj];
|
|
}
|
|
double det_S1=S1_out.determinant();
|
|
double Possibility1=1/sqrt(2*PI*det_S1)*exp(-0.5*d1);
|
|
double det_S2=S2_out.determinant();
|
|
double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2);
|
|
double det_S3=S3_out.determinant();
|
|
double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3);
|
|
|
|
//更新模型概率
|
|
double u_last[3];
|
|
u_last[0]=(*trust_track)[track_index-1].u[0];
|
|
u_last[1]=(*trust_track)[track_index-1].u[1];
|
|
u_last[2]=(*trust_track)[track_index-1].u[2];
|
|
|
|
double c[3];
|
|
c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
|
|
c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
|
|
c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];
|
|
|
|
(*trust_track)[track_index-1].u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
|
|
(*trust_track)[track_index-1].u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
|
|
(*trust_track)[track_index-1].u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
|
|
|
|
//更新 X1 X2 X3 P1 P2 P3
|
|
for(int ii=0;ii<6;ii++)
|
|
{
|
|
(*trust_track)[track_index-1].X1[ii]=X1_filter[ii];
|
|
(*trust_track)[track_index-1].X2[ii]=X2_filter[ii];
|
|
(*trust_track)[track_index-1].X3[ii]=X3_filter[ii];
|
|
}
|
|
for(int ii=0;ii<6;ii++)
|
|
for (int jj=0;jj<6;jj++)
|
|
{
|
|
(*trust_track)[track_index-1].P1[ii][jj]=P1_filter[ii][jj];
|
|
(*trust_track)[track_index-1].P2[ii][jj]=P2_filter[ii][jj];
|
|
(*trust_track)[track_index-1].P3[ii][jj]=P3_filter[ii][jj];
|
|
}
|
|
|
|
//更新航迹时间
|
|
(*trust_track)[track_index-1].T_track = point_process[point_index-1].CPI_Time;
|
|
|
|
|
|
|
|
//更新关联上的点迹信息
|
|
(*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;
|
|
(*trust_track)[track_index-1].vr_point=point_process[point_index-1].Velocity;
|
|
(*trust_track)[track_index-1].point_type = 0;
|
|
(*trust_track)[track_index-1].prf_point = point_process[point_index-1].PRF_index;
|
|
(*trust_track)[track_index-1].snr_point = point_process[point_index-1].snr;
|
|
(*trust_track)[track_index-1].Amplitude= point_process[point_index-1].Amplitude; //幅度
|
|
|
|
(*trust_track)[track_index-1].RCS = point_process[point_index-1].RCS;
|
|
|
|
(*trust_track)[track_index-1].pitch_num = point_process[point_index-1].pitch_num;
|
|
|
|
std::memcpy((*trust_track)[track_index-1].speed_dim, point_process[point_index-1].speed_dim, sizeof((*trust_track)[track_index-1].speed_dim));
|
|
std::memcpy((*trust_track)[track_index-1].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[track_index-1].range_dim));
|
|
|
|
//更新高度
|
|
track_hight_update(track_index,point_index,trust_track);
|
|
|
|
}
|
|
(*trust_track)[track_index-1].Extrapolate_round = 0; //连续未用实点更新时间
|
|
(*trust_track)[track_index-1].point_flag=1; //实点
|
|
(*trust_track)[track_index-1].associate_point_number = (*trust_track)[track_index-1].associate_point_number+1; //关联点数+1
|
|
|
|
//删除associated_info中关联上的航迹 点迹信息
|
|
for (int ii=0;ii<associated_num;)
|
|
{
|
|
if(associated_info[ii].track_index==track_index)
|
|
{
|
|
associated_info.erase(associated_info.begin()+ii);
|
|
associated_num=associated_info.size();
|
|
}
|
|
else
|
|
ii++;
|
|
}
|
|
associated_num=associated_info.size();
|
|
for (int ii=0;ii<associated_num;)
|
|
{
|
|
if(associated_info[ii].point_index==point_index)
|
|
{
|
|
associated_info.erase(associated_info.begin()+ii);
|
|
associated_num=associated_info.size();
|
|
}
|
|
else
|
|
ii++;
|
|
}
|
|
associated_num=associated_info.size();
|
|
|
|
//在接收点迹中 将关联上的点迹的使用标记位置为1 等待删除
|
|
point_process[point_index-1].Use_Flag = 1;
|
|
}
|
|
}
|
|
|
|
|
|
//未关联上的航迹 进行外推 tws
|
|
for (int i =0; i<(*trust_track).size();i++ )
|
|
{
|
|
if((*trust_track)[i].point_flag == 0 && (*trust_track)[i].manual_tracking_flag == 0)
|
|
{
|
|
double v_track = sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
|
|
double delta_T;
|
|
if(Work_Parameter.work_mode == 0)
|
|
{
|
|
delta_T = Work_Parameter.DATA_RATE_SHORT;
|
|
}
|
|
else if(Work_Parameter.work_mode == 1)
|
|
{
|
|
delta_T = Work_Parameter.DATA_RATE_MIDDLE;
|
|
}
|
|
else if(Work_Parameter.work_mode == 2)
|
|
{
|
|
delta_T = Work_Parameter.DATA_RATE_FAR;
|
|
}
|
|
else
|
|
{
|
|
delta_T = Work_Parameter.DATA_RATE_SHORT;
|
|
}
|
|
|
|
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);
|
|
|
|
//航迹X P
|
|
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
|
|
memcpy(X1,(*trust_track)[i].X1,6*sizeof(double));
|
|
memcpy(X2,(*trust_track)[i].X2,6*sizeof(double));
|
|
memcpy(X3,(*trust_track)[i].X3,6*sizeof(double));
|
|
memcpy(P1,(*trust_track)[i].P1,6*6*sizeof(double));
|
|
memcpy(P2,(*trust_track)[i].P2,6*6*sizeof(double));
|
|
memcpy(P3,(*trust_track)[i].P3,6*6*sizeof(double));
|
|
|
|
//外推
|
|
|
|
double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
|
|
kalman Kalman;
|
|
Kalman.kalman_pred(F, Q1, X1, P1, X1_pred, P1_pred);
|
|
Kalman.kalman_pred(F, Q2, X2, P2, X2_pred, P2_pred);
|
|
Kalman.kalman_pred(F, Q3, X3, P3, X3_pred, P3_pred);
|
|
memcpy((*trust_track)[i].X1,X1_pred,6*sizeof(double));
|
|
memcpy((*trust_track)[i].X2,X2_pred,6*sizeof(double));
|
|
memcpy((*trust_track)[i].X3,X3_pred,6*sizeof(double));
|
|
memcpy((*trust_track)[i].P1,P1_pred,6*6*sizeof(double));
|
|
memcpy((*trust_track)[i].P2,P2_pred,6*6*sizeof(double));
|
|
memcpy((*trust_track)[i].P3,P3_pred,6*6*sizeof(double));
|
|
|
|
|
|
(*trust_track)[i].T_track = (*trust_track)[i].T_track+delta_T*1000.0; //更新航迹时间
|
|
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间
|
|
(*trust_track)[i].point_flag=0; //虚点
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
void Track_Asso:: model_output(QVector <Trust_Track> *trust_track)
|
|
{
|
|
for(int loop_of_track=0; loop_of_track<trust_track->size(); loop_of_track++)
|
|
{
|
|
|
|
// if((*trust_track)[loop_of_track].manual_tracking_flag==0)
|
|
// {
|
|
double u_now[3];
|
|
u_now[0]=(*trust_track)[loop_of_track].u[0];
|
|
u_now[1]=(*trust_track)[loop_of_track].u[1];
|
|
u_now[2]=(*trust_track)[loop_of_track].u[2];
|
|
|
|
double X1_filter[6],X2_filter[6],X3_filter[6];
|
|
double P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
|
|
for (int ii=0;ii<6;ii++)
|
|
{
|
|
X1_filter[ii]=(*trust_track)[loop_of_track].X1[ii];
|
|
X2_filter[ii]=(*trust_track)[loop_of_track].X2[ii];
|
|
X3_filter[ii]=(*trust_track)[loop_of_track].X3[ii];
|
|
}
|
|
for (int ii=0;ii<6;ii++)
|
|
for(int jj=0;jj<6;jj++)
|
|
{
|
|
P1_filter[ii][jj]=(*trust_track)[loop_of_track].P1[ii][jj];
|
|
P2_filter[ii][jj]=(*trust_track)[loop_of_track].P2[ii][jj];
|
|
P3_filter[ii][jj]=(*trust_track)[loop_of_track].P3[ii][jj];
|
|
}
|
|
|
|
double X_filter[6], P_filter[6][6];
|
|
for (int i=0;i<6;i++)
|
|
X_filter[i]=u_now[0]*X1_filter[i]+u_now[1]*X2_filter[i]+u_now[2]*X3_filter[i];
|
|
|
|
double X1_sub_X[6], X2_sub_X[6], X3_sub_X[6];
|
|
double X1X[6][6], X2X[6][6],X3X[6][6];
|
|
for (int i=0;i<6;i++)
|
|
{
|
|
X1_sub_X[i]=X1_filter[i]-X_filter[i];
|
|
X2_sub_X[i]=X2_filter[i]-X_filter[i];
|
|
X3_sub_X[i]=X3_filter[i]-X_filter[i];
|
|
}
|
|
for(int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
{
|
|
X1X[i][j]=X1_sub_X[i]*X1_sub_X[j];
|
|
X2X[i][j]=X2_sub_X[i]*X2_sub_X[j];
|
|
X3X[i][j]=X3_sub_X[i]*X3_sub_X[j];
|
|
}
|
|
for(int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
P_filter[i][j]=u_now[0]*(P1_filter[i][j]+X1X[i][j])+u_now[1]*(P2_filter[i][j]+X2X[i][j])+u_now[2]*(P3_filter[i][j]+X3X[i][j]);
|
|
|
|
|
|
//本地航迹文件更新
|
|
(*trust_track)[loop_of_track].X[0]=X_filter[0]; //位置 速度
|
|
(*trust_track)[loop_of_track].X[1]=X_filter[1];
|
|
(*trust_track)[loop_of_track].X[2]=X_filter[2];
|
|
(*trust_track)[loop_of_track].X[3]=X_filter[3];
|
|
(*trust_track)[loop_of_track].X[4]=X_filter[4];
|
|
(*trust_track)[loop_of_track].X[5]=X_filter[5];
|
|
for(int ii=0;ii<6;ii++) //协方差矩阵
|
|
for (int jj=0;jj<6;jj++)
|
|
(*trust_track)[loop_of_track].P[ii][jj]=P_filter[ii][jj];
|
|
|
|
// //航迹区更新
|
|
// double r, azi;
|
|
// coor_trans Coor_trans;
|
|
// Coor_trans.cart2polar((*trust_track)[loop_of_track].X[0], (*trust_track)[loop_of_track].X[3], &r, &azi);
|
|
|
|
// if(azi/PI*180>=TRACK_SECTION_1_START && azi/PI*180<TRACK_SECTION_2_START)
|
|
// (*trust_track)[loop_of_track].Track_section_idx = 1;
|
|
|
|
// if(azi/PI*180>=TRACK_SECTION_2_START && azi/PI*180<TRACK_SECTION_3_START)
|
|
// (*trust_track)[loop_of_track].Track_section_idx = 2;
|
|
|
|
// if(azi/PI*180>=TRACK_SECTION_3_START && azi/PI*180<TRACK_SECTION_4_START)
|
|
// (*trust_track)[loop_of_track].Track_section_idx = 3;
|
|
|
|
// if(azi/PI*180>=TRACK_SECTION_4_START && azi/PI*180<TRACK_SECTION_5_START)
|
|
// (*trust_track)[loop_of_track].Track_section_idx = 4;
|
|
|
|
// if(azi/PI*180>=TRACK_SECTION_5_START && azi/PI*180<TRACK_SECTION_6_START)
|
|
// (*trust_track)[loop_of_track].Track_section_idx = 5;
|
|
|
|
// if(azi/PI*180>=TRACK_SECTION_6_START && azi/PI*180<TRACK_SECTION_7_START)
|
|
// (*trust_track)[loop_of_track].Track_section_idx = 6;
|
|
|
|
// if(azi/PI*180>=TRACK_SECTION_7_START && azi/PI*180<TRACK_SECTION_8_START)
|
|
// (*trust_track)[loop_of_track].Track_section_idx = 7;
|
|
|
|
// if(azi/PI*180>=TRACK_SECTION_8_START && azi/PI*180<TRACK_SECTION_9_START)
|
|
// (*trust_track)[loop_of_track].Track_section_idx = 8;
|
|
|
|
// if(azi/PI*180>=TRACK_SECTION_9_START || azi/PI*180<TRACK_SECTION_1_START)
|
|
// (*trust_track)[loop_of_track].Track_section_idx = 9;
|
|
|
|
}
|
|
// }
|
|
|
|
}
|
|
|
|
|
|
|
|
//高度维更新
|
|
void Track_Asso:: track_hight_update(int updata_track_index, //更新的航迹号
|
|
int asso_point_index, //点迹号
|
|
QVector <Trust_Track> *trust_track //航迹
|
|
)
|
|
{
|
|
|
|
|
|
if(updata_track_index<=trust_track->size() && asso_point_index<=point_process.size())
|
|
{
|
|
|
|
(*trust_track)[updata_track_index-1].Hight_smooth.push_back(point_process[asso_point_index-1].Height);
|
|
|
|
|
|
int height_win_length;
|
|
|
|
if((*trust_track)[updata_track_index-1].range_point <= 1000)
|
|
{
|
|
height_win_length = H_F_WIN_LEN+2;
|
|
}
|
|
else if((*trust_track)[updata_track_index-1].range_point <= 2000 && (*trust_track)[updata_track_index-1].range_point > 1000)
|
|
{
|
|
height_win_length = H_F_WIN_LEN+3;
|
|
}
|
|
else if((*trust_track)[updata_track_index-1].range_point <= 4000 && (*trust_track)[updata_track_index-1].range_point > 2000)
|
|
{
|
|
height_win_length = H_F_WIN_LEN+4;
|
|
}
|
|
else
|
|
{
|
|
height_win_length = H_F_WIN_LEN+6;
|
|
}
|
|
|
|
double sum=0;
|
|
if((*trust_track)[updata_track_index-1].Hight_smooth.size()<height_win_length)
|
|
{
|
|
|
|
for (int i=0;i<(*trust_track)[updata_track_index-1].Hight_smooth.size();i++)
|
|
{
|
|
sum=sum+(*trust_track)[updata_track_index-1].Hight_smooth[i];
|
|
|
|
}
|
|
|
|
(*trust_track)[updata_track_index-1].Height=sum/(*trust_track)[updata_track_index-1].Hight_smooth.size();
|
|
|
|
}
|
|
else if((*trust_track)[updata_track_index-1].Hight_smooth.size()>=height_win_length)
|
|
{
|
|
|
|
int N=(*trust_track)[updata_track_index-1].Hight_smooth.size();
|
|
for (int i=0;i<height_win_length;i++)
|
|
{
|
|
sum=sum+(*trust_track)[updata_track_index-1].Hight_smooth[N-1-i];
|
|
}
|
|
(*trust_track)[updata_track_index-1].Height=sum/height_win_length;
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
Track_Asso::Track_Asso()
|
|
{
|
|
Pt[0][0]=0.8;Pt[0][1]=0.15;Pt[0][2]=0.05; //IMM 转移概率
|
|
Pt[1][0]=0.3;Pt[1][1]=0.4;Pt[1][2]=0.3;
|
|
Pt[2][0]=0.05;Pt[2][1]=0.15;Pt[2][2]=0.8;
|
|
|
|
|
|
};
|