#include "track_asso.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;


// //////////////////////////// 用point_recv中点迹与trust_track中的点迹进行关联，                          /////////////
// //////////////////////////// 航迹更新结果通过Trust_Track_Output[MAX_TRACK_NUM][10]和 Trust_track_num_Output输出        /////////////

int Track_Asso:: track_asso_process(QVector<PointRecv>               *point_recv,                           //输入点迹
                                    QVector <Trust_Track>		     *trust_track,                            //航迹文件
                                    struct  Track                    Trust_Track_Output[MAX_TRACK_NUM][10],   //更新航迹信息
                                    int                              *Trust_track_num_Output,                 //更新航迹数
                                    struct  RadarPara                Work_Parameter                           //雷达参数
                                    )
{

    //取出对应点迹区的点
    for (int i=0;i< point_recv->size();i++)
    {
        point_process.push_back((*point_recv)[i]);
        int n=point_process.size();
        point_process[n-1].point_section_asso = 1;
    }


    //IMM算法
    model_interaction(trust_track);

    model_filter(trust_track, Work_Parameter);

    model_output(trust_track);


    //删除关联上的点
    QVector <PointRecv>::iterator Iter;
    for (Iter= point_process.begin(); Iter!=point_process.end();)
    {
        if((*Iter).Use_Flag==1 )
        {
            point_process.erase(Iter);
            Iter=point_process.begin();
        }
        else
        {
            Iter++;
        }
    }

    //剩余点重新存入点迹
    QVector<PointRecv>().swap((*point_recv));
    for (int i=0;i<point_process.size();i++)
    {

       (*point_recv).push_back(point_process[i]);

    }

    //point_process清空
    QVector<PointRecv>().swap(point_process);

    //输出航迹
    for (int i=0;i<trust_track->size();i++)
    {																																					// 输出更新航迹条件：
//        if((*trust_track)[i].manual_tracking_flag==0)
//        {

            *Trust_track_num_Output= *Trust_track_num_Output+1;

            Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
            double x,y;
            x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
            y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
            coor_trans Coor_trans;
            double r, azi;
            Coor_trans.cart2polar(x, y, &r, &azi);
            Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r;
            Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azi/PI*180;

            if((*trust_track)[i].Height<r)
                Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin((*trust_track)[i].Height/r)/PI*180;
            else
                Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = (*trust_track)[i].elev_point;

            Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index;

            Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V =
            sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
            Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = (*trust_track)[i].Amplitude;


            Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
            Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track)[i].T_track/1000.0;
            Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track)[i].point_flag;
            Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height;


            double Direction_Angle;
            Direction_Angle=atan((*trust_track)[i].X[4]/(*trust_track)[i].X[1]);
            if((*trust_track)[i].X[1]<0){Direction_Angle=Direction_Angle+PI;}
            if((*trust_track)[i].X[1]>0&&(*trust_track)[i].X[4]<0){Direction_Angle=Direction_Angle+2*PI;}
            Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180;

            //关联点信息
            Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=(*trust_track)[i].range_point;
            Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track)[i].azi_point;
            Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track)[i].elev_point;
            Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track)[i].vr_point;
            Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=0;
            Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = (*trust_track)[i].prf_point;
            Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;

            //直接输出点迹高度-20260605
            Trust_Track_Output[*Trust_track_num_Output-1][0].z = (*trust_track)[i].range_point * sin((*trust_track)[i].elev_point/180.0*PI);
            Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = (*trust_track)[i].elev_point;

            Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track)[i].X[0];
            Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track)[i].X[3];
            Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track)[i].X[1];
            Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track)[i].X[4];

            Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*trust_track)[i].RCS;
            Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;

            std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim, (*trust_track)[i].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim));
            std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim, (*trust_track)[i].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim));
//        }
    }

    return 0;
}

