#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(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(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(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[2][2],
					   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(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<2;i++)
		for (int j=0;j<2;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(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(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;


	return 150000.0/(freq*prt);

}