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/kalman.cpp
+19 -51
View File
@@ -1,14 +1,18 @@
#include "kalman.h"
#include "coor_trans.h"
#include "parameters.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
using namespace std;
static double wrapAnglePi(double a)
{
while (a > PI) a -= 2*PI;
while (a < -PI) a += 2*PI;
return a;
}
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4])
@@ -31,7 +35,6 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub
R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R[1][0]=R[0][1];
P[0][0]=R[0][0];
P[0][1]=R[0][0]/T;
P[0][2]=R[0][1];
@@ -47,7 +50,6 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub
P[2][2]=R[1][1];
P[2][3]=R[1][1]/T;
P[3][0]=R[0][1]/T;
P[3][1]=2*R[0][1]/pow(T,2);
P[3][2]=R[1][1]/T;
@@ -55,7 +57,6 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub
}
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
{
@@ -111,8 +112,6 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
Matrix2d S;
S=H*P_pred*H.transpose()+R;
//d
Vector2d Z_presnet= Vector2d(Z[0],Z[1]);
Vector2d delta_z;
@@ -123,8 +122,6 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
}
void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6])
{
double x0=Z0[0];
@@ -141,8 +138,6 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d
X[4]=(y2-y1)/T2 ;
X[5]=((y2-y1)/T2-(y1-y0)/T1)/((T2+T1)/2);
double R0[2][2];
double R1[2][2];
double R2[2][2];
@@ -157,14 +152,12 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d
R0[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R0[1][0]=R0[0][1];
Coor_trans.cart2polar(Z1[0],Z1[1],&rho,&theta);
R1[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R1[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R1[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R1[1][0]=R1[0][1];
Coor_trans.cart2polar(Z2[0],Z2[1],&rho,&theta);
R2[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R2[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
@@ -200,9 +193,6 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d
};
double kalman::d_cal(double Z[2],double X[6],double P[6][6])
{
VectorXd X_pred(6);
@@ -241,13 +231,11 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6])
Matrix2d S;
S=H*P_pred*H.transpose()+R;
//d
Vector2d delta_z;
delta_z=Z_mea-Z_pred;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
};
@@ -273,7 +261,6 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
for (int j=0;j<6;j++)
P1(i,j)=P[i][j];
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
X_pred = F1*X1;
@@ -318,15 +305,14 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
Vector3d Z_mea(Z[0],Z[1],Z[2]);
Vector3d delta_z;
delta_z=Z_mea-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
}
double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind)
{
@@ -348,7 +334,6 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
for (int j=0;j<6;j++)
P1(i,j)=P[i][j];
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
Vector3d Z_mea(Z[0],Z[1],vr);
@@ -356,7 +341,6 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
X_pred = F1*X1;
P_pred = F1*P1*F1.transpose()+Q1;
//Z(k+1|k)
double x=X_pred[0];
double vx=X_pred[1];
@@ -396,18 +380,16 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
Matrix3d S;
S=H*P_pred*H.transpose()+R;
// bind_speed
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d delta_z;
delta_z=Z_mea-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
};
@@ -435,7 +417,6 @@ void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6]
VectorXd X1_pred(6);
MatrixXd P1_pred(6,6);
X1_pred = F1*X1;
P1_pred = F1*P1*F1.transpose()+Q1;
@@ -446,11 +427,10 @@ void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6]
for(int j=0;j<6;j++)
P_pred[i][j]=P1_pred(i,j);
};
void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],double P_cur[6][6],double Z[3],
double X_filter[6],double P_filter[6][6],double S_filter[2][2],
double X_filter[6],double P_filter[6][6],double S_filter[3][3],
double prt,double freq_ind )
{
MatrixXd F1(6,6);
@@ -474,12 +454,9 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
X_pred = F1*X1;
P_pred = F1*P1*F1.transpose()+Q1;
Vector3d Z_mea(Z[0],Z[1],Z[2]);
Vector3d Z_pred;
@@ -521,10 +498,10 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
//bind_speed
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d delta_z;
delta_z=Z_mea-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
//X(k+1|k+1)
@@ -536,10 +513,8 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
MatrixXd I;
I.setIdentity(6, 6);
P=(I-K*H)*P_pred;
for (int i=0;i<6;i++)
X_filter[i]=X(i);
@@ -547,13 +522,12 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
for (int j=0;j<6;j++)
P_filter[i][j]=P(i,j);
for (int i=0;i<2;i++)
for (int j=0;j<2;j++)
for (int i=0;i<3;i++)
for (int j=0;j<3;j++)
S_filter[i][j]=S(i,j);
}
void kalman::kalman_filter(double F[6][6], double Q[6][6],
double X_cur[6],double P_cur[6][6],
double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2])
@@ -579,16 +553,11 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
X_pred = F1*X1;
P_pred = F1*P1*F1.transpose()+Q1;
Vector2d Z_mea(Z[0],Z[1]);
//Z(k+1|k)
MatrixXd H(2,6);
H<< 1,0,0,0,0,0,
@@ -614,7 +583,6 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
Matrix2d S;
S=H*P_pred*H.transpose()+R;
//kalmam gain
MatrixXd K;
@@ -629,10 +597,8 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
MatrixXd I;
I.setIdentity(6, 6);
P=(I-K*H)*P_pred;
for (int i=0;i<6;i++)
X_filter[i]=X(i);
@@ -718,6 +684,7 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
Vector3d Z_presnet= Vector3d(Z[0],Z[1],Z[2]);
Vector3d delta_z;
delta_z=Z_presnet-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
double d=delta_z.transpose()*S.inverse()*delta_z;
@@ -754,7 +721,6 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
Vector3d Z_pred;
Z_pred=H*X_pred;
//P(k+1|k)
Matrix2d Q;
Q<< 0.03*0.03, 0,
@@ -803,6 +769,7 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr);
Vector3d delta_z;
delta_z=Z_presnet-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
double d=delta_z.transpose()*S.inverse()*delta_z;
@@ -810,11 +777,12 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
};
double kalman::Bind_speed(double prt,double freq_ind)
{
double freq=FREQ0+freq_ind*0.02;
if (prt <= 0.0 || freq <= 0.0)
return 1.0; // 避免除零；正常流程 PRI 不允许为 0
return 150000.0/(freq*prt);