//遍历所有航迹  进行多模型交互
void Track_Asso:: model_interaction(QVector <Trust_Track>	*trust_track)
{


    for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
    {

//        if((*trust_track)[loop_of_track].manual_tracking_flag == 0)
//        {
            double u_last[3];
            u_last[0]=(*trust_track)[loop_of_track].u[0];
            u_last[1]=(*trust_track)[loop_of_track].u[1];
            u_last[2]=(*trust_track)[loop_of_track].u[2];

            double u_t[3][3];
            double c[3];
            c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
            c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
            c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];

            u_t[0][0]=Pt[0][0]*u_last[0]/c[0];
            u_t[1][0]=Pt[1][0]*u_last[1]/c[0];
            u_t[2][0]=Pt[2][0]*u_last[2]/c[0];
            u_t[0][1]=Pt[0][1]*u_last[0]/c[1];
            u_t[1][1]=Pt[1][1]*u_last[1]/c[1];
            u_t[2][1]=Pt[2][1]*u_last[2]/c[1];
            u_t[0][2]=Pt[0][2]*u_last[0]/c[2];
            u_t[1][2]=Pt[1][2]*u_last[1]/c[2];
            u_t[2][2]=Pt[2][2]*u_last[2]/c[2];


            double Xo1_last[6],Xo2_last[6],Xo3_last[6],X1[6],X2[6],X3[6];
            for (int i=0;i<6;i++)
            {
                X1[i]=(*trust_track)[loop_of_track].X1[i];
                X2[i]=(*trust_track)[loop_of_track].X2[i];
                X3[i]=(*trust_track)[loop_of_track].X3[i];
            }


            for (int i=0;i<6;i++)
            {
                Xo1_last[i]=u_t[0][0]*X1[i]+u_t[1][0]*X2[i]+u_t[2][0]*X3[i];
                Xo2_last[i]=u_t[0][1]*X1[i]+u_t[1][1]*X2[i]+u_t[2][1]*X3[i];
                Xo3_last[i]=u_t[0][2]*X1[i]+u_t[1][2]*X2[i]+u_t[2][2]*X3[i];
            }


            double X1_sub_Xo1[6],X2_sub_Xo1[6],X3_sub_Xo1[6];
            double X1_sub_Xo2[6],X2_sub_Xo2[6],X3_sub_Xo2[6];
            double X1_sub_Xo3[6],X2_sub_Xo3[6],X3_sub_Xo3[6];
            double X11[6][6],X21[6][6],X31[6][6];
            double X12[6][6],X22[6][6],X32[6][6];
            double X13[6][6],X23[6][6],X33[6][6];


            for (int i=0;i<6;i++)
            {

                X1_sub_Xo1[i]=X1[i]-Xo1_last[i];
                X2_sub_Xo1[i]=X2[i]-Xo1_last[i];
                X3_sub_Xo1[i]=X3[i]-Xo1_last[i];
                X1_sub_Xo2[i]=X1[i]-Xo2_last[i];
                X2_sub_Xo2[i]=X2[i]-Xo2_last[i];
                X3_sub_Xo2[i]=X3[i]-Xo2_last[i];
                X1_sub_Xo3[i]=X1[i]-Xo3_last[i];
                X2_sub_Xo3[i]=X2[i]-Xo3_last[i];
                X3_sub_Xo3[i]=X3[i]-Xo3_last[i];

            }


            for (int i=0;i<6;i++)
                for(int j=0;j<6;j++)
                {
                    X11[i][j]=X1_sub_Xo1[i]*X1_sub_Xo1[j];
                    X21[i][j]=X2_sub_Xo1[i]*X2_sub_Xo1[j];
                    X31[i][j]=X3_sub_Xo1[i]*X3_sub_Xo1[j];
                    X12[i][j]=X1_sub_Xo2[i]*X1_sub_Xo2[j];
                    X22[i][j]=X2_sub_Xo2[i]*X2_sub_Xo2[j];
                    X32[i][j]=X3_sub_Xo2[i]*X3_sub_Xo2[j];
                    X13[i][j]=X1_sub_Xo3[i]*X1_sub_Xo3[j];
                    X23[i][j]=X2_sub_Xo3[i]*X2_sub_Xo3[j];
                    X33[i][j]=X3_sub_Xo3[i]*X3_sub_Xo3[j];
                }


            double Po1_last[6][6],Po2_last[6][6],Po3_last[6][6];
            for (int i=0;i<6;i++)
                for(int j=0;j<6;j++)
                {
                    Po1_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X11[i][j])*u_t[0][0]+((*trust_track)[loop_of_track].P2[i][j]+X21[i][j])*u_t[1][0]+((*trust_track)[loop_of_track].P3[i][j]+X31[i][j])*u_t[2][0];
                    Po2_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X12[i][j])*u_t[0][1]+((*trust_track)[loop_of_track].P2[i][j]+X22[i][j])*u_t[1][1]+((*trust_track)[loop_of_track].P3[i][j]+X32[i][j])*u_t[2][1];
                    Po3_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X13[i][j])*u_t[0][2]+((*trust_track)[loop_of_track].P2[i][j]+X23[i][j])*u_t[1][2]+((*trust_track)[loop_of_track].P3[i][j]+X33[i][j])*u_t[2][2];
                }

            for(int i=0;i<6;i++)
            {
                (*trust_track)[loop_of_track].X1[i]=Xo1_last[i];
                (*trust_track)[loop_of_track].X2[i]=Xo2_last[i];
                (*trust_track)[loop_of_track].X3[i]=Xo3_last[i];
            }
            for(int i=0;i<6;i++)
                for (int j=0;j<6;j++)
                {
                    (*trust_track)[loop_of_track].P1[i][j]=Po1_last[i][j];
                    (*trust_track)[loop_of_track].P2[i][j]=Po2_last[i][j];
                    (*trust_track)[loop_of_track].P3[i][j]=Po3_last[i][j];
                }
