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

更新：1、修复kalman.cpp中部分参数命名冲突的问题；

Browse Source 
2、TWS点迹关联时增加点航距离门限，限制部分突然关联到很远的点迹的问题。

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-06-18 09:16:04 +08:00
parent 167fa793e9
commit 958ff6b11b
38 changed files with 8774 additions and 1775 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/kalman.cpp
+203 -203
View File
@@ -11,7 +11,7 @@ using namespace std;
void kalman:: kalman_filter_init_2dots(float Z0[2], float Z1[2], float T,float X[4], float P[4][4])
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];
@@ -19,13 +19,13 @@ void kalman:: kalman_filter_init_2dots(float Z0[2], float Z1[2], float T,float X
X[2]=Z1[1];
X[3]=(Z1[1]-Z0[1])/T;
float rho,theta;
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z1[0],Z1[1],&rho,&theta);
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
float R[2][2];
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);
@@ -56,68 +56,68 @@ void kalman:: kalman_filter_init_2dots(float Z0[2], float Z1[2], float T,float X
}
float kalman::d_cal_track_init(float Z[2],float X[4],float P[4][4],float T)
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
{
//X(k+1|k)
Matrix4f F;
Matrix4d F;
F<< 1, T, 0, 0,
0, 1, 0, 0,
0, 0, 1, T,
0, 0, 0, 1;
Vector4f X_present = Vector4f(X[0],X[1],X[2],X[3]);
Vector4f X_pred;
Vector4d X_present = Vector4d(X[0],X[1],X[2],X[3]);
Vector4d X_pred;
X_pred=F*X_present;
//Z(k+1|k)
MatrixXf H(2,4);
MatrixXd H(2,4);
H<< 1,0,0,0,
0,0,1,0;
Vector2f Z_pred;
Vector2d Z_pred;
Z_pred=H*X_pred;
//P(k+1|k)
Matrix2f Q;
Matrix2d Q;
Q<< 0.03*0.03, 0,
0, 0.03*0.03;
MatrixXf G(4,2);
MatrixXd G(4,2);
G<< T*T/2, 0,
T, 0,
0, T*T/2,
0, T;
float rho,theta;
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
Matrix2f R;
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(1,0)=R(0,1);
Matrix4f P_present;
Matrix4d P_present;
for (int i=0;i<4;i++)
for (int j=0;j<4;j++)
P_present(i,j)=P[i][j];
Matrix4f P_pred;
Matrix4d P_pred;
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
//S
Matrix2f S;
Matrix2d S;
S=H*P_pred*H.transpose()+R;
//d
Vector2f Z_presnet= Vector2f(Z[0],Z[1]);
Vector2f delta_z;
Vector2d Z_presnet= Vector2d(Z[0],Z[1]);
Vector2d delta_z;
delta_z=Z_presnet-Z_pred;
float d=delta_z.transpose()*S.inverse()*delta_z;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
@@ -125,14 +125,14 @@ float kalman::d_cal_track_init(float Z[2],float X[4],float P[4][4],float T)
void kalman::kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], float T1,float T2, float X[6], float 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])
{
float x0=Z0[0];
float y0=Z0[1];
float x1=Z1[0];
float y1=Z1[1];
float x2=Z2[0];
float y2=Z2[1];
double x0=Z0[0];
double y0=Z0[1];
double x1=Z1[0];
double y1=Z1[1];
double x2=Z2[0];
double y2=Z2[1];
X[0]=x2;
X[1]=(x2-x1)/T2 ;
@@ -143,15 +143,15 @@ void kalman::kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], floa
float R0[2][2];
float R1[2][2];
float R2[2][2];
double R0[2][2];
double R1[2][2];
double R2[2][2];
float rho,theta;
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z0[0],Z0[1],&rho,&theta);
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
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);
@@ -171,15 +171,15 @@ void kalman::kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], floa
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[1][0]=R2[0][1];
float P11[3][3]={{R2[0][0], R2[0][0]/T2, (R2[0][0]/T2)/((T2+T1)/2)} ,
double P11[3][3]={{R2[0][0], R2[0][0]/T2, (R2[0][0]/T2)/((T2+T1)/2)} ,
{R2[0][0]/T2, (R2[0][0]+R1[0][0])/pow(T2,2), ((R2[0][0]+R1[0][0])/pow(T2,2)+R1[0][0]/(T1*T2))/((T1+T2)/2)} ,
{(R2[0][0]/T2)/((T2+T1)/2), ((R2[0][0]+R1[0][0])/pow(T2,2)+R1[0][0]/(T1*T2))/((T1+T2)/2) , 4*((R2[0][0]+R1[0][0])/pow(T2,2)+(R1[0][0]+R0[0][0])/pow(T1,2)+2*R1[0][0]/(T1*T2))/pow(T1+T2,2)}};
float P12[3][3]={{R2[0][1], R2[0][1]/T2, (R2[0][1]/T2)/((T2+T1)/2)} ,
double P12[3][3]={{R2[0][1], R2[0][1]/T2, (R2[0][1]/T2)/((T2+T1)/2)} ,
{R2[0][1]/T2, (R2[0][1]+R1[0][1])/pow(T2,2), ((R2[0][1]+R1[0][1])/pow(T2,2)+R1[0][1]/(T1*T2))/((T1+T2)/2)} ,
{(R2[0][1]/T2)/((T2+T1)/2), ((R2[0][1]+R1[0][1])/pow(T2,2)+R1[0][1]/(T1*T2))/((T1+T2)/2) , 4*((R2[0][1]+R1[0][1])/pow(T2,2)+(R1[0][1]+R0[0][1])/pow(T1,2)+2*R1[0][1]/(T1*T2))/pow(T1+T2,2)}};
float P22[3][3]={{R2[1][1], R2[1][1]/T2, (R2[1][1]/T2)/((T2+T1)/2)} ,
double P22[3][3]={{R2[1][1], R2[1][1]/T2, (R2[1][1]/T2)/((T2+T1)/2)} ,
{R2[1][1]/T2, (R2[1][1]+R1[1][1])/pow(T2,2), ((R2[1][1]+R1[1][1])/pow(T2,2)+R1[1][1]/(T1*T2))/((T1+T2)/2)} ,
{(R2[1][1]/T2)/((T2+T1)/2), ((R2[1][1]+R1[1][1])/pow(T2,2)+R1[1][1]/(T1*T2))/((T1+T2)/2) , 4*((R2[1][1]+R1[1][1])/pow(T2,2)+(R1[1][1]+R0[1][1])/pow(T1,2)+2*R1[1][1]/(T1*T2))/pow(T1+T2,2)}};
for (int i=0;i<3;i++)
@@ -203,11 +203,11 @@ void kalman::kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], floa
float kalman::d_cal(float Z[2],float X[6],float P[6][6])
double kalman::d_cal(double Z[2],double X[6],double P[6][6])
{
VectorXf X_pred(6);
MatrixXf P_pred(6,6);
Vector2f Z_mea(Z[0],Z[1]);
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
Vector2d Z_mea(Z[0],Z[1]);
for (int i=0;i<6;i++)
X_pred(i)=X[i];
@@ -217,46 +217,46 @@ float kalman::d_cal(float Z[2],float X[6],float P[6][6])
P_pred(i,j)=P[i][j];
//Z(k+1|k)
MatrixXf H(2,6);
MatrixXd H(2,6);
H<< 1,0,0,0,0,0,
0,0,0,1,0,0;
Vector2f Z_pred;
Vector2d Z_pred;
Z_pred=H*X_pred;
//R
float rho,theta;
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
Matrix2f R;
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(1,0)=R(0,1);
//S
Matrix2f S;
Matrix2d S;
S=H*P_pred*H.transpose()+R;
//d
Vector2f delta_z;
Vector2d delta_z;
delta_z=Z_mea-Z_pred;
float d=delta_z.transpose()*S.inverse()*delta_z;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
};
float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],float P[6][6],float prt,float 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)
{
MatrixXf F1(6,6);
MatrixXf Q1(6,6);
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
for (int i=0;i<6;i++)
for (int j=0;j<6;j++)
@@ -264,8 +264,8 @@ float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],floa
F1(i,j) = F[i][j];
Q1(i,j) = Q[i][j] ;
}
VectorXf X1(6);
MatrixXf P1(6,6);
VectorXd X1(6);
MatrixXd P1(6,6);
for (int i=0;i<6;i++)
X1(i)=X[i];
@@ -274,29 +274,29 @@ float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],floa
P1(i,j)=P[i][j];
VectorXf X_pred(6);
MatrixXf P_pred(6,6);
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
X_pred = F1*X1;
P_pred = F1*P1*F1.transpose()+Q1;
Vector3f Z_pred;
float x=X_pred(0);
float vx=X_pred(1);
float y=X_pred(3);
float vy=X_pred(4);
Vector3d Z_pred;
double x=X_pred(0);
double vx=X_pred(1);
double y=X_pred(3);
double vy=X_pred(4);
Z_pred(0) = sqrt(x*x+y*y);
Z_pred(1)=atan2(y,x);
if(Z_pred(1)<0)
Z_pred(1)=Z_pred(1)+2*PI;
Z_pred(2)= -(x*vx+y*vy)/sqrt(x*x+y*y);
MatrixXf H(3,6);
MatrixXd H(3,6);
H(0,0) = x/sqrt(x*x+y*y); H(0,1)=0; H(0,2)=0; H(0,3)=y/sqrt(x*x+y*y); H(0,4)=0; H(0,5)=0;
H(1,0) = -y/(x*x+y*y); H(1,1)=0; H(1,2)=0; H(1,3)=x/(x*x+y*y); H(1,4)=0; H(1,5)=0;
H(2,0) = -y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,1)=-x/sqrt(x*x+y*y); H(2,2)=0;
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;
Matrix3f R;
Matrix3d R;
R(0,0)=SIGMA_R*SIGMA_R;
R(1,1)=SIGMA_A*SIGMA_A;
R(2,2)=SIGMA_V*SIGMA_V;
@@ -308,18 +308,18 @@ float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],floa
R(1,2)=0;
//S
Matrix3f S;
Matrix3d S;
S=H*P_pred*H.transpose()+R;
//bind_speed
float v_bind=Bind_speed(prt, freq_ind);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3f Z_mea(Z[0],Z[1],Z[2]);
Vector3f delta_z;
Vector3d Z_mea(Z[0],Z[1],Z[2]);
Vector3d delta_z;
delta_z=Z_mea-Z_pred;
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
float d=delta_z.transpose()*S.inverse()*delta_z;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
@@ -327,11 +327,11 @@ float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],floa
}
float kalman:: d_cal_with_doppler(float F[6][6], float Q[6][6] , float Z[2],float X[6],float P[6][6],float vr,float prt,float 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)
{
MatrixXf F1(6,6);
MatrixXf Q1(6,6);
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
for (int i=0;i<6;i++)
for (int j=0;j<6;j++)
@@ -339,8 +339,8 @@ float kalman:: d_cal_with_doppler(float F[6][6], float Q[6][6] , float Z[2],floa
F1(i,j) = F[i][j];
Q1(i,j) = Q[i][j] ;
}
VectorXf X1(6);
MatrixXf P1(6,6);
VectorXd X1(6);
MatrixXd P1(6,6);
for (int i=0;i<6;i++)
X1(i)=X[i];
@@ -349,39 +349,39 @@ float kalman:: d_cal_with_doppler(float F[6][6], float Q[6][6] , float Z[2],floa
P1(i,j)=P[i][j];
VectorXf X_pred(6);
MatrixXf P_pred(6,6);
Vector3f Z_mea(Z[0],Z[1],vr);
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
Vector3d Z_mea(Z[0],Z[1],vr);
X_pred = F1*X1;
P_pred = F1*P1*F1.transpose()+Q1;
//Z(k+1|k)
float x=X[0];
float vx=X[1];
float y=X[3];
float vy=X[4];
float h31=-y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y));
float h32=-x/sqrt(x*x+y*y);
float h34=-x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y));
float h35=-y/sqrt(x*x+y*y);
MatrixXf H(3,6);
double x=X[0];
double vx=X[1];
double y=X[3];
double vy=X[4];
double h31=-y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y));
double h32=-x/sqrt(x*x+y*y);
double h34=-x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y));
double h35=-y/sqrt(x*x+y*y);
MatrixXd H(3,6);
H(0,0)=1;H(0,1)=0;H(0,2)=0;H(0,3)=0;H(0,4)=0;H(0,5)=0;
H(1,0)=0;H(1,1)=0;H(1,2)=0;H(1,3)=1;H(1,4)=0;H(1,5)=0;
H(2,0)=h31;H(2,1)=h32;H(2,2)=0;H(2,3)=h34;H(2,4)=h35;H(2,5)=0;
Vector3f Z_pred;
Vector3d Z_pred;
Z_pred=H*X_pred;
//R
float rho,theta;
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
Matrix3f R;
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);
@@ -393,29 +393,29 @@ float kalman:: d_cal_with_doppler(float F[6][6], float Q[6][6] , float Z[2],floa
R(1,2)=0;
//S
Matrix3f S;
Matrix3d S;
S=H*P_pred*H.transpose()+R;
//bind_speed
float v_bind=Bind_speed(prt, freq_ind);
// bind_speed
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3f delta_z;
Vector3d delta_z;
delta_z=Z_mea-Z_pred;
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
float d=delta_z.transpose()*S.inverse()*delta_z;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
};
void kalman::kalman_pred(float F[6][6], float Q[6][6] ,float X[6],float P[6][6],float X_pred[6],float P_pred[6][6])
void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6][6],double X_pred[6],double P_pred[6][6])
{
MatrixXf F1(6,6);
MatrixXf Q1(6,6);
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
for (int i=0;i<6;i++)
for (int j=0;j<6;j++)
@@ -423,8 +423,8 @@ void kalman::kalman_pred(float F[6][6], float Q[6][6] ,float X[6],float P[6][6],
F1(i,j) = F[i][j];
Q1(i,j) = Q[i][j] ;
}
VectorXf X1(6);
MatrixXf P1(6,6);
VectorXd X1(6);
MatrixXd P1(6,6);
for (int i=0;i<6;i++)
X1(i)=X[i];
@@ -432,8 +432,8 @@ void kalman::kalman_pred(float F[6][6], float Q[6][6] ,float X[6],float P[6][6],
for (int j=0;j<6;j++)
P1(i,j)=P[i][j];
VectorXf X1_pred(6);
MatrixXf P1_pred(6,6);
VectorXd X1_pred(6);
MatrixXd P1_pred(6,6);
X1_pred = F1*X1;
@@ -449,12 +449,12 @@ void kalman::kalman_pred(float F[6][6], float Q[6][6] ,float X[6],float P[6][6],
};
void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[6][6],float Z[3],
float X_filter[6],float P_filter[6][6],float S_filter[2][2],
float prt,float freq_ind )
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 prt,double freq_ind )
{
MatrixXf F1(6,6);
MatrixXf Q1(6,6);
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
for (int i=0;i<6;i++)
for (int j=0;j<6;j++)
@@ -462,17 +462,17 @@ void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[
F1(i,j) = F[i][j];
Q1(i,j) = Q[i][j] ;
}
VectorXf X1(6);
MatrixXf P1(6,6);
VectorXd X1(6);
MatrixXd P1(6,6);
for (int i=0;i<6;i++)
X1(i)=X[i];
X1(i)=X_cur[i];
for (int i=0;i<6;i++)
for (int j=0;j<6;j++)
P1(i,j)=P[i][j];
P1(i,j)=P_cur[i][j];
VectorXf X_pred(6);
MatrixXf P_pred(6,6);
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
X_pred = F1*X1;
@@ -480,26 +480,26 @@ void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[
Vector3f Z_mea(Z[0],Z[1],Z[2]);
Vector3d Z_mea(Z[0],Z[1],Z[2]);
Vector3f Z_pred;
float x=X_pred(0);
float vx=X_pred(1);
float y=X_pred(3);
float vy=X_pred(4);
Vector3d Z_pred;
double x=X_pred(0);
double vx=X_pred(1);
double y=X_pred(3);
double vy=X_pred(4);
Z_pred(0) = sqrt(x*x+y*y);
Z_pred(1)=atan2(y,x);
if(Z_pred(1)<0)
Z_pred(1)=Z_pred(1)+2*PI;
Z_pred(2)= -(x*vx+y*vy)/sqrt(x*x+y*y);
MatrixXf H(3,6);
MatrixXd H(3,6);
H(0,0) = x/sqrt(x*x+y*y); H(0,1)=0; H(0,2)=0; H(0,3)=y/sqrt(x*x+y*y); H(0,4)=0; H(0,5)=0;
H(1,0) = -y/(x*x+y*y); H(1,1)=0; H(1,2)=0; H(1,3)=x/(x*x+y*y); H(1,4)=0; H(1,5)=0;
H(2,0) = -y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,1)=-x/sqrt(x*x+y*y); H(2,2)=0;
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;
Matrix3f R;
Matrix3d R;
R(0,0)=SIGMA_R*SIGMA_R;
R(1,1)=SIGMA_A*SIGMA_A;
R(2,2)=SIGMA_V*SIGMA_V;
@@ -511,29 +511,29 @@ void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[
R(1,2)=0;
//S
Matrix3f S;
Matrix3d S;
S=H*P_pred*H.transpose()+R;
//kalmam gain
MatrixXf K;
MatrixXd K;
K=P_pred*H.transpose()*S.inverse();
//bind_speed
float v_bind=Bind_speed(prt, freq_ind);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3f delta_z;
Vector3d delta_z;
delta_z=Z_mea-Z_pred;
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
//X(k+1|k+1)
VectorXf X;
VectorXd X;
X=X_pred+K*delta_z;
//P(k+1|k+1)
MatrixXf P;
MatrixXf I;
MatrixXd P;
MatrixXd I;
I.setIdentity(6, 6);
@@ -554,12 +554,12 @@ void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[
}
void kalman::kalman_filter(float F[6][6], float Q[6][6],
float X[6],float P[6][6],
float Z[2],float X_filter[6],float P_filter[6][6],float S_filter[2][2])
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])
{
MatrixXf F1(6,6);
MatrixXf Q1(6,6);
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
for (int i=0;i<6;i++)
for (int j=0;j<6;j++)
@@ -567,17 +567,17 @@ void kalman::kalman_filter(float F[6][6], float Q[6][6],
F1(i,j) = F[i][j];
Q1(i,j) = Q[i][j] ;
}
VectorXf X1(6);
MatrixXf P1(6,6);
VectorXd X1(6);
MatrixXd P1(6,6);
for (int i=0;i<6;i++)
X1(i)=X[i];
X1(i)=X_cur[i];
for (int i=0;i<6;i++)
for (int j=0;j<6;j++)
P1(i,j)=P[i][j];
P1(i,j)=P_cur[i][j];
VectorXf X_pred(6);
MatrixXf P_pred(6,6);
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
X_pred = F1*X1;
@@ -585,48 +585,48 @@ void kalman::kalman_filter(float F[6][6], float Q[6][6],
Vector2f Z_mea(Z[0],Z[1]);
Vector2d Z_mea(Z[0],Z[1]);
//Z(k+1|k)
MatrixXf H(2,6);
MatrixXd H(2,6);
H<< 1,0,0,0,0,0,
0,0,0,1,0,0;
Vector2f Z_pred;
Vector2d Z_pred;
Z_pred=H*X_pred;
//R
float rho,theta;
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
Matrix2f R;
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(1,0)=R(0,1);
//S
Matrix2f S;
Matrix2d S;
S=H*P_pred*H.transpose()+R;
//kalmam gain
MatrixXf K;
MatrixXd K;
K=P_pred*H.transpose()*S.inverse();
//X(k+1|k+1)
VectorXf X;
VectorXd X;
X=X_pred+K*(Z_mea-Z_pred);
//P(k+1|k+1)
MatrixXf P;
MatrixXf I;
MatrixXd P;
MatrixXd I;
I.setIdentity(6, 6);
@@ -646,57 +646,57 @@ void kalman::kalman_filter(float F[6][6], float Q[6][6],
};
float kalman::d_cal_track_init_EKF(float Z[3],float X[4],float P[4][4],float T,float prt,float 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)
{
//X(k+1|k)
Matrix4f F;
Matrix4d F;
F<< 1, T, 0, 0,
0, 1, 0, 0,
0, 0, 1, T,
0, 0, 0, 1;
Vector4f X_present = Vector4f(X[0],X[1],X[2],X[3]);
Vector4f X_pred;
Vector4d X_present = Vector4d(X[0],X[1],X[2],X[3]);
Vector4d X_pred;
X_pred=F*X_present;
//P(k+1|k)
Matrix2f Q;
Matrix2d Q;
Q<< 0.03*0.03, 0,
0, 0.03*0.03;
MatrixXf G(4,2);
MatrixXd G(4,2);
G<< T*T/2, 0,
T, 0,
0, T*T/2,
0, T;
Matrix4f P_present;
Matrix4d P_present;
for (int i=0;i<4;i++)
for (int j=0;j<4;j++)
P_present(i,j)=P[i][j];
Matrix4f P_pred;
Matrix4d P_pred;
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
//Z(k+1|k)
Vector3f Z_pred;
float x=X_pred(0);
float vx=X_pred(1);
float y=X_pred(2);
float vy=X_pred(3);
Vector3d Z_pred;
double x=X_pred(0);
double vx=X_pred(1);
double y=X_pred(2);
double vy=X_pred(3);
Z_pred(0) = sqrt(x*x+y*y);
Z_pred(1)=atan2(y,x);
if(Z_pred(1)<0)
Z_pred(1)=Z_pred(1)+2*PI;
Z_pred(2)= -(x*vx+y*vy)/sqrt(x*x+y*y);
MatrixXf H(3,4);
MatrixXd H(3,4);
H(0,0) = x/sqrt(x*x+y*y); H(0,1)=0; H(0,2)=y/sqrt(x*x+y*y); H(0,3)=0;
H(1,0) = -y/(x*x+y*y); H(1,1)=0; H(1,2)=x/(x*x+y*y); H(1,3)=0;
H(2,0) = -y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,1)=-x/sqrt(x*x+y*y);
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);
Matrix3f R;
Matrix3d R;
R(0,0)=SIGMA_R*SIGMA_R;
R(1,1)=SIGMA_A*SIGMA_A;
R(2,2)=SIGMA_V*SIGMA_V;
@@ -708,80 +708,80 @@ float kalman::d_cal_track_init_EKF(float Z[3],float X[4],float P[4][4],float T,f
R(1,2)=0;
//S
Matrix3f S;
Matrix3d S;
S=H*P_pred*H.transpose()+R;
//bind_speed
float v_bind=Bind_speed(prt, freq_ind);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3f Z_presnet= Vector3f(Z[0],Z[1],Z[2]);
Vector3f delta_z;
Vector3d Z_presnet= Vector3d(Z[0],Z[1],Z[2]);
Vector3d delta_z;
delta_z=Z_presnet-Z_pred;
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
float d=delta_z.transpose()*S.inverse()*delta_z;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
}
float kalman::d_cal_track_init_with_doppler(float Z[2],float X[4],float P[4][4],float T,float vr,float prt,float 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)
{
//X(k+1|k)
Matrix4f F;
Matrix4d F;
F<< 1, T, 0, 0,
0, 1, 0, 0,
0, 0, 1, T,
0, 0, 0, 1;
Vector4f X_present = Vector4f(X[0],X[1],X[2],X[3]);
Vector4f X_pred;
Vector4d X_present = Vector4d(X[0],X[1],X[2],X[3]);
Vector4d X_pred;
X_pred=F*X_present;
//Z(k+1|k)
float x=X_pred(0);
float vx=X_pred(1);
float y=X_pred(2);
float vy=X_pred(3);
float h31=-y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y));
float h32=-x/sqrt(x*x+y*y);
float h33=-x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y));
float h34=-y/sqrt(x*x+y*y);
MatrixXf H(3,4);
double x=X_pred(0);
double vx=X_pred(1);
double y=X_pred(2);
double vy=X_pred(3);
double h31=-y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y));
double h32=-x/sqrt(x*x+y*y);
double h33=-x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y));
double h34=-y/sqrt(x*x+y*y);
MatrixXd H(3,4);
H(0,0)=1;H(0,1)=0;H(0,2)=0;H(0,3)=0;
H(1,0)=0;H(1,1)=0;H(1,2)=1;H(1,3)=0;
H(2,0)=h31;H(2,1)=h32;H(2,2)=h33;H(2,3)=h34;
Vector3f Z_pred;
Vector3d Z_pred;
Z_pred=H*X_pred;
//P(k+1|k)
Matrix2f Q;
Matrix2d Q;
Q<< 0.03*0.03, 0,
0, 0.03*0.03;
MatrixXf G(4,2);
MatrixXd G(4,2);
G<< T*T/2, 0,
T, 0,
0, T*T/2,
0, T;
Matrix4f P_present;
Matrix4d P_present;
for (int i=0;i<4;i++)
for (int j=0;j<4;j++)
P_present(i,j)=P[i][j];
Matrix4f P_pred;
Matrix4d P_pred;
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
//R
float rho,theta;
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
Matrix3f R;
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);
@@ -793,27 +793,27 @@ float kalman::d_cal_track_init_with_doppler(float Z[2],float X[4],float P[4][4],
R(1,2)=0;
//S
Matrix3f S;
Matrix3d S;
S=H*P_pred*H.transpose()+R;
//bind_speed
float v_bind=Bind_speed(prt, freq_ind);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3f Z_presnet= Vector3f(Z[0],Z[1],vr);
Vector3f delta_z;
Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr);
Vector3d delta_z;
delta_z=Z_presnet-Z_pred;
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
float d=delta_z.transpose()*S.inverse()*delta_z;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
};
float kalman::Bind_speed(float prt,float freq_ind)
double kalman::Bind_speed(double prt,double freq_ind)
{
float freq=FREQ0+freq_ind*0.02;
double freq=FREQ0+freq_ind*0.02;
return 150000.0/(freq*prt);