waiwaylee
01d28e0d28
2、修复若干逻辑bug，具体参考BUG_FIX_REPORT.md Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
790 lines
20 KiB
C++
790 lines
20 KiB
C++
#include "kalman.h"
|
|
#include "coor_trans.h"
|
|
#include "parameters.h"
|
|
#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])
|
|
{
|
|
|
|
X[0]=Z1[0];
|
|
X[1]=(Z1[0]-Z0[0])/T;
|
|
X[2]=Z1[1];
|
|
X[3]=(Z1[1]-Z0[1])/T;
|
|
|
|
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 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[1][0]=R[0][1];
|
|
|
|
P[0][0]=R[0][0];
|
|
P[0][1]=R[0][0]/T;
|
|
P[0][2]=R[0][1];
|
|
P[0][3]=R[0][1]/T;
|
|
|
|
P[1][0]=R[0][0]/T;
|
|
P[1][1]=2*R[0][0]/pow(T,2);
|
|
P[1][2]=R[0][1]/T;
|
|
P[1][3]=2*R[0][1]/pow(T,2);
|
|
|
|
P[2][0]=R[0][1];
|
|
P[2][1]=R[0][1]/T;
|
|
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;
|
|
P[3][3]=2*R[1][1]/pow(T,2);
|
|
|
|
}
|
|
|
|
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
|
|
{
|
|
|
|
//X(k+1|k)
|
|
Matrix4d F;
|
|
F<< 1, T, 0, 0,
|
|
0, 1, 0, 0,
|
|
0, 0, 1, T,
|
|
0, 0, 0, 1;
|
|
Vector4d X_present = Vector4d(X[0],X[1],X[2],X[3]);
|
|
Vector4d X_pred;
|
|
X_pred=F*X_present;
|
|
|
|
//Z(k+1|k)
|
|
MatrixXd H(2,4);
|
|
H<< 1,0,0,0,
|
|
0,0,1,0;
|
|
|
|
Vector2d Z_pred;
|
|
Z_pred=H*X_pred;
|
|
|
|
//P(k+1|k)
|
|
Matrix2d Q;
|
|
Q<< 0.03*0.03, 0,
|
|
0, 0.03*0.03;
|
|
|
|
MatrixXd G(4,2);
|
|
G<< T*T/2, 0,
|
|
T, 0,
|
|
0, T*T/2,
|
|
0, 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);
|
|
|
|
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);
|
|
|
|
Matrix4d P_present;
|
|
for (int i=0;i<4;i++)
|
|
for (int j=0;j<4;j++)
|
|
P_present(i,j)=P[i][j];
|
|
Matrix4d P_pred;
|
|
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
|
|
|
|
//S
|
|
Matrix2d S;
|
|
S=H*P_pred*H.transpose()+R;
|
|
|
|
//d
|
|
Vector2d Z_presnet= Vector2d(Z[0],Z[1]);
|
|
Vector2d delta_z;
|
|
delta_z=Z_presnet-Z_pred;
|
|
double d=delta_z.transpose()*S.inverse()*delta_z;
|
|
|
|
return d;
|
|
|
|
}
|
|
|
|
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];
|
|
double x1=Z1[0];
|
|
double y1=Z1[1];
|
|
double x2=Z2[0];
|
|
double y2=Z2[1];
|
|
|
|
X[0]=x2;
|
|
X[1]=(x2-x1)/T2 ;
|
|
X[2]=((x2-x1)/T2-(x1-x0)/T1)/((T2+T1)/2);
|
|
X[3]=y2;
|
|
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];
|
|
|
|
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);
|
|
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));
|
|
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];
|
|
|
|
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)}};
|
|
|
|
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)}};
|
|
|
|
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++)
|
|
for (int j=0;j<3;j++)
|
|
P[i][j]=P11[i][j];
|
|
|
|
for (int i=0;i<3;i++)
|
|
for (int j=0;j<3;j++)
|
|
P[i][j+3]=P12[i][j];
|
|
|
|
for (int i=0;i<3;i++)
|
|
for (int j=0;j<3;j++)
|
|
P[i+3][j]=P12[i][j];
|
|
|
|
for (int i=0;i<3;i++)
|
|
for (int j=0;j<3;j++)
|
|
P[i+3][j+3]=P22[i][j];
|
|
|
|
};
|
|
|
|
double kalman::d_cal(double Z[2],double X[6],double P[6][6])
|
|
{
|
|
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];
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
P_pred(i,j)=P[i][j];
|
|
|
|
//Z(k+1|k)
|
|
MatrixXd H(2,6);
|
|
H<< 1,0,0,0,0,0,
|
|
0,0,0,1,0,0;
|
|
|
|
Vector2d Z_pred;
|
|
Z_pred=H*X_pred;
|
|
|
|
//R
|
|
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);
|
|
|
|
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
|
|
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;
|
|
|
|
};
|
|
|
|
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);
|
|
MatrixXd Q1(6,6);
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
{
|
|
F1(i,j) = F[i][j];
|
|
Q1(i,j) = Q[i][j] ;
|
|
}
|
|
VectorXd X1(6);
|
|
MatrixXd P1(6,6);
|
|
for (int i=0;i<6;i++)
|
|
X1(i)=X[i];
|
|
|
|
for (int i=0;i<6;i++)
|
|
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;
|
|
P_pred = F1*P1*F1.transpose()+Q1;
|
|
|
|
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);
|
|
|
|
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;
|
|
|
|
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,1)=0;
|
|
R(1,0)=0;
|
|
R(2,0)=0;
|
|
R(2,1)=0;
|
|
R(0,2)=0;
|
|
R(1,2)=0;
|
|
|
|
//S
|
|
Matrix3d S;
|
|
S=H*P_pred*H.transpose()+R;
|
|
|
|
//bind_speed
|
|
double v_bind=Bind_speed(prt, freq_ind);
|
|
|
|
//d
|
|
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)
|
|
{
|
|
|
|
MatrixXd F1(6,6);
|
|
MatrixXd Q1(6,6);
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
{
|
|
F1(i,j) = F[i][j];
|
|
Q1(i,j) = Q[i][j] ;
|
|
}
|
|
VectorXd X1(6);
|
|
MatrixXd P1(6,6);
|
|
for (int i=0;i<6;i++)
|
|
X1(i)=X[i];
|
|
|
|
for (int i=0;i<6;i++)
|
|
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);
|
|
|
|
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];
|
|
double y=X_pred[3];
|
|
double vy=X_pred[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;
|
|
|
|
Vector3d Z_pred;
|
|
Z_pred=H*X_pred;
|
|
|
|
//R
|
|
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);
|
|
|
|
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(1,0)=R(0,1);
|
|
R(2,2)=SIGMA_V*SIGMA_V;
|
|
R(2,0)=0;
|
|
R(2,1)=0;
|
|
R(0,2)=0;
|
|
R(1,2)=0;
|
|
|
|
//S
|
|
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;
|
|
|
|
};
|
|
|
|
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])
|
|
{
|
|
MatrixXd F1(6,6);
|
|
MatrixXd Q1(6,6);
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
{
|
|
F1(i,j) = F[i][j];
|
|
Q1(i,j) = Q[i][j] ;
|
|
}
|
|
VectorXd X1(6);
|
|
MatrixXd P1(6,6);
|
|
for (int i=0;i<6;i++)
|
|
X1(i)=X[i];
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
P1(i,j)=P[i][j];
|
|
|
|
VectorXd X1_pred(6);
|
|
MatrixXd P1_pred(6,6);
|
|
|
|
X1_pred = F1*X1;
|
|
P1_pred = F1*P1*F1.transpose()+Q1;
|
|
|
|
for (int i=0;i<6;i++)
|
|
X_pred[i]=X1_pred(i);
|
|
|
|
for (int i=0;i<6;i++)
|
|
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[3][3],
|
|
double prt,double freq_ind )
|
|
{
|
|
MatrixXd F1(6,6);
|
|
MatrixXd Q1(6,6);
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
{
|
|
F1(i,j) = F[i][j];
|
|
Q1(i,j) = Q[i][j] ;
|
|
}
|
|
VectorXd X1(6);
|
|
MatrixXd P1(6,6);
|
|
for (int i=0;i<6;i++)
|
|
X1(i)=X_cur[i];
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
P1(i,j)=P_cur[i][j];
|
|
|
|
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;
|
|
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);
|
|
|
|
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;
|
|
|
|
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,1)=0;
|
|
R(1,0)=0;
|
|
R(2,0)=0;
|
|
R(2,1)=0;
|
|
R(0,2)=0;
|
|
R(1,2)=0;
|
|
|
|
//S
|
|
Matrix3d S;
|
|
S=H*P_pred*H.transpose()+R;
|
|
|
|
//kalmam gain
|
|
MatrixXd K;
|
|
K=P_pred*H.transpose()*S.inverse();
|
|
|
|
//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)
|
|
VectorXd X;
|
|
X=X_pred+K*delta_z;
|
|
|
|
//P(k+1|k+1)
|
|
MatrixXd P;
|
|
MatrixXd I;
|
|
I.setIdentity(6, 6);
|
|
|
|
P=(I-K*H)*P_pred;
|
|
|
|
for (int i=0;i<6;i++)
|
|
X_filter[i]=X(i);
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
P_filter[i][j]=P(i,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])
|
|
{
|
|
MatrixXd F1(6,6);
|
|
MatrixXd Q1(6,6);
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
{
|
|
F1(i,j) = F[i][j];
|
|
Q1(i,j) = Q[i][j] ;
|
|
}
|
|
VectorXd X1(6);
|
|
MatrixXd P1(6,6);
|
|
for (int i=0;i<6;i++)
|
|
X1(i)=X_cur[i];
|
|
|
|
for (int i=0;i<6;i++)
|
|
for (int j=0;j<6;j++)
|
|
P1(i,j)=P_cur[i][j];
|
|
|
|
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,
|
|
0,0,0,1,0,0;
|
|
|
|
Vector2d Z_pred;
|
|
Z_pred=H*X_pred;
|
|
|
|
//R
|
|
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);
|
|
|
|
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
|
|
Matrix2d S;
|
|
S=H*P_pred*H.transpose()+R;
|
|
|
|
//kalmam gain
|
|
|
|
MatrixXd K;
|
|
K=P_pred*H.transpose()*S.inverse();
|
|
|
|
//X(k+1|k+1)
|
|
VectorXd X;
|
|
X=X_pred+K*(Z_mea-Z_pred);
|
|
|
|
//P(k+1|k+1)
|
|
MatrixXd P;
|
|
MatrixXd I;
|
|
I.setIdentity(6, 6);
|
|
|
|
P=(I-K*H)*P_pred;
|
|
|
|
for (int i=0;i<6;i++)
|
|
X_filter[i]=X(i);
|
|
|
|
for (int i=0;i<6;i++)
|
|
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++)
|
|
S_filter[i][j]=S(i,j);
|
|
|
|
};
|
|
|
|
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)
|
|
Matrix4d F;
|
|
F<< 1, T, 0, 0,
|
|
0, 1, 0, 0,
|
|
0, 0, 1, T,
|
|
0, 0, 0, 1;
|
|
Vector4d X_present = Vector4d(X[0],X[1],X[2],X[3]);
|
|
Vector4d X_pred;
|
|
X_pred=F*X_present;
|
|
|
|
//P(k+1|k)
|
|
Matrix2d Q;
|
|
Q<< 0.03*0.03, 0,
|
|
0, 0.03*0.03;
|
|
|
|
MatrixXd G(4,2);
|
|
G<< T*T/2, 0,
|
|
T, 0,
|
|
0, T*T/2,
|
|
0, T;
|
|
|
|
Matrix4d P_present;
|
|
for (int i=0;i<4;i++)
|
|
for (int j=0;j<4;j++)
|
|
P_present(i,j)=P[i][j];
|
|
|
|
Matrix4d P_pred;
|
|
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
|
|
|
|
//Z(k+1|k)
|
|
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);
|
|
|
|
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);
|
|
|
|
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,1)=0;
|
|
R(1,0)=0;
|
|
R(2,0)=0;
|
|
R(2,1)=0;
|
|
R(0,2)=0;
|
|
R(1,2)=0;
|
|
|
|
//S
|
|
Matrix3d S;
|
|
S=H*P_pred*H.transpose()+R;
|
|
|
|
//bind_speed
|
|
double v_bind=Bind_speed(prt, freq_ind);
|
|
|
|
//d
|
|
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;
|
|
|
|
return d;
|
|
|
|
}
|
|
|
|
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;
|
|
F<< 1, T, 0, 0,
|
|
0, 1, 0, 0,
|
|
0, 0, 1, T,
|
|
0, 0, 0, 1;
|
|
Vector4d X_present = Vector4d(X[0],X[1],X[2],X[3]);
|
|
Vector4d X_pred;
|
|
X_pred=F*X_present;
|
|
|
|
//Z(k+1|k)
|
|
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;
|
|
|
|
Vector3d Z_pred;
|
|
Z_pred=H*X_pred;
|
|
|
|
//P(k+1|k)
|
|
Matrix2d Q;
|
|
Q<< 0.03*0.03, 0,
|
|
0, 0.03*0.03;
|
|
|
|
MatrixXd G(4,2);
|
|
G<< T*T/2, 0,
|
|
T, 0,
|
|
0, T*T/2,
|
|
0, T;
|
|
|
|
Matrix4d P_present;
|
|
for (int i=0;i<4;i++)
|
|
for (int j=0;j<4;j++)
|
|
P_present(i,j)=P[i][j];
|
|
|
|
Matrix4d P_pred;
|
|
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
|
|
|
|
//R
|
|
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);
|
|
|
|
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(1,0)=R(0,1);
|
|
R(2,2)=SIGMA_V*SIGMA_V;
|
|
R(2,0)=0;
|
|
R(2,1)=0;
|
|
R(0,2)=0;
|
|
R(1,2)=0;
|
|
|
|
//S
|
|
Matrix3d S;
|
|
S=H*P_pred*H.transpose()+R;
|
|
|
|
//bind_speed
|
|
double v_bind=Bind_speed(prt, freq_ind);
|
|
|
|
//d
|
|
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;
|
|
|
|
return d;
|
|
|
|
};
|
|
|
|
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);
|
|
|
|
}
|