//        }
    }
}


// 产生 F Q 矩阵
void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct  RadarPara   Work_Parameter)
{

    //F
    double alpha=0.2;
    memset(F,0,36*sizeof(double));
    F[0][0]=1;
    F[0][1]=delta_T;
    F[0][2]=(alpha*delta_T-1+exp(-alpha*delta_T))/pow(alpha,2);
    F[1][0]=0;
    F[1][1]=1;
    F[1][2]=(1-exp(-alpha*delta_T))/alpha;
    F[2][0]=0;
    F[2][1]=0;
    F[2][2]=exp(-alpha*delta_T);

    F[3][3]=1;
    F[3][4]=delta_T;
    F[3][5]=(alpha*delta_T-1+exp(-alpha*delta_T))/pow(alpha,2);
    F[4][3]=0;
    F[4][4]=1;
    F[4][5]=(1-exp(-alpha*delta_T))/alpha;
    F[5][3]=0;
    F[5][4]=0;
    F[5][5]=exp(-alpha*delta_T);



    //模型1
    //Q
    double q1=Work_Parameter.Model1_Q_slow;
    double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
    double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
    double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
    double q221=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
    double q231=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
    double q331=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
    memset(Q1,0,36*sizeof(double));
    Q1[0][0]=q1*q111; Q1[0][1]=q1*q121; Q1[0][2]=q1*q131;
    Q1[1][0]=q1*q121; Q1[1][1]=q1*q221; Q1[1][2]=q1*q231;
    Q1[2][0]=q1*q131; Q1[2][1]=q1*q231; Q1[2][2]=q1*q331;

    Q1[3][3]=q1*q111; Q1[3][4]=q1*q121; Q1[3][5]=q1*q131;
    Q1[4][3]=q1*q121; Q1[4][4]=q1*q221; Q1[4][5]=q1*q231;
    Q1[5][3]=q1*q131; Q1[5][4]=q1*q231; Q1[5][5]=q1*q331;


    //模型2
    //Q
    double q2;
    if(v_track<=5)
    {
        q2=Work_Parameter.Model2_Q_slow/20;
    }
    else if(v_track<=15&&v_track>5)
    {
        q2=Work_Parameter.Model2_Q_slow;
    }
    else if(v_track<=30&&v_track>15)
    {
        q2=Work_Parameter.Model2_Q_slow;
    }
    else if(v_track<=100&&v_track>30)
    {
        q2=Work_Parameter.Model2_Q_fast;

    }
    else
    {
        q2=Work_Parameter.Model2_Q_fast;
    }

    double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
    double q122=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
    double q132=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
    double q222=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
    double q232=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
    double q332=1/(2*alpha)*(1-exp(-2*alpha*delta_T));

    memset(Q2,0,36*sizeof(double));
    Q2[0][0]=q2*q112;Q2[0][1]=q2*q122; Q2[0][2]=q2*q132;
    Q2[1][0]=q2*q122;Q2[1][1]=q2*q222; Q2[1][2]=q2*q232;
    Q2[2][0]=q2*q132; Q2[2][1]=q2*q232; Q2[2][2]=q2*q332;

    Q2[3][3]=q2*q112; Q2[3][4]=q2*q122; Q2[3][5]=q2*q132;
    Q2[4][3]=q2*q122; Q2[4][4]=q2*q222; Q2[4][5]=q2*q232;
    Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332;


    //模型3
    //Q
    double q3;
    if(v_track<=5)
    {
        q3=Work_Parameter.Model3_Q_slow/20;
    }
    else if(v_track<=15&&v_track>5)
    {
        q3=Work_Parameter.Model3_Q_slow;
    }
    else if(v_track<=20&&v_track>15)
    {
        q3=Work_Parameter.Model3_Q_slow;
    }
    else if(v_track<=50&&v_track>20)
    {
        q3=Work_Parameter.Model3_Q_fast;

    }
    else if(v_track<=100&&v_track>50)
    {
        q3=2*Work_Parameter.Model3_Q_fast;

    }
    else
    {
        q3=10*Work_Parameter.Model3_Q_fast;
    }

    double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
    double q123=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
    double q133=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
    double q223=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
    double q233=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
    double q333=1/(2*alpha)*(1-exp(-2*alpha*delta_T));


    memset(Q3,0,36*sizeof(double));
    Q3[0][0]=q3*q113; Q3[0][1]=q3*q123; Q3[0][2]=q3*q133;
    Q3[1][0]=q3*q123; Q3[1][1]=q3*q223; Q3[1][2]=q3*q233;
    Q3[2][0]=q3*q133; Q3[2][1]=q3*q233; Q3[2][2]=q3*q333;

    Q3[3][3]=q3*q113; Q3[3][4]=q3*q123; Q3[3][5]=q3*q133;
    Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233;
    Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333;


}


