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

更新：1、将参数配置信息对象改为全局可访问，并避免通过函数参数传入；

Browse Source 
Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-10-27 15:56:02 +08:00
parent 76b0b460a5
commit 7e4cb2922d
34 changed files with 202 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/kalman.cpp
+16 -16
View File
@@ -15,7 +15,7 @@ static double wrapAnglePi(double a)
}
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4], const RDP_parameter& rdp_parameter)
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4])
{
X[0]=Z1[0];
@@ -57,7 +57,7 @@ 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, const RDP_parameter& rdp_parameter)
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
{
//X(k+1|k)
@@ -122,7 +122,7 @@ 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], const RDP_parameter& rdp_parameter)
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];
double y0=Z0[1];
@@ -193,7 +193,7 @@ 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], const RDP_parameter& rdp_parameter)
double kalman::d_cal(double Z[2],double X[6],double P[6][6])
{
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
@@ -240,7 +240,7 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6], const RDP_parameter
};
double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind, const RDP_parameter& rdp_parameter)
double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind)
{
MatrixXd F1(6,6);
@@ -299,7 +299,7 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
S=H*P_pred*H.transpose()+R;
//bind_speed
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d Z_mea(Z[0],Z[1],Z[2]);
@@ -313,7 +313,7 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
}
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, const RDP_parameter& rdp_parameter)
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)
{
MatrixXd F1(6,6);
@@ -381,7 +381,7 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
S=H*P_pred*H.transpose()+R;
// bind_speed
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d delta_z;
@@ -431,7 +431,7 @@ void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6]
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[3][3],
double prt,double freq_ind, const RDP_parameter& rdp_parameter )
double prt,double freq_ind )
{
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
@@ -496,7 +496,7 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
K=P_pred*H.transpose()*S.inverse();
//bind_speed
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d delta_z;
@@ -530,7 +530,7 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
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], const RDP_parameter& rdp_parameter)
double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2])
{
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
@@ -612,7 +612,7 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
};
double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind, const RDP_parameter& rdp_parameter)
double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind)
{
//X(k+1|k)
@@ -678,7 +678,7 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
S=H*P_pred*H.transpose()+R;
//bind_speed
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d Z_presnet= Vector3d(Z[0],Z[1],Z[2]);
@@ -692,7 +692,7 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
}
double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind, const RDP_parameter& rdp_parameter)
double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind)
{
//X(k+1|k)
Matrix4d F;
@@ -763,7 +763,7 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
S=H*P_pred*H.transpose()+R;
//bind_speed
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr);
@@ -777,7 +777,7 @@ 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, const RDP_parameter& rdp_parameter)
double kalman::Bind_speed(double prt,double freq_ind)
{
double freq=rdp_parameter.freq_0_GHz+freq_ind*0.02;