Signed-off-by: Yang Jianchao <yangjianchao999@126.com>
This commit is contained in:
Yang Jianchao
@@ -0,0 +1,821 @@
|
||||
#include "kalman.h"
|
||||
#include "coor_trans.h"
|
||||
#include "parameters.h"
|
||||
#include <qmath.h>
|
||||
#include "memory.h"
|
||||
#include <QVector>
|
||||
using namespace std;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void kalman:: kalman_filter_init_2dots(float Z0[2], float Z1[2], float T,float X[4], float 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;
|
||||
|
||||
float 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);
|
||||
|
||||
float 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[1][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);
|
||||
|
||||
}
|
||||
|
||||
|
||||
float kalman::d_cal_track_init(float Z[2],float X[4],float P[4][4],float T)
|
||||
{
|
||||
|
||||
//X(k+1|k)
|
||||
Matrix4f 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;
|
||||
X_pred=F*X_present;
|
||||
|
||||
//Z(k+1|k)
|
||||
MatrixXf H(2,4);
|
||||
H<< 1,0,0,0,
|
||||
0,0,1,0;
|
||||
|
||||
Vector2f Z_pred;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
//P(k+1|k)
|
||||
Matrix2f Q;
|
||||
Q<< 0.03*0.03, 0,
|
||||
0, 0.03*0.03;
|
||||
|
||||
MatrixXf G(4,2);
|
||||
G<< T*T/2, 0,
|
||||
T, 0,
|
||||
0, T*T/2,
|
||||
0, T;
|
||||
|
||||
float 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);
|
||||
|
||||
Matrix2f 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;
|
||||
for (int i=0;i<4;i++)
|
||||
for (int j=0;j<4;j++)
|
||||
P_present(i,j)=P[i][j];
|
||||
Matrix4f P_pred;
|
||||
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
|
||||
|
||||
//S
|
||||
Matrix2f S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
|
||||
|
||||
//d
|
||||
Vector2f Z_presnet= Vector2f(Z[0],Z[1]);
|
||||
Vector2f delta_z;
|
||||
delta_z=Z_presnet-Z_pred;
|
||||
float d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
|
||||
return d;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
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])
|
||||
{
|
||||
float x0=Z0[0];
|
||||
float y0=Z0[1];
|
||||
float x1=Z1[0];
|
||||
float y1=Z1[1];
|
||||
float x2=Z2[0];
|
||||
float 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);
|
||||
|
||||
|
||||
|
||||
float R0[2][2];
|
||||
float R1[2][2];
|
||||
float R2[2][2];
|
||||
|
||||
float 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);
|
||||
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];
|
||||
|
||||
float 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)} ,
|
||||
{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)} ,
|
||||
{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];
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
float kalman::d_cal(float Z[2],float X[6],float P[6][6])
|
||||
{
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf P_pred(6,6);
|
||||
Vector2f 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)
|
||||
MatrixXf H(2,6);
|
||||
H<< 1,0,0,0,0,0,
|
||||
0,0,0,1,0,0;
|
||||
|
||||
Vector2f Z_pred;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
//R
|
||||
float 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);
|
||||
|
||||
Matrix2f 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;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
|
||||
//d
|
||||
Vector2f delta_z;
|
||||
delta_z=Z_mea-Z_pred;
|
||||
float 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)
|
||||
{
|
||||
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf 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] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf 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];
|
||||
|
||||
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf 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);
|
||||
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);
|
||||
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;
|
||||
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
|
||||
Matrix3f S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
//d
|
||||
Vector3f Z_mea(Z[0],Z[1],Z[2]);
|
||||
Vector3f 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;
|
||||
|
||||
return d;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf 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] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf 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];
|
||||
|
||||
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf P_pred(6,6);
|
||||
Vector3f 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);
|
||||
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;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
//R
|
||||
float 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);
|
||||
|
||||
Matrix3f 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
|
||||
Matrix3f S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
|
||||
//d
|
||||
Vector3f 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;
|
||||
|
||||
|
||||
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])
|
||||
{
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf 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] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf 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];
|
||||
|
||||
VectorXf X1_pred(6);
|
||||
MatrixXf 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(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 )
|
||||
{
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf 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] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf 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];
|
||||
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf P_pred(6,6);
|
||||
|
||||
|
||||
X_pred = F1*X1;
|
||||
P_pred = F1*P1*F1.transpose()+Q1;
|
||||
|
||||
|
||||
|
||||
Vector3f 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);
|
||||
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);
|
||||
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;
|
||||
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
|
||||
Matrix3f S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//kalmam gain
|
||||
MatrixXf K;
|
||||
K=P_pred*H.transpose()*S.inverse();
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
|
||||
//d
|
||||
Vector3f 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;
|
||||
X=X_pred+K*delta_z;
|
||||
|
||||
//P(k+1|k+1)
|
||||
MatrixXf P;
|
||||
MatrixXf 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);
|
||||
|
||||
}
|
||||
|
||||
|
||||
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])
|
||||
{
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf 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] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf 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];
|
||||
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf P_pred(6,6);
|
||||
|
||||
|
||||
X_pred = F1*X1;
|
||||
P_pred = F1*P1*F1.transpose()+Q1;
|
||||
|
||||
|
||||
|
||||
Vector2f Z_mea(Z[0],Z[1]);
|
||||
|
||||
|
||||
|
||||
//Z(k+1|k)
|
||||
MatrixXf H(2,6);
|
||||
H<< 1,0,0,0,0,0,
|
||||
0,0,0,1,0,0;
|
||||
|
||||
Vector2f Z_pred;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
//R
|
||||
float 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);
|
||||
|
||||
Matrix2f 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;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
|
||||
//kalmam gain
|
||||
|
||||
MatrixXf K;
|
||||
K=P_pred*H.transpose()*S.inverse();
|
||||
|
||||
//X(k+1|k+1)
|
||||
VectorXf X;
|
||||
X=X_pred+K*(Z_mea-Z_pred);
|
||||
|
||||
//P(k+1|k+1)
|
||||
MatrixXf P;
|
||||
MatrixXf 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);
|
||||
|
||||
};
|
||||
|
||||
float kalman::d_cal_track_init_EKF(float Z[3],float X[4],float P[4][4],float T,float prt,float freq_ind)
|
||||
{
|
||||
|
||||
//X(k+1|k)
|
||||
Matrix4f 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;
|
||||
X_pred=F*X_present;
|
||||
|
||||
//P(k+1|k)
|
||||
Matrix2f Q;
|
||||
Q<< 0.03*0.03, 0,
|
||||
0, 0.03*0.03;
|
||||
|
||||
MatrixXf G(4,2);
|
||||
G<< T*T/2, 0,
|
||||
T, 0,
|
||||
0, T*T/2,
|
||||
0, T;
|
||||
|
||||
Matrix4f 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;
|
||||
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);
|
||||
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);
|
||||
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;
|
||||
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
|
||||
Matrix3f S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
//d
|
||||
Vector3f Z_presnet= Vector3f(Z[0],Z[1],Z[2]);
|
||||
Vector3f 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;
|
||||
|
||||
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)
|
||||
{
|
||||
//X(k+1|k)
|
||||
Matrix4f 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;
|
||||
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);
|
||||
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;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
|
||||
//P(k+1|k)
|
||||
Matrix2f Q;
|
||||
Q<< 0.03*0.03, 0,
|
||||
0, 0.03*0.03;
|
||||
|
||||
MatrixXf G(4,2);
|
||||
G<< T*T/2, 0,
|
||||
T, 0,
|
||||
0, T*T/2,
|
||||
0, T;
|
||||
|
||||
Matrix4f 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;
|
||||
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
|
||||
|
||||
//R
|
||||
float 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);
|
||||
|
||||
Matrix3f 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
|
||||
Matrix3f S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
//d
|
||||
Vector3f Z_presnet= Vector3f(Z[0],Z[1],vr);
|
||||
Vector3f 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;
|
||||
|
||||
return d;
|
||||
|
||||
};
|
||||
|
||||
|
||||
float kalman::Bind_speed(float prt,float freq_ind)
|
||||
{
|
||||
float freq=FREQ0+freq_ind*0.02;
|
||||
|
||||
|
||||
return 150000.0/(freq*prt);
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user