// 计算三个 d
void Track_Asso::IMM_d_cal(double v_track,
                           double X1[6], double P1[6][6],
                           double X2[6], double P2[6][6],
                           double X3[6], double P3[6][6],
                           double Z[3], double prt,double freq_ind,
                           double delta_T,
                           double *d1,double *d2,double *d3,
                            struct  RadarPara   Work_Parameter)
{
    double F[6][6],  Q1[6][6],  Q2[6][6],  Q3[6][6];
    IMM_F_Q_gen( v_track, delta_T,F, Q1,  Q2,  Q3,Work_Parameter);

    kalman Kalman;
    *d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
    *d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
    *d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);

}




void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct  RadarPara Work_Parameter)
{

    //存储关联信息
    struct asso_info
    {
        double d1;
        double d2;
        double d3;
        double d_min;
        int	point_index;
        int track_index;
    };
    QVector <asso_info> associated_info;

    //遍历所有点迹航迹  计算点迹和航迹的统计距离
    for (int loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
    {
//        if((*trust_track)[loop_of_track].manual_tracking_flag==0)
//        {
            (*trust_track)[loop_of_track].point_flag = 0;  //航迹的point_flag置为0 关联上点后再置为1

            for (int loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
            {


                //时间差
                double delta_T = (point_process[loop_of_point].CPI_Time - (*trust_track)[loop_of_track].T_track)/1000.0;
                //航迹X P
                double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
                memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
                memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
                memcpy(X3,(*trust_track)[loop_of_track].X3,6*sizeof(double));
                memcpy(P1,(*trust_track)[loop_of_track].P1,6*6*sizeof(double));
                memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
                memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
                double v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
                double h_track = (*trust_track)[loop_of_track].Height;
                double r_track =  sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));

                //量测信息
                double Z[3] = {point_process[loop_of_point].Range,
                              point_process[loop_of_point].Azimuth,
                              point_process[loop_of_point].Velocity};
                double prt = point_process[loop_of_point].PRF_index;
                double freq_ind =  point_process[loop_of_point].Freq_index;
                double h_point = point_process[loop_of_point].Height;

                //计算统计距离d
                double d1,d2,d3;
                IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);

                //计算高度门限
                double d_h;
                if (r_track<1000)
                {
                    if(abs(h_track-h_point)<=150)
                    {
                       d_h = 1;
                    }
                    else
                    {
                        d_h = 0;
                    }

                }
                else if (r_track<2000 && r_track>=1000)
                {
                    if(abs(h_track-h_point)<=200)
                    {
                       d_h = 1;
                    }
                    else
                    {
                        d_h = 0;
                    }

                }
                else if(r_track>=2000 && r_track<3000 )
                {
                    if(abs(h_track-h_point)<=200)
                    {
                       d_h = 1;
                    }
                    else
                    {
                        d_h = 0;
                    }
                }
                else if(r_track>=3000 && r_track<5000)
                {
                    if(abs(h_track-h_point)<=300)
                    {
                       d_h = 1;
                    }
                    else
                    {
                        d_h = 0;
                    }
                }
                else
                {
                   d_h = 1;
                }

                qDebug() << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
                qDebug() << h_track<<" "<<h_point<<" "<<d_h;
                d_h = 1;

                //计算距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差
                double& t_x0 = (*trust_track)[loop_of_track].X[0];
                double& t_y0 = (*trust_track)[loop_of_track].X[3];
                double a_track = atan2(t_y0, t_x0);                             //航迹方位角
                double& r_point = point_process[loop_of_point].Range;           //点迹距离
                double a_delta = abs(a_track - point_process[loop_of_point].Azimuth);       //方位差
                if(a_delta > PI){
                    a_delta = 2*PI - a_delta;
                }

                double R = sqrt(r_track * r_track + r_point*r_point - 2*r_track*r_point*cos(a_delta));
                double VT = v_track*delta_T;

                bool d_r = true;

                if(VT < 100 && R > 300){
                    //VT<100m，使用固定门限300m
                    d_r = false;
                }else if((VT>=100 && VT<=300 && R/VT>3)){
                    d_r = false;
                }else if(VT>300 && R > 1200){
                    d_r = false;
                }

                //小于关联门限  保存关联信息
                if(
                    (  r_track<=500               && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000) && d_h == 1 && d_r)//
                    ||( (r_track>500&&r_track<1000) && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/100) && d_h == 1 && d_r)//
                    ||(  r_track>=1000              && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD) && d_h == 1 && d_r)//
                )
                {
                     qDebug() << "asooooooooooo";

                    struct asso_info associated_info_tmp;
                    associated_info_tmp.d1=d1;
                    associated_info_tmp.d2=d2;
                    associated_info_tmp.d3=d3;
                    associated_info_tmp.point_index=loop_of_point+1;
                    associated_info_tmp.track_index=loop_of_track+1;
                    if(d1<=d2&&d1<=d3)
                        associated_info_tmp.d_min=d1;
                    else if(d2<=d1&& d2<=d3)
                        associated_info_tmp.d_min=d2;
                    else
                        associated_info_tmp.d_min=d3;

                    associated_info.push_back(associated_info_tmp);
                }
            }
