waiwaylee/radar_data_process
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
15215926c50a0091af4b26844bdb7878a0a69d05
radar_data_process/data_process_class_dll/track_asso.cpp
T
waiwaylee 15215926c5 更新：1、增加日志模块，将标准输出信息写入到日志文件； 
2、将RadarPara中DATA_RATE_TAS的单位由Hz改为s；
3、修复TAS_Ctrl::tas_beam_output函数中给Tracking_beam->TAS_track_index赋值错误的问题；

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
2026-09-07 19:16:32 +08:00

987 lines
37 KiB
C++
Raw Blame History

#include "track_asso.h"
#include "kalman.h"
#include "coor_trans.h"
#include "rdp_log.h"
#include <cmath>
#include <cstring>
#include <vector>
#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(std::vector<PointRecv> *point_recv, //输入点迹
std::vector <Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter //雷达参数
)
{
//取出对应点迹区的点
for (size_t 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);
//删除关联上的点
std::vector <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++;
}
}
//剩余点重新存入点迹
std::vector<PointRecv>().swap((*point_recv));
for (size_t i=0;i<point_process.size();i++)
{
(*point_recv).push_back(point_process[i]);
}
//point_process清空
std::vector<PointRecv>().swap(point_process);
//输出航迹
for (size_t 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=static_cast<float>(r);
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=static_cast<float>(azi/PI*180);
if((*trust_track)[i].Height<r)
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>(asin((*trust_track)[i].Height/r)/PI*180);
else
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*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 =
static_cast<float>(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 = static_cast<float>((*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=static_cast<float>((*trust_track)[i].T_track/1000.0);
Trust_Track_Output[*Trust_track_num_Output-1][0].GNSS_time=(*trust_track)[i].GNSS_time;
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=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
double Direction_Angle;
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
//关联点信息
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=static_cast<float>((*trust_track)[i].range_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=static_cast<float>((*trust_track)[i].azi_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=static_cast<float>((*trust_track)[i].elev_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=static_cast<float>((*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 = static_cast<float>((*trust_track)[i].prf_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*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].z = static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*trust_track)[i].elev_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].x=static_cast<float>((*trust_track)[i].X[0]);
Trust_Track_Output[*Trust_track_num_Output-1][0].y=static_cast<float>((*trust_track)[i].X[3]);
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=static_cast<float>((*trust_track)[i].X[1]);
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=static_cast<float>((*trust_track)[i].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = static_cast<float>((*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(std::vector <Trust_Track> *trust_track)
{
for (size_t loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
// if((*trust_track)[loop_of_track].manual_tracking_flag == 0)
// {
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];
if (c[0] <= 1e-12) c[0] = 1e-12;
if (c[1] <= 1e-12) c[1] = 1e-12;
if (c[2] <= 1e-12) c[2] = 1e-12;
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(std::vector <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;
};
std::vector <asso_info> associated_info;
//遍历所有点迹航迹 计算点迹和航迹的统计距离
for (size_t loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
{
// if((*trust_track)[loop_of_track].manual_tracking_flag==0)
// {
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
for (size_t loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
{
//时间差
double delta_T = (point_process[loop_of_point].CPI_Time - (*trust_track)[loop_of_track].T_track)/1000.0;
//航迹X P
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
memcpy(X3,(*trust_track)[loop_of_track].X3,6*sizeof(double));
memcpy(P1,(*trust_track)[loop_of_track].P1,6*6*sizeof(double));
memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
double v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
double h_track = (*trust_track)[loop_of_track].Height;
double r_track = sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));
//量测信息
double 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;
// }
// std::cout << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// std::cout << h_track<<" "<<h_point<<" "<<d_h;
// d_h = 1;
//计算俯仰门限
bool d_p = true;
// double p_track = atan2(h_track, r_track); //航迹俯仰
// double p_point = asin(h_point/point_process[loop_of_point].Range); //点迹俯仰
// if( (r_track > 3000 && abs(p_track - p_point)*180/PI <= 7) || \
// (r_track > 1000 && r_track <= 3000 && abs(p_track - p_point)*180/PI <= 5) || \
// (r_track <= 1000 && abs(p_track - p_point)*180/PI <= 10)){
// d_p = true;
// }
//计算距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差
double& t_x0 = (*trust_track)[loop_of_track].X[0];
double& t_y0 = (*trust_track)[loop_of_track].X[3];
double a_track = atan2(t_y0, t_x0); //航迹方位角
double& r_point = point_process[loop_of_point].Range; //点迹距离
double a_delta = abs(a_track - point_process[loop_of_point].Azimuth); //方位差
if(a_delta > PI){
a_delta = 2*PI - a_delta;
}
double R = sqrt(r_track * r_track + r_point*r_point - 2*r_track*r_point*cos(a_delta));
double VT = v_track*delta_T;
bool d_r = true;
if(VT < 100 && R > 300){
//VT<100m，使用固定门限300m
d_r = false;
}else if((VT>=100 && VT<=300 && R/VT>3)){
d_r = false;
}else if(VT>300 && R > 1200){
d_r = false;
}
//小于关联门限 保存关联信息
if(
( r_track<=500 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000.0) && d_p && d_r)//
||( (r_track>500&&r_track<1000) && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/100.0) && d_p && d_r)//
||( r_track>=1000 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD) && d_p && d_r)//
)
{
//std::cout << "asooooooooooo";
struct asso_info associated_info_tmp;
associated_info_tmp.d1=d1;
associated_info_tmp.d2=d2;
associated_info_tmp.d3=d3;
associated_info_tmp.point_index=static_cast<int>(loop_of_point)+1;
associated_info_tmp.track_index=static_cast<int>(loop_of_track)+1;
if(d1<=d2&&d1<=d3)
associated_info_tmp.d_min=d1;
else if(d2<=d1&& d2<=d3)
associated_info_tmp.d_min=d2;
else
associated_info_tmp.d_min=d3;
associated_info.push_back(associated_info_tmp);
}
}
// }
}
//最近邻法关联
int associated_num=associated_info.size();
for (size_t 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)
{
RDP_LOG<<"delta_T <= 0, track_index: "<<track_index<<", point_index: "<<point_index<<", delta_T: "<<delta_T<<std::endl;
// 乱序/重复时间戳：不作为有效关联，不标记点迹已使用、不刷新航迹新鲜度
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();
continue;
}
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[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
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 );
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;jj++)
{
S1_out(ii,jj)=static_cast<float>(S1[ii][jj]);
S2_out(ii,jj)=static_cast<float>(S2[ii][jj]);
S3_out(ii,jj)=static_cast<float>(S3[ii][jj]);
}
double det_S1=S1_out.determinant();
double Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0;
double det_S2=S2_out.determinant();
double Possibility2=(det_S2>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S2)*exp(-0.5*d2)):0.0;
double det_S3=S3_out.determinant();
double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0;
//更新模型概率
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];
double u_den = Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2];
if (u_den > 1e-12)
{
(*trust_track)[track_index-1].u[0]=Possibility1*c[0]/u_den;
(*trust_track)[track_index-1].u[1]=Possibility2*c[1]/u_den;
(*trust_track)[track_index-1].u[2]=Possibility3*c[2]/u_den;
}
// 若分母异常，保持上一拍模型概率
//更新 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].GNSS_time = point_process[point_index-1].GNSS_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 (size_t 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 = static_cast<long long>((*trust_track)[i].T_track+delta_T*1000.0); //更新航迹时间
(*trust_track)[i].GNSS_time = static_cast<long long>((*trust_track)[i].GNSS_time+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(std::vector <Trust_Track> *trust_track)
{
for(size_t loop_of_track=0; loop_of_track<trust_track->size(); loop_of_track++)
{
// if((*trust_track)[loop_of_track].manual_tracking_flag==0)
// {
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, //点迹号
std::vector <Trust_Track> *trust_track //航迹
)
{
if(updata_track_index<=static_cast<int>(trust_track->size()) && asso_point_index<=static_cast<int>(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()<static_cast<size_t>(height_win_length))
{
for (size_t 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()>=static_cast<size_t>(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;
};
void Track_Asso::reset()
{
point_process.clear();
}
Reference in New Issue View Git Blame Copy Permalink