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 10:36:14 +08:00
parent 0e325fb153
commit 423c200659
39 changed files with 19638 additions and 945 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/kalman.cpp
+69 -69
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])
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)
{
X[0]=Z1[0];
@@ -26,13 +26,13 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z1[0],Z1[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
double R[2][2];
R[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));
R[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));
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[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R[1][0]=R[0][1];
P[0][0]=R[0][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)
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T, const RDP_parameter& rdp_parameter)
{
//X(k+1|k)
@@ -92,13 +92,13 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix2d R;
R(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));
R(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));
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(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
Matrix4d P_present;
@@ -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])
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)
{
double x0=Z0[0];
double y0=Z0[1];
@@ -145,23 +145,23 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z0[0],Z0[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
R0[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));
R0[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));
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);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
R0[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R0[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R0[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*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[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R1[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R1[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*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));
R2[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);
R2[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R2[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R2[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R2[1][0]=R2[0][1];
double P11[3][3]={{R2[0][0], R2[0][0]/T2, (R2[0][0]/T2)/((T2+T1)/2)} ,
@@ -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])
double kalman::d_cal(double Z[2],double X[6],double P[6][6], const RDP_parameter& rdp_parameter)
{
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
@@ -218,13 +218,13 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6])
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix2d R;
R(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));
R(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));
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(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
//S
@@ -240,7 +240,7 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6])
};
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)
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)
{
MatrixXd F1(6,6);
@@ -284,9 +284,9 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
H(2,3) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,4)=-y/sqrt(x*x+y*y); H(2,5)=0;
Matrix3d R;
R(0,0)=SIGMA_R*SIGMA_R;
R(1,1)=SIGMA_A*SIGMA_A;
R(2,2)=SIGMA_V*SIGMA_V;
R(0,0)=rdp_parameter.sigma_range*rdp_parameter.sigma_range;
R(1,1)=rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(0,1)=0;
R(1,0)=0;
R(2,0)=0;
@@ -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);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//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)
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)
{
MatrixXd F1(6,6);
@@ -362,15 +362,15 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix3d R;
R(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));
R(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));
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(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
R(2,2)=SIGMA_V*SIGMA_V;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(2,0)=0;
R(2,1)=0;
R(0,2)=0;
@@ -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);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//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 )
double prt,double freq_ind, const RDP_parameter& rdp_parameter )
{
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
@@ -477,9 +477,9 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
H(2,3) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,4)=-y/sqrt(x*x+y*y); H(2,5)=0;
Matrix3d R;
R(0,0)=SIGMA_R*SIGMA_R;
R(1,1)=SIGMA_A*SIGMA_A;
R(2,2)=SIGMA_V*SIGMA_V;
R(0,0)=rdp_parameter.sigma_range*rdp_parameter.sigma_range;
R(1,1)=rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(0,1)=0;
R(1,0)=0;
R(2,0)=0;
@@ -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);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//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])
double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2], const RDP_parameter& rdp_parameter)
{
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
@@ -570,13 +570,13 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix2d R;
R(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));
R(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));
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(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
//S
@@ -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)
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)
{
//X(k+1|k)
@@ -663,9 +663,9 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
H(2,2) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,3)=-y/sqrt(x*x+y*y);
Matrix3d R;
R(0,0)=SIGMA_R*SIGMA_R;
R(1,1)=SIGMA_A*SIGMA_A;
R(2,2)=SIGMA_V*SIGMA_V;
R(0,0)=rdp_parameter.sigma_range*rdp_parameter.sigma_range;
R(1,1)=rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(0,1)=0;
R(1,0)=0;
R(2,0)=0;
@@ -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);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//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)
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)
{
//X(k+1|k)
Matrix4d F;
@@ -744,15 +744,15 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix3d R;
R(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));
R(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));
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(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
R(2,2)=SIGMA_V*SIGMA_V;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(2,0)=0;
R(2,1)=0;
R(0,2)=0;
@@ -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);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//d
Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr);
@@ -777,9 +777,9 @@ 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 kalman::Bind_speed(double prt,double freq_ind, const RDP_parameter& rdp_parameter)
{
double freq=FREQ0+freq_ind*0.02;
double freq=rdp_parameter.freq_0_GHz+freq_ind*0.02;
if (prt <= 0.0 || freq <= 0.0)
return 1.0; // 避免除零；正常流程 PRI 不允许为 0