//        }
    }


    //最近邻法关联
    int associated_num=associated_info.size();
    for (int i=0;i<trust_track->size();i++)
    {
        //寻找最小d
        if(associated_num>0)
        {
            int min_index=1;
            double min_d=associated_info[0].d_min;
            for (int ii=0;ii<associated_num;ii++)
            {
                if(associated_info[ii].d_min<min_d)
                {
                    min_d=associated_info[ii].d_min;
                    min_index=ii+1;
                }
            }
            int track_index=associated_info[min_index-1].track_index;
            int point_index=associated_info[min_index-1].point_index;

            double d1=associated_info[min_index-1].d1;
            double d2=associated_info[min_index-1].d2;
            double d3=associated_info[min_index-1].d3;

            //卡尔曼滤波
            double Z[3]={point_process[point_index-1].Range,point_process[point_index-1].Azimuth,point_process[point_index-1].Velocity};
            double prt = point_process[point_index-1].PRF_index;
            double freq_ind =  point_process[point_index-1].Freq_index;
            double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
            memcpy(X1,(*trust_track)[track_index-1].X1,6*sizeof(double));
            memcpy(X2,(*trust_track)[track_index-1].X2,6*sizeof(double));
            memcpy(X3,(*trust_track)[track_index-1].X3,6*sizeof(double));
            memcpy(P1,(*trust_track)[track_index-1].P1,6*6*sizeof(double));
            memcpy(P2,(*trust_track)[track_index-1].P2,6*6*sizeof(double));
            memcpy(P3,(*trust_track)[track_index-1].P3,6*6*sizeof(double));
            double v_track = sqrt(pow((*trust_track)[track_index-1].X[1],2)+pow((*trust_track)[track_index-1].X[4],2));
            double delta_T = (point_process[point_index-1].CPI_Time - (*trust_track)[track_index-1].T_track)/1000.0;
            double F[6][6],  Q1[6][6],  Q2[6][6],  Q3[6][6];
            IMM_F_Q_gen( v_track, delta_T,F, Q1,  Q2,  Q3,Work_Parameter);

            if(delta_T>0)
            {
                double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
                double S1[2][2],S2[2][2],S3[2][2];
                kalman Kalman;
                Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind );
                Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind );
                Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind );


                Matrix2f S1_out,S2_out,S3_out;
                for (int ii=0;ii<2;ii++)
                    for (int jj=0;jj<2;jj++)
                    {
                        S1_out(ii,jj)=S1[ii][jj];
                        S2_out(ii,jj)=S2[ii][jj];
                        S3_out(ii,jj)=S3[ii][jj];
                    }
                double det_S1=S1_out.determinant();
                double Possibility1=1/sqrt(2*PI*det_S1)*exp(-0.5*d1);
                double det_S2=S2_out.determinant();
                double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2);
                double det_S3=S3_out.determinant();
                double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3);

                //更新模型概率
                double u_last[3];
                u_last[0]=(*trust_track)[track_index-1].u[0];
                u_last[1]=(*trust_track)[track_index-1].u[1];
                u_last[2]=(*trust_track)[track_index-1].u[2];

                double c[3];
                c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
                c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
                c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];

                (*trust_track)[track_index-1].u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
                (*trust_track)[track_index-1].u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
                (*trust_track)[track_index-1].u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);

                //更新 X1 X2 X3 P1 P2 P3
                for(int ii=0;ii<6;ii++)
                {
                    (*trust_track)[track_index-1].X1[ii]=X1_filter[ii];
                    (*trust_track)[track_index-1].X2[ii]=X2_filter[ii];
                    (*trust_track)[track_index-1].X3[ii]=X3_filter[ii];
                }
                for(int ii=0;ii<6;ii++)
                    for (int jj=0;jj<6;jj++)
                    {
                        (*trust_track)[track_index-1].P1[ii][jj]=P1_filter[ii][jj];
                        (*trust_track)[track_index-1].P2[ii][jj]=P2_filter[ii][jj];
                        (*trust_track)[track_index-1].P3[ii][jj]=P3_filter[ii][jj];
                    }

                //更新航迹时间
                (*trust_track)[track_index-1].T_track = point_process[point_index-1].CPI_Time;



                //更新关联上的点迹信息
                (*trust_track)[track_index-1].range_point=point_process[point_index-1].Range;
                (*trust_track)[track_index-1].azi_point=point_process[point_index-1].Azimuth/PI*180;
                (*trust_track)[track_index-1].elev_point= asin(point_process[point_index-1].Height/point_process[point_index-1].Range)/PI*180;
                (*trust_track)[track_index-1].vr_point=point_process[point_index-1].Velocity;
                (*trust_track)[track_index-1].point_type = 0;
                (*trust_track)[track_index-1].prf_point = point_process[point_index-1].PRF_index;
                (*trust_track)[track_index-1].snr_point = point_process[point_index-1].snr;
                (*trust_track)[track_index-1].Amplitude= point_process[point_index-1].Amplitude;	//幅度

                (*trust_track)[track_index-1].RCS = point_process[point_index-1].RCS;

                (*trust_track)[track_index-1].pitch_num = point_process[point_index-1].pitch_num;

                std::memcpy((*trust_track)[track_index-1].speed_dim, point_process[point_index-1].speed_dim, sizeof((*trust_track)[track_index-1].speed_dim));
                std::memcpy((*trust_track)[track_index-1].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[track_index-1].range_dim));

                //更新高度
                track_hight_update(track_index,point_index,trust_track);

            }
            (*trust_track)[track_index-1].Extrapolate_round = 0;		//连续未用实点更新时间
            (*trust_track)[track_index-1].point_flag=1;                 //实点
            (*trust_track)[track_index-1].associate_point_number = (*trust_track)[track_index-1].associate_point_number+1; //关联点数+1

            //删除associated_info中关联上的航迹 点迹信息
            for (int ii=0;ii<associated_num;)
            {
                if(associated_info[ii].track_index==track_index)
                {
                    associated_info.erase(associated_info.begin()+ii);
                    associated_num=associated_info.size();
                }
                else
                    ii++;
            }
            associated_num=associated_info.size();
            for (int ii=0;ii<associated_num;)
            {
                if(associated_info[ii].point_index==point_index)
                {
                    associated_info.erase(associated_info.begin()+ii);
                    associated_num=associated_info.size();
                }
                else
                    ii++;
            }
            associated_num=associated_info.size();

            //在接收点迹中 将关联上的点迹的使用标记位置为1 等待删除
            point_process[point_index-1].Use_Flag = 1;
        }
    }


    //未关联上的航迹 进行外推 tws
    for (int i =0; i<(*trust_track).size();i++ )
    {
        if((*trust_track)[i].point_flag == 0 && (*trust_track)[i].manual_tracking_flag == 0)
        {
            double v_track = sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
            double delta_T;
            if(Work_Parameter.work_mode == 0)
            {
               delta_T = Work_Parameter.DATA_RATE_SHORT;
            }
            else if(Work_Parameter.work_mode == 1)
            {
                delta_T = Work_Parameter.DATA_RATE_MIDDLE;
            }
            else if(Work_Parameter.work_mode == 2)
            {
                delta_T = Work_Parameter.DATA_RATE_FAR;
            }
            else
            {
                delta_T = Work_Parameter.DATA_RATE_SHORT;
            }

            double F[6][6],  Q1[6][6],  Q2[6][6],  Q3[6][6];
            IMM_F_Q_gen( v_track, delta_T, F, Q1,  Q2,  Q3,Work_Parameter);

            //航迹X P
            double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
            memcpy(X1,(*trust_track)[i].X1,6*sizeof(double));
            memcpy(X2,(*trust_track)[i].X2,6*sizeof(double));
            memcpy(X3,(*trust_track)[i].X3,6*sizeof(double));
            memcpy(P1,(*trust_track)[i].P1,6*6*sizeof(double));
            memcpy(P2,(*trust_track)[i].P2,6*6*sizeof(double));
            memcpy(P3,(*trust_track)[i].P3,6*6*sizeof(double));

            //外推

            double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
            kalman Kalman;
            Kalman.kalman_pred(F, Q1, X1, P1, X1_pred, P1_pred);
            Kalman.kalman_pred(F, Q2, X2, P2, X2_pred, P2_pred);
            Kalman.kalman_pred(F, Q3, X3, P3, X3_pred, P3_pred);
            memcpy((*trust_track)[i].X1,X1_pred,6*sizeof(double));
            memcpy((*trust_track)[i].X2,X2_pred,6*sizeof(double));
            memcpy((*trust_track)[i].X3,X3_pred,6*sizeof(double));
            memcpy((*trust_track)[i].P1,P1_pred,6*6*sizeof(double));
            memcpy((*trust_track)[i].P2,P2_pred,6*6*sizeof(double));
            memcpy((*trust_track)[i].P3,P3_pred,6*6*sizeof(double));


            (*trust_track)[i].T_track = (*trust_track)[i].T_track+delta_T*1000.0;            //更新航迹时间
            (*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1;		//连续未用实点更新时间
            (*trust_track)[i].point_flag=0;                                                 //虚点
        }
    }


}



