#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);

}