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

更新：1、使用VS code+cmake重新编译，编译器保持不变；

Browse Source 
2、修复若干逻辑bug，具体参考BUG_FIX_REPORT.md

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-08-18 11:26:19 +08:00
parent 60ad5c136f
commit 01d28e0d28
49 changed files with 1452 additions and 957 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/track_asso.cpp
+71 -70
View File
@@ -1,19 +1,18 @@
#include "track_asso.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#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(QVector<PointRecv> *point_recv, //输入点迹
QVector <Trust_Track> *trust_track, //航迹文件
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 //雷达参数
@@ -28,7 +27,6 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
point_process[n-1].point_section_asso = 1;
}
//IMM算法
model_interaction(trust_track);
@@ -36,9 +34,8 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
model_output(trust_track);
//删除关联上的点
QVector <PointRecv>::iterator Iter;
std::vector <PointRecv>::iterator Iter;
for (Iter= point_process.begin(); Iter!=point_process.end();)
{
if((*Iter).Use_Flag==1 )
@@ -53,7 +50,7 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
}
//剩余点重新存入点迹
QVector<PointRecv>().swap((*point_recv));
std::vector<PointRecv>().swap((*point_recv));
for (int i=0;i<point_process.size();i++)
{
@@ -62,7 +59,7 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
}
//point_process清空
QVector<PointRecv>().swap(point_process);
std::vector<PointRecv>().swap(point_process);
//输出航迹
for (int i=0;i<trust_track->size();i++)
@@ -93,18 +90,15 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
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].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=(*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;}
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=Direction_Angle/PI*180;
//关联点信息
@@ -140,10 +134,9 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
}
//遍历所有航迹 进行多模型交互
void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
void Track_Asso:: model_interaction(std::vector <Trust_Track> *trust_track)
{
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
@@ -159,6 +152,9 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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];
@@ -170,7 +166,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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++)
{
@@ -179,7 +174,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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];
@@ -187,7 +181,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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];
@@ -195,7 +188,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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++)
{
@@ -211,7 +203,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
}
for (int i=0;i<6;i++)
for(int j=0;j<6;j++)
{
@@ -226,7 +217,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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++)
@@ -253,7 +243,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
}
}
// 产生 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)
{
@@ -281,8 +270,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
F[5][4]=0;
F[5][5]=exp(-alpha*delta_T);
//模型1
//Q
double q1=Work_Parameter.Model1_Q_slow;
@@ -301,7 +288,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
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;
@@ -343,7 +329,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
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;
@@ -381,7 +366,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
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;
@@ -391,10 +375,8 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
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],
@@ -415,10 +397,7 @@ void Track_Asso::IMM_d_cal(double v_track,
}
void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarPara Work_Parameter)
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct RadarPara Work_Parameter)
{
//存储关联信息
@@ -431,7 +410,7 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
int point_index;
int track_index;
};
QVector <asso_info> associated_info;
std::vector <asso_info> associated_info;
//遍历所有点迹航迹 计算点迹和航迹的统计距离
for (int loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
@@ -443,7 +422,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
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
@@ -523,8 +501,8 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
// d_h = 1;
// }
// qDebug() << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// qDebug() << h_track<<" "<<h_point<<" "<<d_h;
// std::cout << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// std::cout << h_track<<" "<<h_point<<" "<<d_h;
// d_h = 1;
//计算俯仰门限
@@ -565,12 +543,12 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
//小于关联门限 保存关联信息
if(
( r_track<=500 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000) && 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) && d_p && d_r)//
( 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)//
)
{
//qDebug() << "asooooooooooo";
//std::cout << "asooooooooooo";
struct asso_info associated_info_tmp;
associated_info_tmp.d1=d1;
@@ -591,7 +569,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
// }
}
//最近邻法关联
int associated_num=associated_info.size();
for (int i=0;i<trust_track->size();i++)
@@ -632,30 +609,57 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
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)
{
// 乱序/重复时间戳：不作为有效关联，不标记点迹已使用、不刷新航迹新鲜度
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[2][2],S2[2][2],S3[2][2];
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 );
Matrix2f S1_out,S2_out,S3_out;
for (int ii=0;ii<2;ii++)
for (int jj=0;jj<2;jj++)
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)=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 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=1/sqrt(2*PI*det_S2)*exp(-0.5*d2);
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=1/sqrt(2*PI*det_S3)*exp(-0.5*d3);
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];
@@ -668,9 +672,14 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
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]);
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++)
@@ -691,7 +700,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
(*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;
@@ -746,7 +754,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
}
}
//未关联上的航迹 进行外推 tws
for (int i =0; i<(*trust_track).size();i++ )
{
@@ -797,7 +804,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
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].GNSS_time = (*trust_track)[i].GNSS_time+delta_T*1000.0; //更新航迹时间
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间
@@ -805,12 +811,9 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
}
}
}
void Track_Asso:: model_output(QVector <Trust_Track> *trust_track)
void Track_Asso:: model_output(std::vector <Trust_Track> *trust_track)
{
for(int loop_of_track=0; loop_of_track<trust_track->size(); loop_of_track++)
{
@@ -861,7 +864,6 @@ void Track_Asso:: model_output(QVector <Trust_Track> *trust_track)
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];
@@ -910,22 +912,18 @@ void Track_Asso:: model_output(QVector <Trust_Track> *trust_track)
}
//高度维更新
void Track_Asso:: track_hight_update(int updata_track_index, //更新的航迹号
int asso_point_index, //点迹号
QVector <Trust_Track> *trust_track //航迹
std::vector <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)
@@ -971,12 +969,15 @@ void Track_Asso:: track_hight_update(int updata_
}
};
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();
}