void Track_Asso:: model_output(QVector <Trust_Track>	*trust_track)
{
    for(int loop_of_track=0; loop_of_track<trust_track->size(); loop_of_track++)
    {

//        if((*trust_track)[loop_of_track].manual_tracking_flag==0)
//        {
            double u_now[3];
            u_now[0]=(*trust_track)[loop_of_track].u[0];
            u_now[1]=(*trust_track)[loop_of_track].u[1];
            u_now[2]=(*trust_track)[loop_of_track].u[2];

            double X1_filter[6],X2_filter[6],X3_filter[6];
            double P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
            for (int ii=0;ii<6;ii++)
            {
                    X1_filter[ii]=(*trust_track)[loop_of_track].X1[ii];
                    X2_filter[ii]=(*trust_track)[loop_of_track].X2[ii];
                    X3_filter[ii]=(*trust_track)[loop_of_track].X3[ii];
            }
            for (int ii=0;ii<6;ii++)
                for(int jj=0;jj<6;jj++)
                {
                        P1_filter[ii][jj]=(*trust_track)[loop_of_track].P1[ii][jj];
                        P2_filter[ii][jj]=(*trust_track)[loop_of_track].P2[ii][jj];
                        P3_filter[ii][jj]=(*trust_track)[loop_of_track].P3[ii][jj];
                }

            double X_filter[6], P_filter[6][6];
            for (int i=0;i<6;i++)
                X_filter[i]=u_now[0]*X1_filter[i]+u_now[1]*X2_filter[i]+u_now[2]*X3_filter[i];

            double X1_sub_X[6], X2_sub_X[6], X3_sub_X[6];
            double X1X[6][6], X2X[6][6],X3X[6][6];
            for (int i=0;i<6;i++)
            {
                X1_sub_X[i]=X1_filter[i]-X_filter[i];
                X2_sub_X[i]=X2_filter[i]-X_filter[i];
                X3_sub_X[i]=X3_filter[i]-X_filter[i];
            }
            for(int i=0;i<6;i++)
                for (int j=0;j<6;j++)
                {
                    X1X[i][j]=X1_sub_X[i]*X1_sub_X[j];
                    X2X[i][j]=X2_sub_X[i]*X2_sub_X[j];
                    X3X[i][j]=X3_sub_X[i]*X3_sub_X[j];
                }
            for(int i=0;i<6;i++)
                for (int j=0;j<6;j++)
                    P_filter[i][j]=u_now[0]*(P1_filter[i][j]+X1X[i][j])+u_now[1]*(P2_filter[i][j]+X2X[i][j])+u_now[2]*(P3_filter[i][j]+X3X[i][j]);


            //本地航迹文件更新
            (*trust_track)[loop_of_track].X[0]=X_filter[0];  //位置 速度
            (*trust_track)[loop_of_track].X[1]=X_filter[1];
            (*trust_track)[loop_of_track].X[2]=X_filter[2];
            (*trust_track)[loop_of_track].X[3]=X_filter[3];
            (*trust_track)[loop_of_track].X[4]=X_filter[4];
            (*trust_track)[loop_of_track].X[5]=X_filter[5];
            for(int ii=0;ii<6;ii++)							//协方差矩阵
                for (int jj=0;jj<6;jj++)
                    (*trust_track)[loop_of_track].P[ii][jj]=P_filter[ii][jj];

    //        //航迹区更新
    //        double r, azi;
    //        coor_trans Coor_trans;
    //        Coor_trans.cart2polar((*trust_track)[loop_of_track].X[0], (*trust_track)[loop_of_track].X[3], &r, &azi);

    //        if(azi/PI*180>=TRACK_SECTION_1_START && azi/PI*180<TRACK_SECTION_2_START)
    //           (*trust_track)[loop_of_track].Track_section_idx = 1;

    //        if(azi/PI*180>=TRACK_SECTION_2_START && azi/PI*180<TRACK_SECTION_3_START)
    //           (*trust_track)[loop_of_track].Track_section_idx = 2;

    //        if(azi/PI*180>=TRACK_SECTION_3_START && azi/PI*180<TRACK_SECTION_4_START)
    //           (*trust_track)[loop_of_track].Track_section_idx = 3;

    //        if(azi/PI*180>=TRACK_SECTION_4_START && azi/PI*180<TRACK_SECTION_5_START)
    //           (*trust_track)[loop_of_track].Track_section_idx = 4;

    //        if(azi/PI*180>=TRACK_SECTION_5_START && azi/PI*180<TRACK_SECTION_6_START)
    //           (*trust_track)[loop_of_track].Track_section_idx = 5;

    //        if(azi/PI*180>=TRACK_SECTION_6_START && azi/PI*180<TRACK_SECTION_7_START)
    //           (*trust_track)[loop_of_track].Track_section_idx = 6;

    //        if(azi/PI*180>=TRACK_SECTION_7_START && azi/PI*180<TRACK_SECTION_8_START)
    //           (*trust_track)[loop_of_track].Track_section_idx = 7;

    //        if(azi/PI*180>=TRACK_SECTION_8_START && azi/PI*180<TRACK_SECTION_9_START)
    //           (*trust_track)[loop_of_track].Track_section_idx = 8;

    //        if(azi/PI*180>=TRACK_SECTION_9_START || azi/PI*180<TRACK_SECTION_1_START)
    //            (*trust_track)[loop_of_track].Track_section_idx = 9;

        }
//    }

}



//高度维更新
void Track_Asso:: track_hight_update(int                                 updata_track_index,                     //更新的航迹号
                                     int                                 asso_point_index,                       //点迹号
                                     QVector <Trust_Track>               *trust_track                            //航迹
                                     )
{


    if(updata_track_index<=trust_track->size() &&  asso_point_index<=point_process.size())
    {

        (*trust_track)[updata_track_index-1].Hight_smooth.push_back(point_process[asso_point_index-1].Height);


        int height_win_length;

        if((*trust_track)[updata_track_index-1].range_point <= 1000)
        {
            height_win_length = H_F_WIN_LEN+2;
        }
        else if((*trust_track)[updata_track_index-1].range_point <= 2000 && (*trust_track)[updata_track_index-1].range_point > 1000)
        {
            height_win_length = H_F_WIN_LEN+3;
        }
        else if((*trust_track)[updata_track_index-1].range_point <= 4000 && (*trust_track)[updata_track_index-1].range_point > 2000)
        {
            height_win_length = H_F_WIN_LEN+4;
        }
        else
        {
            height_win_length = H_F_WIN_LEN+6;
        }

        double sum=0;
        if((*trust_track)[updata_track_index-1].Hight_smooth.size()<height_win_length)
        {

            for (int i=0;i<(*trust_track)[updata_track_index-1].Hight_smooth.size();i++)
            {
                sum=sum+(*trust_track)[updata_track_index-1].Hight_smooth[i];

            }

            (*trust_track)[updata_track_index-1].Height=sum/(*trust_track)[updata_track_index-1].Hight_smooth.size();

        }
        else if((*trust_track)[updata_track_index-1].Hight_smooth.size()>=height_win_length)
        {

            int N=(*trust_track)[updata_track_index-1].Hight_smooth.size();
            for (int i=0;i<height_win_length;i++)
            {
                sum=sum+(*trust_track)[updata_track_index-1].Hight_smooth[N-1-i];
            }
            (*trust_track)[updata_track_index-1].Height=sum/height_win_length;
        }
    }
};


Track_Asso::Track_Asso()
{
    Pt[0][0]=0.8;Pt[0][1]=0.15;Pt[0][2]=0.05;		//IMM 转移概率
    Pt[1][0]=0.3;Pt[1][1]=0.4;Pt[1][2]=0.3;
    Pt[2][0]=0.05;Pt[2][1]=0.15;Pt[2][2]=0.8;


};