diff --git a/data_process_class_dll/coor_trans.cpp b/data_process_class_dll/coor_trans.cpp index 6f94b4c..33187c9 100644 --- a/data_process_class_dll/coor_trans.cpp +++ b/data_process_class_dll/coor_trans.cpp @@ -7,14 +7,14 @@ using namespace std; //极坐标转直角坐标 void coor_trans::polar2cart(double *x, double *y, double Range, double Azimuth) { - *x=Range*cos(Azimuth); - *y=Range*sin(Azimuth); + *x=Range*cos(Azimuth); + *y=Range*sin(Azimuth); } void coor_trans::cart2polar(double x, double y, double *Range, double *Azimuth) { - *Range=sqrt(x*x+y*y); - *Azimuth=atan2(y,x); - if(*Azimuth<0) - *Azimuth=*Azimuth+2*PI; + *Range=sqrt(x*x+y*y); + *Azimuth=atan2(y,x); + if(*Azimuth<0) + *Azimuth=*Azimuth+2*PI; } diff --git a/data_process_class_dll/coor_trans.h b/data_process_class_dll/coor_trans.h index 93eb757..6aac2f1 100644 --- a/data_process_class_dll/coor_trans.h +++ b/data_process_class_dll/coor_trans.h @@ -6,9 +6,9 @@ class coor_trans { public: - void polar2cart( double*x, double *y, double Range, double Azimuth); + void polar2cart( double*x, double *y, double Range, double Azimuth); - void cart2polar(double x, double y, double *Range, double *Azimuth); + void cart2polar(double x, double y, double *Range, double *Azimuth); }; diff --git a/data_process_class_dll/data_process.cpp b/data_process_class_dll/data_process.cpp index 37b746d..39f427a 100644 --- a/data_process_class_dll/data_process.cpp +++ b/data_process_class_dll/data_process.cpp @@ -13,71 +13,71 @@ using namespace std; //数据预处理 -int Data_Process::data_preprocess(struct DataRev Data_Input[150]) +int Data_Process::data_preprocess(struct DataRev Data_Input[150]) { - //数据存入缓存区域 Data_buffer - if(Data_Input[0].point_type == 0) //tws数据 - { - data_num=Data_Input[0].Point_Sum; - TAS_track_idx = Data_Input[0].TAS_track_index; - for (int i=0; i().swap(Data_buffer); - } + dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,Work_Parameter); + QVector().swap(Data_buffer); + } - if(model==1) - { - *Trust_track_num_Output=0; //航迹更新数 - *Track_die_num_Output=0; //航迹消亡数目 + if(model==1) + { + *Trust_track_num_Output=0; //航迹更新数 + *Track_die_num_Output=0; //航迹消亡数目 - track_asso.track_asso_process(&point_recv, - &trust_track, - Trust_Track_Output, - Trust_track_num_Output, - Work_Parameter - ); + track_asso.track_asso_process(&point_recv, + &trust_track, + Trust_Track_Output, + Trust_track_num_Output, + Work_Parameter + ); - track_die.track_die_process( &trust_track, - Track_die_Index_Output, - Track_die_num_Output); + track_die.track_die_process( &trust_track, + Track_die_Index_Output, + Track_die_num_Output); - track_init.track_init_process_logic( &point_recv, - &trust_track, - &temp_track, - Trust_Track_Output, - Trust_track_num_Output, - Work_Parameter); + track_init.track_init_process_logic( &point_recv, + &trust_track, + &temp_track, + Trust_Track_Output, + Trust_track_num_Output, + Work_Parameter); - } + } - // TAS处理 - if(model==3) - { - *Trust_track_num_Output=0; //航迹更新数 - *Track_die_num_Output=0; //航迹消亡数目 + // TAS处理 + if(model==3) + { + *Trust_track_num_Output=0; //航迹更新数 + *Track_die_num_Output=0; //航迹消亡数目 - dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas); //点迹凝聚 + dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas); //点迹凝聚 // if(point_recv_tas.size()>0) // { @@ -157,42 +157,42 @@ int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][1 // } - QVector().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer + QVector().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer - track_asso_tas.track_asso_process_tas(&point_recv_tas, //点迹文件 - &trust_track, //航迹文件 - Trust_Track_Output, //更新航迹信息 - Trust_track_num_Output, //更新航迹数 - Work_Parameter, //工作参数 - TAS_track_idx); + track_asso_tas.track_asso_process_tas(&point_recv_tas, //点迹文件 + &trust_track, //航迹文件 + Trust_Track_Output, //更新航迹信息 + Trust_track_num_Output, //更新航迹数 + Work_Parameter, //工作参数 + TAS_track_idx); - QVector().swap(point_recv_tas); //清除 point_recv_tas + QVector().swap(point_recv_tas); //清除 point_recv_tas - track_die_tas.track_die_process_tas( &trust_track, - Track_die_Index_Output, - Track_die_num_Output); + track_die_tas.track_die_process_tas( &trust_track, + Track_die_Index_Output, + Track_die_num_Output); - } + } - return 1; + return 1; } //波束控制 -void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam, - struct Track Trust_Track_Output[][10], - int *Trust_track_num_Output) +void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam, + struct Track Trust_Track_Output[][10], + int *Trust_track_num_Output) { - tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,Work_Parameter); + tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,Work_Parameter); } @@ -202,10 +202,10 @@ void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam, int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parameter) { - //工作参数设置 - memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara)); + //工作参数设置 + memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara)); - return 0; + return 0; } @@ -216,59 +216,59 @@ int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parame //数据处理参数修改 int Data_Process::track_process_parameters_modify(struct RadarPara Radar_Parameter) { - memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara)); - return 0; + memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara)); + return 0; } //航迹清空函数 int Data_Process::track_clear_all(void) { - //清空可靠航迹 + //清空可靠航迹 - return 0; + return 0; } //手动航迹删除函数 -int Data_Process:: track_delete(int delete_track_num, //手动删除的航迹数目 - int delete_track_index[]) //手动删除的航迹号 +int Data_Process:: track_delete(int delete_track_num, //手动删除的航迹数目 + int delete_track_index[]) //手动删除的航迹号 { - for (int i=0;i Data_buffer; //接收点迹 TWS 输入凝聚 - QVector Data_buffer_tas; //接收点迹 TAS 输入凝聚 + QVector Data_buffer; //接收点迹 TWS 输入凝聚 + QVector Data_buffer_tas; //接收点迹 TAS 输入凝聚 - QVector point_recv; //凝聚后输出的点迹 按点迹区存入 输入航迹关联、起始 - QVector point_recv_tas; //凝聚后输出的点迹 TAS - QVector trust_track; //可靠航迹 按航迹区存入 - QVector > temp_track; //临时航迹 按临时航迹区存入 + QVector point_recv; //凝聚后输出的点迹 按点迹区存入 输入航迹关联、起始 + QVector point_recv_tas; //凝聚后输出的点迹 TAS + QVector trust_track; //可靠航迹 按航迹区存入 + QVector > temp_track; //临时航迹 按临时航迹区存入 - int Beam_num; //波位数 - int beam_count; //波位计数 - int data_num; //波位1点数 + int Beam_num; //波位数 + int beam_count; //波位计数 + int data_num; //波位1点数 - int beam_index[3]; //3个波位号(当前帧) + int beam_index[3]; //3个波位号(当前帧) - int TAS_track_idx; //TAS数据的目标批号 + int TAS_track_idx; //TAS数据的目标批号 - int Track_Mode; //跟踪模式 1TAS 0TWS + int Track_Mode; //跟踪模式 1TAS 0TWS - struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入 + struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入 - Dot_Coh dot_coh; //点迹凝聚 + Dot_Coh dot_coh; //点迹凝聚 - Dot_Coh_TAS dot_coh_tas; + Dot_Coh_TAS dot_coh_tas; - Track_Asso track_asso; //航迹关联 + Track_Asso track_asso; //航迹关联 - Track_Asso_Tas track_asso_tas; + Track_Asso_Tas track_asso_tas; - Track_Init track_init; //航迹起始 + Track_Init track_init; //航迹起始 - Track_Die track_die; //航迹消亡 + Track_Die track_die; //航迹消亡 - Track_Die_Tas track_die_tas; + Track_Die_Tas track_die_tas; - TAS_Ctrl tas_ctrl; //TAS波束控制 + TAS_Ctrl tas_ctrl; //TAS波束控制 }; diff --git a/data_process_class_dll/data_process_class_dll.cpp b/data_process_class_dll/data_process_class_dll.cpp index 22c5fb0..6fcef3e 100644 --- a/data_process_class_dll/data_process_class_dll.cpp +++ b/data_process_class_dll/data_process_class_dll.cpp @@ -13,12 +13,12 @@ Data_process_class_dll *Data_Process_Factory::p = 0; Data_process_class_dll *Data_Process_Factory::GetB() { - if(!p) - { - p=new Data_Process(); - } + if(!p) + { + p=new Data_Process(); + } - return p; + return p; } void Data_Process_Factory::Destroy() @@ -26,8 +26,8 @@ void Data_Process_Factory::Destroy() // if (p) // delete p; - if (p) { - delete p; - p = nullptr; - } + if (p) { + delete p; + p = nullptr; + } } diff --git a/data_process_class_dll/data_process_class_dll.h b/data_process_class_dll/data_process_class_dll.h index 09b2b55..e21d48c 100644 --- a/data_process_class_dll/data_process_class_dll.h +++ b/data_process_class_dll/data_process_class_dll.h @@ -8,74 +8,74 @@ // 输入点迹的数据结构 用户可见 struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT DataRev { - float Range; //径向距离 （米） - float Azimuth; //方位 （度） - float Elevation; //俯仰 （度） - float Velocity; //速度 （m/s） - float Amplitude; //幅度 - float Threshold; //目标门限 - float Snr; //目标信噪比 - float RCS; //目标RCS - int CPI_time; //CPI时间 该波位无点迹输入时也需要给出当前时间 - int Beam_index_azi_0; //上一个cpi波位 - int Beam_index_aiz; //方位波位号（0-24） 该波位无输入点迹时 也需要给出当前波位 - int Beam_index_elev; //俯仰波位号（0-13） - int PRI; //PRI(us) 给0 - int Freq_index; //频点号 给0 - int Point_Sum; //输入点迹总数 若该波位无点迹输入 置为0 - int Point_Num; //点迹号(1,2,3,4...) - int UseFlag; //点迹使用标记 使用1/未使用0 主程序存入数据时置为0 - int TAS_track_index; //TAS的航迹号 - int point_type; //点迹类型 TAS 1/TWS 0 - int pitch_num; // 俯仰波位号 - float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 - float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 + float Range; //径向距离 （米） + float Azimuth; //方位 （度） + float Elevation; //俯仰 （度） + float Velocity; //速度 （m/s） + float Amplitude; //幅度 + float Threshold; //目标门限 + float Snr; //目标信噪比 + float RCS; //目标RCS + int CPI_time; //CPI时间 该波位无点迹输入时也需要给出当前时间 + int Beam_index_azi_0; //上一个cpi波位 + int Beam_index_aiz; //方位波位号（0-24） 该波位无输入点迹时 也需要给出当前波位 + int Beam_index_elev; //俯仰波位号（0-13） + int PRI; //PRI(us) 给0 + int Freq_index; //频点号 给0 + int Point_Sum; //输入点迹总数 若该波位无点迹输入 置为0 + int Point_Num; //点迹号(1,2,3,4...) + int UseFlag; //点迹使用标记 使用1/未使用0 主程序存入数据时置为0 + int TAS_track_index; //TAS的航迹号 + int point_type; //点迹类型 TAS 1/TWS 0 + int pitch_num; // 俯仰波位号 + float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 + float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 }; //输出航迹的数据结构 用户可见 struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Track { - float x; //x坐标 - float y; //y坐标 - float z; //z坐标 - float v_x; //x速度 - float v_y; //y速度 - float v_z; //z速度 - float Range; //径向距离 - float Azimuth; //方位 - float Elevation; //俯仰 - float Range_V; //速度 - float Amplitude; //幅度 - int Track_Index; //航迹号 - int Point_Sum; //本条航迹更新点的数目 - int Flag_Direction; //方向标志1靠近/0远离 - int Flag_Point; //点迹类型1实点/0补点 - float x_Predict; //x坐标的一步预测 - float y_Predict; //y坐标的一步预测 - float z_Predict; //z坐标的一步预测 - float Range_Predict; //径向距离的一步预测 - float Azimuth_Predict; //方位的一步预测 - float Elevation_Predict; //俯仰的一步预测 - int Track_Mode; //跟踪模式,TAS 1/TWS 0 - float Direction_Angle; //目标航向角 - int Target_Type; //目标类型 + float x; //x坐标 + float y; //y坐标 + float z; //z坐标 + float v_x; //x速度 + float v_y; //y速度 + float v_z; //z速度 + float Range; //径向距离 + float Azimuth; //方位 + float Elevation; //俯仰 + float Range_V; //速度 + float Amplitude; //幅度 + int Track_Index; //航迹号 + int Point_Sum; //本条航迹更新点的数目 + int Flag_Direction; //方向标志1靠近/0远离 + int Flag_Point; //点迹类型1实点/0补点 + float x_Predict; //x坐标的一步预测 + float y_Predict; //y坐标的一步预测 + float z_Predict; //z坐标的一步预测 + float Range_Predict; //径向距离的一步预测 + float Azimuth_Predict; //方位的一步预测 + float Elevation_Predict; //俯仰的一步预测 + int Track_Mode; //跟踪模式,TAS 1/TWS 0 + float Direction_Angle; //目标航向角 + int Target_Type; //目标类型 - int point_type; //点迹类型 TAS 1/TWS 0 + int point_type; //点迹类型 TAS 1/TWS 0 - float range_point; //关联上的点迹信息（距离、方位、俯仰、径向速度） - float azi_point; - float elev_point; - float vr_point; - float prf_point; + float range_point; //关联上的点迹信息（距离、方位、俯仰、径向速度） + float azi_point; + float elev_point; + float vr_point; + float prf_point; - float track_time; //航迹时间 (单位 s) - float track_snr; //信噪比 - float track_rcs; //rcs + float track_time; //航迹时间 (单位 s) + float track_snr; //信噪比 + float track_rcs; //rcs - int pitch_num; // 俯仰波位号 - float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 - float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 + int pitch_num; // 俯仰波位号 + float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 + float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 }; @@ -83,24 +83,24 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Track // 跟踪波束信息的结构体 struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT TrackingBeam { - int open_flag; //是否开启 1开启 0不开启 + int open_flag; //是否开启 1开启 0不开启 - int type; //类型 - float Range; //跟踪目标距离 - float Azi; //跟踪目标方位 - float Elev; //跟踪目标俯仰 + int type; //类型 + float Range; //跟踪目标距离 + float Azi; //跟踪目标方位 + float Elev; //跟踪目标俯仰 - int TAS_track_index; //目标批号 + int TAS_track_index; //目标批号 }; //引导跟踪信息的结构体 struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Target_direct_tracking { - int Track_ID; //引导目标批号 - float Range; //引导目标距离 - float Azi; //引导目标方位 - float Elev; //引导目标俯仰 - int open_flag; // 1 引导跟踪 0 结束引导跟踪 + int Track_ID; //引导目标批号 + float Range; //引导目标距离 + float Azi; //引导目标方位 + float Elev; //引导目标俯仰 + int open_flag; // 1 引导跟踪 0 结束引导跟踪 }; @@ -108,65 +108,65 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Target_direct_tracking // 雷达参数结构体 struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara { - int Beam_num; //波位数 + int Beam_num; //波位数 - float Height; //雷达平台高度 （m） + float Height; //雷达平台高度 （m） - float Sys_delay; //系统延迟 (s) - float R_TAS_max; //TAS最大距离 （m） - float R_TAS_min; //TAS最小距离 (m) + float Sys_delay; //系统延迟 (s) + float R_TAS_max; //TAS最大距离 （m） + float R_TAS_min; //TAS最小距离 (m) - float V_TAS_max; //TAS最大速度 (m/s) - float V_TAS_min; //TAS最小速度 (m/s) + float V_TAS_max; //TAS最大速度 (m/s) + float V_TAS_min; //TAS最小速度 (m/s) - float TAS_Height_1; //TAS跟踪高度1 (m) - float TAS_Height_2; //TAS跟踪高度2 (m) - float TAS_Height_3; //TAS跟踪高度3 (m) - float TAS_Height_4; //TAS跟踪高度4 (m) - float TAS_Height_5; //TAS跟踪高度5 (m) + float TAS_Height_1; //TAS跟踪高度1 (m) + float TAS_Height_2; //TAS跟踪高度2 (m) + float TAS_Height_3; //TAS跟踪高度3 (m) + float TAS_Height_4; //TAS跟踪高度4 (m) + float TAS_Height_5; //TAS跟踪高度5 (m) - int track_prohibite_area_num; //禁止航迹起始的区域数目 - float R_max_track_prohibited[30]; //最大距离 (m) - float R_min_track_prohibited[30]; //最小距离 (m) - float Azimuth_max_track_prohibited[30]; //最大方位角 (度) - float Azimuth_min_track_prohibited[30]; //最小方位角 （度） + int track_prohibite_area_num; //禁止航迹起始的区域数目 + float R_max_track_prohibited[30]; //最大距离 (m) + float R_min_track_prohibited[30]; //最小距离 (m) + float Azimuth_max_track_prohibited[30]; //最大方位角 (度) + float Azimuth_min_track_prohibited[30]; //最小方位角 （度） - int TAS_prohibite_area_num; //禁止TAS的区域数目 - float R_max_TAS_prohibited[30]; //最大距离 （m） - float R_min_TAS_prohibited[30]; //最小距离 (m) - float Azimuth_max_TAS_prohibited[30]; //最大方位角 (度) - float Azimuth_min_TAS_prohibited[30]; //最小方位角 (度) + int TAS_prohibite_area_num; //禁止TAS的区域数目 + float R_max_TAS_prohibited[30]; //最大距离 （m） + float R_min_TAS_prohibited[30]; //最小距离 (m) + float Azimuth_max_TAS_prohibited[30]; //最大方位角 (度) + float Azimuth_min_TAS_prohibited[30]; //最小方位角 (度) - int cfar_th; //cfar门限 + int cfar_th; //cfar门限 - int work_mode; //工作模式 0进程 1中程 3远程 - int north_angle; //北偏角 + int work_mode; //工作模式 0进程 1中程 3远程 + int north_angle; //北偏角 - float V_MAX; //最大速度 - float V_MIN; //最小速度 + float V_MAX; //最大速度 + float V_MIN; //最小速度 - float DATA_RATE_SHORT; //三种不同模式下的 数据率 - float DATA_RATE_MIDDLE; - float DATA_RATE_FAR; + float DATA_RATE_SHORT; //三种不同模式下的 数据率 + float DATA_RATE_MIDDLE; + float DATA_RATE_FAR; - //数据关联参数 - int track_start_point_num; //起批点数(典型值 3或4) - float track_start_threshold; //起航波门大小(典型值 3) - float track_asso_threshold; //关联波门大小(典型值 3) - float track_asso_threshold_tas; //关联波门大小tas (典型值 3) + //数据关联参数 + int track_start_point_num; //起批点数(典型值 3或4) + float track_start_threshold; //起航波门大小(典型值 3) + float track_asso_threshold; //关联波门大小(典型值 3) + float track_asso_threshold_tas; //关联波门大小tas (典型值 3) - //singer模型机动参数 - float Model1_Q_fast; //0.00001 - float Model1_Q_slow; //0.00001 + //singer模型机动参数 + float Model1_Q_fast; //0.00001 + float Model1_Q_slow; //0.00001 - float Model2_Q_fast; //0.1 - float Model2_Q_slow; //0.001 + float Model2_Q_fast; //0.1 + float Model2_Q_slow; //0.001 - float Model3_Q_fast; //0.8 - float Model3_Q_slow; //0.4 + float Model3_Q_fast; //0.8 + float Model3_Q_slow; //0.4 }; @@ -178,90 +178,90 @@ class DATA_PROCESS_CLASS_DLLSHARED_EXPORT Data_process_class_dll public: // Data_process_class_dll(); - //数据预处理函数 - virtual int data_preprocess(struct DataRev Data_Input[150]) //雷达的输入点迹 主程序创建全局变量 输入数据 - { - return 0; - } + //数据预处理函数 + virtual int data_preprocess(struct DataRev Data_Input[150]) //雷达的输入点迹 主程序创建全局变量 输入数据 + { + return 0; + } - //数据处理函数 - virtual int track_process(struct Track Trust_Track_Output[][10], //数据处理输出的航迹更新信息 主程序创建全局变量 数据处理函数更新其中数据 - int *Trust_track_num_Output, //数据处理后输出的航迹数目 主程序创建全局变量 数据处理函数更新其中数据 - int Track_die_Index_Output[], //数据处理后要消亡的航迹 主程序创建全局变量 数据处理函数更新其中数据 - int *Track_die_num_Output, //数据处理后要消亡的航迹数目 - int model //模式 输入data_preprocess的返回值 - ) - { - return 0; - } + //数据处理函数 + virtual int track_process(struct Track Trust_Track_Output[][10], //数据处理输出的航迹更新信息 主程序创建全局变量 数据处理函数更新其中数据 + int *Trust_track_num_Output, //数据处理后输出的航迹数目 主程序创建全局变量 数据处理函数更新其中数据 + int Track_die_Index_Output[], //数据处理后要消亡的航迹 主程序创建全局变量 数据处理函数更新其中数据 + int *Track_die_num_Output, //数据处理后要消亡的航迹数目 + int model //模式 输入data_preprocess的返回值 + ) + { + return 0; + } - //波束控制 - virtual void Beam_Ctrl(struct TrackingBeam *Tracking_beam, - struct Track Trust_Track_Output[][10], - int *Trust_track_num_Output) - { + //波束控制 + virtual void Beam_Ctrl(struct TrackingBeam *Tracking_beam, + struct Track Trust_Track_Output[][10], + int *Trust_track_num_Output) + { - } + } - //引导跟踪处理函数 - virtual int direct_tracking_process(struct Target_direct_tracking Target_direct_info, //引导信息 - struct DataRev Data_Input[150], //雷达的输入点迹 - struct Track Trust_Track_Output[][10], //数据处理输出的航迹更新信息 主程序创建全局变量 数据处理函数更新其中数据 - int *Trust_track_num_Output, //数据处理后输出的航迹数目 主程序创建全局变量 数据处理函数更新其中数据 - int Track_die_Index_Output[], //数据处理后要消亡的航迹 主程序创建全局变量 数据处理函数更新其中数据 - int *Track_die_num_Output, //数据处理后要消亡的航迹数目 - struct TrackingBeam *Tracking_beam //波束控制信息 - ) - { - return 0; - } + //引导跟踪处理函数 + virtual int direct_tracking_process(struct Target_direct_tracking Target_direct_info, //引导信息 + struct DataRev Data_Input[150], //雷达的输入点迹 + struct Track Trust_Track_Output[][10], //数据处理输出的航迹更新信息 主程序创建全局变量 数据处理函数更新其中数据 + int *Trust_track_num_Output, //数据处理后输出的航迹数目 主程序创建全局变量 数据处理函数更新其中数据 + int Track_die_Index_Output[], //数据处理后要消亡的航迹 主程序创建全局变量 数据处理函数更新其中数据 + int *Track_die_num_Output, //数据处理后要消亡的航迹数目 + struct TrackingBeam *Tracking_beam //波束控制信息 + ) + { + return 0; + } - //航迹清空函数 - virtual int track_clear_all(void) - { - return 0; - } + //航迹清空函数 + virtual int track_clear_all(void) + { + return 0; + } - //手动航迹删除函数 - virtual int track_delete(int delete_track_num, //手动删除的航迹数目 - int delete_track_index[]) //手动删除的航迹号 - { - return 0; - } + //手动航迹删除函数 + virtual int track_delete(int delete_track_num, //手动删除的航迹数目 + int delete_track_index[]) //手动删除的航迹号 + { + return 0; + } - //手动转TAS跟踪函数 - virtual int tracking_start(int track_index) //需要手动转入TAS跟踪的航迹号 - { - return 0; - } + //手动转TAS跟踪函数 + virtual int tracking_start(int track_index) //需要手动转入TAS跟踪的航迹号 + { + return 0; + } - //手动打跟踪波束 - virtual int tracking_point(float Azimuth) //方位角 - { - return 0; - } + //手动打跟踪波束 + virtual int tracking_point(float Azimuth) //方位角 + { + return 0; + } - //参数设置初始化函数 - virtual int track_process_parameters_initial(struct RadarPara Radar_Parameter) - { - return 0; - } + //参数设置初始化函数 + virtual int track_process_parameters_initial(struct RadarPara Radar_Parameter) + { + return 0; + } - //参数设置修改函数 - virtual int track_process_parameters_modify(struct RadarPara Radar_Parameter) - { - return 0; - } + //参数设置修改函数 + virtual int track_process_parameters_modify(struct RadarPara Radar_Parameter) + { + return 0; + } @@ -272,11 +272,11 @@ public: class DATA_PROCESS_CLASS_DLLSHARED_EXPORT Data_Process_Factory { public: - static Data_process_class_dll *GetB(); //创建 - static void Destroy(); //销毁 + static Data_process_class_dll *GetB(); //创建 + static void Destroy(); //销毁 private: - static Data_process_class_dll *p; + static Data_process_class_dll *p; }; diff --git a/data_process_class_dll/dot_coh.cpp b/data_process_class_dll/dot_coh.cpp index 0f0325a..902151f 100644 --- a/data_process_class_dll/dot_coh.cpp +++ b/data_process_class_dll/dot_coh.cpp @@ -6,249 +6,249 @@ using namespace std; int Dot_Coh::dot_coh_process(QVector *data_input, - QVector *point_recv, - struct RadarPara Work_Parameter) + QVector *point_recv, + struct RadarPara Work_Parameter) { - if(data_input->size()>1) - { + if(data_input->size()>1) + { - for (unsigned int loop_of_point=0; loop_of_pointsize()-1;loop_of_point++ ) - { - if( (*data_input)[loop_of_point].Use_Flag!=1) - { - float point_0_R=(*data_input)[loop_of_point].Range; - float point_0_V=(*data_input)[loop_of_point].Velocity; - float point_0_F=(*data_input)[loop_of_point].Azimuth; - float point_0_A=(*data_input)[loop_of_point].Amplitude; - for (unsigned int i=loop_of_point+1;isize();i++) - { - if((*data_input)[loop_of_point].Use_Flag!=1) - { - float point_1_R=(*data_input)[i].Range; - float point_1_V=(*data_input)[i].Velocity; - float point_1_F=(*data_input)[i].Azimuth; - float point_1_A=(*data_input)[i].Amplitude; + for (unsigned int loop_of_point=0; loop_of_pointsize()-1;loop_of_point++ ) + { + if( (*data_input)[loop_of_point].Use_Flag!=1) + { + float point_0_R=(*data_input)[loop_of_point].Range; + float point_0_V=(*data_input)[loop_of_point].Velocity; + float point_0_F=(*data_input)[loop_of_point].Azimuth; + float point_0_A=(*data_input)[loop_of_point].Amplitude; + for (unsigned int i=loop_of_point+1;isize();i++) + { + if((*data_input)[loop_of_point].Use_Flag!=1) + { + float point_1_R=(*data_input)[i].Range; + float point_1_V=(*data_input)[i].Velocity; + float point_1_F=(*data_input)[i].Azimuth; + float point_1_A=(*data_input)[i].Amplitude; - //凝聚条件： 距离、方位接近 - if(Work_Parameter.work_mode == 0) - { - if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)// - && point_0_A = point_1_A)// - { - (*data_input)[i].Use_Flag=1; - } - } - else - { - float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F); - if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )// - && point_0_A = point_1_A)// - { - (*data_input)[i].Use_Flag=1; - } + //凝聚条件： 距离、方位接近 + if(Work_Parameter.work_mode == 0) + { + if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)// + && point_0_A = point_1_A)// + { + (*data_input)[i].Use_Flag=1; + } + } + else + { + float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F); + if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )// + && point_0_A = point_1_A)// + { + (*data_input)[i].Use_Flag=1; + } - } - } - } - } - } - } + } + } + } + } + } + } - //删除凝聚点 和 量程范围外点 - QVector ::iterator Iter; - for (Iter=data_input->begin(); Iter!=data_input->end();) - { - if((*Iter).Use_Flag==1 || (*Iter).Range R_MAX ) - { - data_input->erase(Iter); - Iter=data_input->begin(); - } - else - { - Iter++; - } - } + //删除凝聚点 和 量程范围外点 + QVector ::iterator Iter; + for (Iter=data_input->begin(); Iter!=data_input->end();) + { + if((*Iter).Use_Flag==1 || (*Iter).Range R_MAX ) + { + data_input->erase(Iter); + Iter=data_input->begin(); + } + else + { + Iter++; + } + } - for (int i=0;isize();i++) //data_buffer_2 ---> point_recv - { - (*point_recv).push_back((*data_input)[i]); - } + for (int i=0;isize();i++) //data_buffer_2 ---> point_recv + { + (*point_recv).push_back((*data_input)[i]); + } - return 0; + return 0; } int Dot_Coh::dot_coh_process_buff( QVector *data_input, //输入的点迹 - QVector *point_recv, //输出点迹 - struct RadarPara Work_Parameter //工作参数 + QVector *point_recv, //输出点迹 + struct RadarPara Work_Parameter //工作参数 ) { - //1. data_input与data_input_buff进行凝聚 + //1. data_input与data_input_buff进行凝聚 - //1.1 data_input、data_input_buff中的数据放在一起 - QVector data_tmp; - for (int i=0;i data_tmp; + for (int i=0;isize();i++) - { - data_tmp.push_back((*data_input)[i]); - data_tmp[data_tmp.size()-1].point_section_asso = 2; - } + } + for (int i=0;isize();i++) + { + data_tmp.push_back((*data_input)[i]); + data_tmp[data_tmp.size()-1].point_section_asso = 2; + } - //1.2 把data_input、data_input_buff清空 + //1.2 把data_input、data_input_buff清空 - QVector().swap(data_input_buff); - QVector().swap(*data_input); + QVector().swap(data_input_buff); + QVector().swap(*data_input); - //1.3 对data_tmp进行凝聚 - if(data_tmp.size()>1) - { + //1.3 对data_tmp进行凝聚 + if(data_tmp.size()>1) + { - for (unsigned int loop_of_point=0; loop_of_point= point_1_A)// - { - data_tmp[i].Use_Flag=1; - } - } - else - { - float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F); - if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )// - && point_0_A = point_1_A)// - { - data_tmp[i].Use_Flag=1; - } + //凝聚条件： 距离、方位接近 + if(Work_Parameter.work_mode == 0) + { + if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)// + && point_0_A = point_1_A)// + { + data_tmp[i].Use_Flag=1; + } + } + else + { + float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F); + if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )// + && point_0_A = point_1_A)// + { + data_tmp[i].Use_Flag=1; + } - } - } - } - } - } - } + } + } + } + } + } + } - //1.4 删除凝聚点 和 量程范围外点 - QVector ::iterator Iter; - for (Iter=data_tmp.begin(); Iter!=data_tmp.end();) - { - if((*Iter).Use_Flag==1 || (*Iter).Range R_MAX ) - { - data_tmp.erase(Iter); - Iter=data_tmp.begin(); - } - else - { - Iter++; - } - } + //1.4 删除凝聚点 和 量程范围外点 + QVector ::iterator Iter; + for (Iter=data_tmp.begin(); Iter!=data_tmp.end();) + { + if((*Iter).Use_Flag==1 || (*Iter).Range R_MAX ) + { + data_tmp.erase(Iter); + Iter=data_tmp.begin(); + } + else + { + Iter++; + } + } - //1.5 将data_tmp中凝聚后的点再分到 data_input_buff和data_input中 - for (int i=0;i().swap(data_tmp); + if(data_tmp[i].point_section_asso==2) + { + (*data_input).push_back(data_tmp[i]); + } + } + QVector().swap(data_tmp); - //2.data_input_buff数据输出给point_recv + //2.data_input_buff数据输出给point_recv - for (int i=0;i().swap(data_input_buff); + QVector().swap(data_input_buff); - //3.data_input数据输出给data_input_buff - for (int i=0;isize();i++) - { - data_input_buff.push_back((*data_input)[i]); - } + //3.data_input数据输出给data_input_buff + for (int i=0;isize();i++) + { + data_input_buff.push_back((*data_input)[i]); + } - return 0; + return 0; } diff --git a/data_process_class_dll/dot_coh.h b/data_process_class_dll/dot_coh.h index d1d422c..a18666a 100644 --- a/data_process_class_dll/dot_coh.h +++ b/data_process_class_dll/dot_coh.h @@ -13,29 +13,29 @@ class Dot_Coh { public: - // 点迹凝聚函数 - int dot_coh_process( QVector *data_input, //输入的点迹 - QVector *point_recv, //输出点迹 - struct RadarPara Work_Parameter //工作参数 - ); + // 点迹凝聚函数 + int dot_coh_process( QVector *data_input, //输入的点迹 + QVector *point_recv, //输出点迹 + struct RadarPara Work_Parameter //工作参数 + ); - //点迹凝聚函数，缓存一帧，一边输出，一边进行滑窗凝聚 - int dot_coh_process_buff( QVector *data_input, //输入的点迹 - QVector *point_recv, //输出点迹 - struct RadarPara Work_Parameter //工作参数 - ); + //点迹凝聚函数，缓存一帧，一边输出，一边进行滑窗凝聚 + int dot_coh_process_buff( QVector *data_input, //输入的点迹 + QVector *point_recv, //输出点迹 + struct RadarPara Work_Parameter //工作参数 + ); - //构造函数 - Dot_Coh(); + //构造函数 + Dot_Coh(); private: - QVector data_input_buff; + QVector data_input_buff; }; diff --git a/data_process_class_dll/dot_coh_tas.cpp b/data_process_class_dll/dot_coh_tas.cpp index c37d236..aeeffb0 100644 --- a/data_process_class_dll/dot_coh_tas.cpp +++ b/data_process_class_dll/dot_coh_tas.cpp @@ -7,73 +7,73 @@ using namespace std; int Dot_Coh_TAS::dot_coh_tas_process(QVector *data_input, //输入点迹 - QVector *point_recv_tas //输出点迹 - ) + QVector *point_recv_tas //输出点迹 + ) { - for (int loop_of_point=0; loop_of_pointsize()-1;loop_of_point++ ) - { - if( (*data_input)[loop_of_point].Use_Flag!=1) - { - float point_0_R=(*data_input)[loop_of_point].Range; - float point_0_V=(*data_input)[loop_of_point].Velocity; - float point_0_F=(*data_input)[loop_of_point].Azimuth; - float point_0_A=(*data_input)[loop_of_point].Amplitude; - for ( int i=loop_of_point+1;i<(*data_input).size();i++) - { - if((*data_input)[loop_of_point].Use_Flag!=1) - { - float point_1_R=(*data_input)[i].Range; - float point_1_V=(*data_input)[i].Velocity; - float point_1_F=(*data_input)[i].Azimuth; - float point_1_A=(*data_input)[i].Amplitude; + for (int loop_of_point=0; loop_of_pointsize()-1;loop_of_point++ ) + { + if( (*data_input)[loop_of_point].Use_Flag!=1) + { + float point_0_R=(*data_input)[loop_of_point].Range; + float point_0_V=(*data_input)[loop_of_point].Velocity; + float point_0_F=(*data_input)[loop_of_point].Azimuth; + float point_0_A=(*data_input)[loop_of_point].Amplitude; + for ( int i=loop_of_point+1;i<(*data_input).size();i++) + { + if((*data_input)[loop_of_point].Use_Flag!=1) + { + float point_1_R=(*data_input)[i].Range; + float point_1_V=(*data_input)[i].Velocity; + float point_1_F=(*data_input)[i].Azimuth; + float point_1_A=(*data_input)[i].Amplitude; - //凝聚条件： 距离、方位接近 - if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V) - && point_0_A = point_1_A) - { - (*data_input)[i].Use_Flag=1; - } - } - } - } - } + //凝聚条件： 距离、方位接近 + if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V) + && point_0_A = point_1_A) + { + (*data_input)[i].Use_Flag=1; + } + } + } + } + } - //删除凝聚点 和 量程范围外点 - QVector ::iterator Iter; - for (Iter=data_input->begin(); Iter!=data_input->end();) - { - if((*Iter).Use_Flag==1 || (*Iter).Range R_MAX ) - { - data_input->erase(Iter); - Iter=data_input->begin(); - } - else - { - Iter++; - } - } + //删除凝聚点 和 量程范围外点 + QVector ::iterator Iter; + for (Iter=data_input->begin(); Iter!=data_input->end();) + { + if((*Iter).Use_Flag==1 || (*Iter).Range R_MAX ) + { + data_input->erase(Iter); + Iter=data_input->begin(); + } + else + { + Iter++; + } + } - for (int i=0;isize();i++) - { - (*point_recv_tas).push_back((*data_input)[i]); - } + for (int i=0;isize();i++) + { + (*point_recv_tas).push_back((*data_input)[i]); + } - return 1; + return 1; } diff --git a/data_process_class_dll/dot_coh_tas.h b/data_process_class_dll/dot_coh_tas.h index 35efc56..337f19b 100644 --- a/data_process_class_dll/dot_coh_tas.h +++ b/data_process_class_dll/dot_coh_tas.h @@ -8,10 +8,10 @@ class Dot_Coh_TAS { public: - // 点迹凝聚函数 - int dot_coh_tas_process(QVector *data_input, //输入点迹 - QVector *point_recv_tas //输出点迹 - ); + // 点迹凝聚函数 + int dot_coh_tas_process(QVector *data_input, //输入点迹 + QVector *point_recv_tas //输出点迹 + ); }; diff --git a/data_process_class_dll/kalman.cpp b/data_process_class_dll/kalman.cpp index 082c5be..1064912 100644 --- a/data_process_class_dll/kalman.cpp +++ b/data_process_class_dll/kalman.cpp @@ -14,44 +14,44 @@ 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; + 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 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[1][1]; + 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[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[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[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[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); + 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); } @@ -59,67 +59,67 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub 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; + //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; + //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; + 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; + //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; + 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); + 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); + 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(); + 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; + //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; + //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; + return d; } @@ -127,76 +127,76 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T) 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]; + 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); + 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 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]; + 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(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]; + 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 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 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]; + 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][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]=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]; + for (int i=0;i<3;i++) + for (int j=0;j<3;j++) + P[i+3][j+3]=P22[i][j]; }; @@ -205,123 +205,123 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d 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]); + 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++) + 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]; + 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; + //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; + 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); + //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); + 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; + //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; + //d + Vector2d delta_z; + delta_z=Z_mea-Z_pred; + double d=delta_z.transpose()*S.inverse()*delta_z; - return d; + 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); + 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++) + { + 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]; + 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; + 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); + 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; + 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; + 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; + //S + Matrix3d S; + S=H*P_pred*H.transpose()+R; - //bind_speed - double v_bind=Bind_speed(prt, freq_ind); + //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; + //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; + return d; } @@ -330,492 +330,492 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6] 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); + 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++) + { + 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]; + 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); + 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; + X_pred = F1*X1; + P_pred = F1*P1*F1.transpose()+Q1; - //Z(k+1|k) - double x=X[0]; - double vx=X[1]; - double y=X[3]; - double vy=X[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; + //Z(k+1|k) + double x=X[0]; + double vx=X[1]; + double y=X[3]; + double vy=X[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; + 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); + //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; + 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; + //S + Matrix3d S; + S=H*P_pred*H.transpose()+R; // bind_speed - double v_bind=Bind_speed(prt, freq_ind); + 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; + //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; + 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); + 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++) + { + 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]; + 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); + VectorXd X1_pred(6); + MatrixXd P1_pred(6,6); - X1_pred = F1*X1; - P1_pred = F1*P1*F1.transpose()+Q1; + 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++) + 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); + 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 ) + 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); + 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++) + { + 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]; + 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); + VectorXd X_pred(6); + MatrixXd P_pred(6,6); - X_pred = F1*X1; - P_pred = F1*P1*F1.transpose()+Q1; + X_pred = F1*X1; + P_pred = F1*P1*F1.transpose()+Q1; - Vector3d Z_mea(Z[0],Z[1],Z[2]); + 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); + 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; + 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; + 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; + //S + Matrix3d S; + S=H*P_pred*H.transpose()+R; - //kalmam gain - MatrixXd K; - K=P_pred*H.transpose()*S.inverse(); + //kalmam gain + MatrixXd K; + K=P_pred*H.transpose()*S.inverse(); - //bind_speed - double v_bind=Bind_speed(prt, freq_ind); + //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; + //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; + //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(k+1|k+1) + MatrixXd P; + MatrixXd I; + I.setIdentity(6, 6); - P=(I-K*H)*P_pred; + 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++) + 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<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); + 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]) + 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); + 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++) + { + 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]; + 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); + VectorXd X_pred(6); + MatrixXd P_pred(6,6); - X_pred = F1*X1; - P_pred = F1*P1*F1.transpose()+Q1; + X_pred = F1*X1; + P_pred = F1*P1*F1.transpose()+Q1; - Vector2d Z_mea(Z[0],Z[1]); + 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; + //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; + 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); + //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); + 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; + //S + Matrix2d S; + S=H*P_pred*H.transpose()+R; - //kalmam gain + //kalmam gain - MatrixXd K; - K=P_pred*H.transpose()*S.inverse(); + MatrixXd K; + K=P_pred*H.transpose()*S.inverse(); - //X(k+1|k+1) - VectorXd X; - X=X_pred+K*(Z_mea-Z_pred); + //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(k+1|k+1) + MatrixXd P; + MatrixXd I; + I.setIdentity(6, 6); - P=(I-K*H)*P_pred; + 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++) + 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<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); + 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; + //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; + //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; + 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_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(); + 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); + //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); + 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; + 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; + //S + Matrix3d S; + S=H*P_pred*H.transpose()+R; - //bind_speed - double v_bind=Bind_speed(prt, freq_ind); + //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; + //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; + 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; + //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; + //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; + 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; + //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; + 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_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(); + 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); + //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; + 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; + //S + Matrix3d S; + S=H*P_pred*H.transpose()+R; - //bind_speed - double v_bind=Bind_speed(prt, freq_ind); + //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; + //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; + return d; }; double kalman::Bind_speed(double prt,double freq_ind) { - double freq=FREQ0+freq_ind*0.02; + double freq=FREQ0+freq_ind*0.02; - return 150000.0/(freq*prt); + return 150000.0/(freq*prt); } diff --git a/data_process_class_dll/kalman.h b/data_process_class_dll/kalman.h index d5aa480..9dcc9e8 100644 --- a/data_process_class_dll/kalman.h +++ b/data_process_class_dll/kalman.h @@ -12,32 +12,32 @@ class kalman { public: - void 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]); + void 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]); - void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4]);//两点初始化卡尔曼滤波器 + void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4]);//两点初始化卡尔曼滤波器 - double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T); //计算点 临时航迹的d + double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T); //计算点 临时航迹的d - double 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); + double 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); - double d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind); + double d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind); - void kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6]); + void 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 d_cal(double Z[2],double X[6],double P[6][6]); + double d_cal(double Z[2],double X[6],double P[6][6]); - double 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); + double 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); - double 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); + double 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); - void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2]); + void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2]); - void kalman_filter_EKF(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[3], - double X_filter[6],double P_filter[6][6],double S_filter[2][2], - double prt,double freq_ind ); + void kalman_filter_EKF(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[3], + double X_filter[6],double P_filter[6][6],double S_filter[2][2], + double prt,double freq_ind ); - double Bind_speed(double prt,double freq_ind); //根据PRT 和 频率 计算不模糊速度 + double Bind_speed(double prt,double freq_ind); //根据PRT 和 频率 计算不模糊速度 }; #endif // KALMAN_H diff --git a/data_process_class_dll/parameters.h b/data_process_class_dll/parameters.h index 1517c7b..94dda83 100644 --- a/data_process_class_dll/parameters.h +++ b/data_process_class_dll/parameters.h @@ -5,27 +5,27 @@ #define PI 3.1415926f //频点(GHz) -#define FREQ0 16.8 -#define FREQ1 16.8 -#define FREQ2 16.8 -#define FREQ3 16.8 -#define FREQ4 16.8 -#define FREQ5 16.8 -#define FREQ6 16.8 -#define FREQ7 16.8 -#define FREQ8 16.8 -#define FREQ9 16.8 -#define FREQ10 16.8 -#define FREQ11 16.8 -#define FREQ12 16.8 -#define FREQ13 16.8 -#define FREQ14 16.8 -#define FREQ15 16.8 -#define FREQ16 16.8 -#define FREQ17 16.8 -#define FREQ18 16.8 -#define FREQ19 16.8 -#define FREQ20 16.8 +#define FREQ0 16.8 +#define FREQ1 16.8 +#define FREQ2 16.8 +#define FREQ3 16.8 +#define FREQ4 16.8 +#define FREQ5 16.8 +#define FREQ6 16.8 +#define FREQ7 16.8 +#define FREQ8 16.8 +#define FREQ9 16.8 +#define FREQ10 16.8 +#define FREQ11 16.8 +#define FREQ12 16.8 +#define FREQ13 16.8 +#define FREQ14 16.8 +#define FREQ15 16.8 +#define FREQ16 16.8 +#define FREQ17 16.8 +#define FREQ18 16.8 +#define FREQ19 16.8 +#define FREQ20 16.8 //波位 @@ -72,35 +72,35 @@ #define DATA_RATE_TAS 0.3 -#define SIGMA_R 10.0 //测量误差 +#define SIGMA_R 10.0 //测量误差 #define SIGMA_A 0.02 #define SIGMA_E 0.2 #define SIGMA_V 2.0 -#define DOT_COH_RANGE 80 //点迹凝聚 -#define DOT_COH_V 2 +#define DOT_COH_RANGE 80 //点迹凝聚 +#define DOT_COH_V 2 #define DOT_COH_AZI 6 -#define MAX_BEAM_NUM 100 //最大TWS波位数 +#define MAX_BEAM_NUM 100 //最大TWS波位数 -#define MAX_TRACK_NUM 500 -#define MAX_TRACK_INDEX 500 //最大航迹批号 +#define MAX_TRACK_NUM 500 +#define MAX_TRACK_INDEX 500 //最大航迹批号 #define ASSOCIATE_THRESHOLD_MIN 1 -#define ASSOCIATE_THRESHOLD_MID 3 //关联波门 +#define ASSOCIATE_THRESHOLD_MID 3 //关联波门 #define ASSOCIATE_THRESHOLD_MAX 10 #define R_MIN 100 #define R_MAX 100000 -//#define V_MAX 500 //最大速度 -//#define V_MIN 1 //速度最小 -#define TRACK_START_THRESHOLD 8 //起航波门 3 越大越容易起批 -#define ALPHA_START 60 //起航夹角 120 越大越容易起批 +//#define V_MAX 500 //最大速度 +//#define V_MIN 1 //速度最小 +#define TRACK_START_THRESHOLD 8 //起航波门 3 越大越容易起批 +#define ALPHA_START 60 //起航夹角 120 越大越容易起批 -#define TRACK_DIE_ROUND 4 // 航迹消亡时间 +#define TRACK_DIE_ROUND 4 // 航迹消亡时间 #define TRACK_DIE_ROUND_TAS 5 #define ASSO_THORD 3 diff --git a/data_process_class_dll/struct.h b/data_process_class_dll/struct.h index cbb973a..8f3491d 100644 --- a/data_process_class_dll/struct.h +++ b/data_process_class_dll/struct.h @@ -9,84 +9,84 @@ using namespace std; // 点迹数据结构 struct PointRecv { - double Range; //径向距离 - double Azimuth; //方位 (弧度) - double Velocity ; //速度 - double Amplitude ; //幅度 - double Height; //高度 - double snr; //信噪比 - double RCS; //目标RCS - int CPI_Time; //CPI时间 - int Point_index; //点迹号 1~50 - int Point_Sum; //点迹总数 - int Use_Flag; //点迹使用标志 1使用 0未使用 - int beam_index; //波位号 - int PRF_index; //PRF号 - int Freq_index; //频点 - int track_mode; //TAS 1 TWS0 - int point_section_asso; //点迹区 + double Range; //径向距离 + double Azimuth; //方位 (弧度) + double Velocity ; //速度 + double Amplitude ; //幅度 + double Height; //高度 + double snr; //信噪比 + double RCS; //目标RCS + int CPI_Time; //CPI时间 + int Point_index; //点迹号 1~50 + int Point_Sum; //点迹总数 + int Use_Flag; //点迹使用标志 1使用 0未使用 + int beam_index; //波位号 + int PRF_index; //PRF号 + int Freq_index; //频点 + int track_mode; //TAS 1 TWS0 + int point_section_asso; //点迹区 - int pitch_num; // 俯仰波位号 - float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 - float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 + int pitch_num; // 俯仰波位号 + float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 + float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 }; // 可靠航迹数据结构 struct Trust_Track { - int Track_Index; //航迹号 + int Track_Index; //航迹号 - double Amplitude; //幅度 - double Height; //高度 + double Amplitude; //幅度 + double Height; //高度 - int T_track; //航迹时间 + int T_track; //航迹时间 - int Track_Sum; //航迹总数 - int Point_Index; //该航迹上的第几个点 + int Track_Sum; //航迹总数 + int Point_Index; //该航迹上的第几个点 - int Track_Mode; //跟踪模式 - int Track_Update_Flag; //航迹更新标志 1已更新/0未更新 + int Track_Mode; //跟踪模式 + int Track_Update_Flag; //航迹更新标志 1已更新/0未更新 - int Extrapolate_round; //航迹持续外推时间 - int point_flag; //是否实点 1实点 0外推 - int approach_flag; //接近标志 1接近 0远离 + int Extrapolate_round; //航迹持续外推时间 + int point_flag; //是否实点 1实点 0外推 + int approach_flag; //接近标志 1接近 0远离 - int manual_delete_flag; //手动航迹删除 - int manual_tracking_flag; //手动TAS - int associate_point_number; //关联的点数 + int manual_delete_flag; //手动航迹删除 + int manual_tracking_flag; //手动TAS + int associate_point_number; //关联的点数 - int Target_Type; //目标类型 搜索目标 低空目标 地面目标 + int Target_Type; //目标类型 搜索目标 低空目标 地面目标 - int Track_section_idx; //航迹区号 + int Track_section_idx; //航迹区号 - QVector Hight_smooth; //高度平滑 + QVector Hight_smooth; //高度平滑 - double range_point; //关联上的点信息（距离、方位、俯仰） - double azi_point; - double elev_point; - double vr_point; - double prf_point; - int point_type; //TWS TAS - double snr_point; //信噪比 + double range_point; //关联上的点信息（距离、方位、俯仰） + double azi_point; + double elev_point; + double vr_point; + double prf_point; + int point_type; //TWS TAS + double snr_point; //信噪比 - double RCS; + double RCS; - int pitch_num; // 俯仰波位号 - float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 - float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 + int pitch_num; // 俯仰波位号 + float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 + float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 - double X[6]; //IMM - double P[6][6]; + double X[6]; //IMM + double P[6][6]; - double X1[6]; - double X2[6]; - double X3[6]; - double P1[6][6]; - double P2[6][6]; - double P3[6][6]; - double u[3]; + double X1[6]; + double X2[6]; + double X3[6]; + double P1[6][6]; + double P2[6][6]; + double P3[6][6]; + double u[3]; }; @@ -96,29 +96,29 @@ struct Trust_Track struct Temp_track { - double X[4]; //状态 - double P[4][4]; //协方差 + double X[4]; //状态 + double P[4][4]; //协方差 - double r; //距离 - double azi; //方位 - double vr; //径向速度 - double height; //高度 - double snr; //信噪比 - double RCS; //RCS - double Amp; //幅度 - int T; //时间戳 + double r; //距离 + double azi; //方位 + double vr; //径向速度 + double height; //高度 + double snr; //信噪比 + double RCS; //RCS + double Amp; //幅度 + int T; //时间戳 - double d; //关联上的点的d + double d; //关联上的点的d - int buff_round; //已缓存圈数 + int buff_round; //已缓存圈数 - int Temp_track_section_idx; //临时航迹区号 与点迹区的划分相同 + int Temp_track_section_idx; //临时航迹区号 与点迹区的划分相同 - int asso_flag; //关联标志 已关联：1 未关联：0 + int asso_flag; //关联标志 已关联：1 未关联：0 - int pitch_num; // 俯仰波位号 - float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 - float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 + int pitch_num; // 俯仰波位号 + float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 + float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 }; @@ -129,8 +129,8 @@ struct Temp_track // TAS跟踪目标结构体 struct Tracking_Target { - int Index; //目标批号 - int empty_flag; //是否为空标志位 1非空 0空 + int Index; //目标批号 + int empty_flag; //是否为空标志位 1非空 0空 }; @@ -138,11 +138,11 @@ struct Tracking_Target //引导跟踪目标结构体 struct Direct_Tracking_Target { - int ID; - double Azi; - double Elev; - double Range; - int empty_flag; //是否为空标志位 1非空 0空 + int ID; + double Azi; + double Elev; + double Range; + int empty_flag; //是否为空标志位 1非空 0空 // int track_init_flag; //是否已经建航 1是 0否 }; diff --git a/data_process_class_dll/tas_ctrl.cpp b/data_process_class_dll/tas_ctrl.cpp index b1f9c18..429bcde 100644 --- a/data_process_class_dll/tas_ctrl.cpp +++ b/data_process_class_dll/tas_ctrl.cpp @@ -13,29 +13,29 @@ using namespace std; TAS_Ctrl::TAS_Ctrl() { - memset(tas_target_queue,0,TAS_QUEUE_LENGTH*sizeof(Tracking_Target)); + memset(tas_target_queue,0,TAS_QUEUE_LENGTH*sizeof(Tracking_Target)); - tas_target_num=0; + tas_target_num=0; } void TAS_Ctrl::tas_ctrl_process(QVector *trust_track, - struct TrackingBeam *Tracking_beam, - struct Track Trust_Track_Output[][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter) + struct TrackingBeam *Tracking_beam, + struct Track Trust_Track_Output[][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter) { - tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter); - tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter); - tas_beam_output(trust_track,Tracking_beam); + tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter); + tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter); + tas_beam_output(trust_track,Tracking_beam); - //跟踪队列移位 - struct Tracking_Target tas_target_tmp; - memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target)); - for (int i=TAS_QUEUE_LENGTH-1;i>0;i--) - { - memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target)); - } - memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target)); + //跟踪队列移位 + struct Tracking_Target tas_target_tmp; + memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target)); + for (int i=TAS_QUEUE_LENGTH-1;i>0;i--) + { + memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target)); + } + memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target)); @@ -43,65 +43,65 @@ void TAS_Ctrl::tas_ctrl_process(QVector *trust_track, void TAS_Ctrl::tas_target_add(QVector *trust_track, - struct Track Trust_Track_Output[][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter) + struct Track Trust_Track_Output[][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter) { - for (int i=0;isize();i++) - { - double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2)); - double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2)); - double h=(*trust_track)[i].Height+Work_Parameter.Height; - double azi = atan2((*trust_track)[i].X[3],(*trust_track)[i].X[0]); - if(azi<0) - azi=azi+2*PI; - azi=azi/PI*180; + for (int i=0;isize();i++) + { + double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2)); + double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2)); + double h=(*trust_track)[i].Height+Work_Parameter.Height; + double azi = atan2((*trust_track)[i].X[3],(*trust_track)[i].X[0]); + if(azi<0) + azi=azi+2*PI; + azi=azi/PI*180; - //进入跟踪的条件： 1.手动跟踪的目标 或 满足速度、距离、高度条件满足 关联点数大于2 2.不在禁止跟踪区域 - if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, Work_Parameter)==1 && (*trust_track)[i][k].associate_point_number>=2) - && tas_target_num < MAX_TAS_NUM - && (*trust_track)[i].Track_Mode == 0 - && tas_prohibite_area( r, azi, v, h, Work_Parameter) == 0) - { - for(int j=0;j=2) + && tas_target_num < MAX_TAS_NUM + && (*trust_track)[i].Track_Mode == 0 + && tas_prohibite_area( r, azi, v, h, Work_Parameter) == 0) + { + for(int j=0;j *trust_track, -//int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter) +//int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter) //{ //// if(r>Work_Parameter.R_TAS_min && rWork_Parameter.V_TAS_min @@ -130,52 +130,52 @@ void TAS_Ctrl::tas_target_add(QVector *trust_track, //}; -int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter) +int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter) { - for (int i=0;iWork_Parameter.R_min_TAS_prohibited[i] && aziWork_Parameter.Azimuth_min_TAS_prohibited[i]) - { - return 1; - } - } + for (int i=0;iWork_Parameter.R_min_TAS_prohibited[i] && aziWork_Parameter.Azimuth_min_TAS_prohibited[i]) + { + return 1; + } + } - return 0; + return 0; } void TAS_Ctrl::tas_target_del(QVector *trust_track, - struct Track Trust_Track_Output[][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter) + struct Track Trust_Track_Output[][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter) { - //查找TAS目标是否已经消批 若消批则移出TAS队列 - for (int i=0;isize();j++) - { - if(tas_target_queue[i].Index==(*trust_track)[j].Track_Index && (*trust_track)[j].Track_Mode == 1) - { - flag=1; - } - } - //目标不存在 说明已消批 从队列里删除 - if(flag == 0) - { - memset(&tas_target_queue[i],0,sizeof(Tracking_Target)); - tas_target_num=tas_target_num-1; - } - } - } + //查找TAS目标是否已经消批 若消批则移出TAS队列 + for (int i=0;isize();j++) + { + if(tas_target_queue[i].Index==(*trust_track)[j].Track_Index && (*trust_track)[j].Track_Mode == 1) + { + flag=1; + } + } + //目标不存在 说明已消批 从队列里删除 + if(flag == 0) + { + memset(&tas_target_queue[i],0,sizeof(Tracking_Target)); + tas_target_num=tas_target_num-1; + } + } + } - //判断TAS目标是否满足自动跟踪条件 若不满足移除TAS队列 通知界面改变目标状态 + //判断TAS目标是否满足自动跟踪条件 若不满足移除TAS队列 通知界面改变目标状态 // for ( int i=0;isize();i++) // { // double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2)); @@ -209,7 +209,7 @@ void TAS_Ctrl::tas_target_del(QVector *trust_track, // Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1; // Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output; // Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180; -// 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].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].Flag_Point=(*trust_track)[i].point_flag; @@ -227,7 +227,7 @@ void TAS_Ctrl::tas_target_del(QVector *trust_track, }; -//int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter) +//int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter) //{ // if((r<=1000 && h1000 && h2000 && h3000 && h4000 && hWork_Parameter.V_TAS_max @@ -249,65 +249,65 @@ void TAS_Ctrl::tas_target_del(QVector *trust_track, void TAS_Ctrl::tas_beam_output(QVector *trust_track, - struct TrackingBeam *Tracking_beam) + struct TrackingBeam *Tracking_beam) { - if(tas_target_queue[0].empty_flag==1) - { + if(tas_target_queue[0].empty_flag==1) + { - double H_track; - double X_now[6]; - for (int i=0;isize();i++ ) - { - if((*trust_track)[i].Track_Index == tas_target_queue[0].Index) - { - H_track = (*trust_track)[i].Height; - for(int ii=0;ii<6;ii++) - X_now[ii]=(*trust_track)[i].X[ii]; - } - } + double H_track; + double X_now[6]; + for (int i=0;isize();i++ ) + { + if((*trust_track)[i].Track_Index == tas_target_queue[0].Index) + { + H_track = (*trust_track)[i].Height; + for(int ii=0;ii<6;ii++) + X_now[ii]=(*trust_track)[i].X[ii]; + } + } - //预测目标位置 计算跟踪波束波位号 俯仰角 + //预测目标位置 计算跟踪波束波位号 俯仰角 - double x_track=X_now[0]+X_now[1]*T_TAS_PRED; - double y_track=X_now[3]+X_now[4]*T_TAS_PRED; - double amzi,range; - coor_trans Coor_trans; - Coor_trans.cart2polar(x_track,y_track,&range,&amzi); + double x_track=X_now[0]+X_now[1]*T_TAS_PRED; + double y_track=X_now[3]+X_now[4]*T_TAS_PRED; + double amzi,range; + coor_trans Coor_trans; + Coor_trans.cart2polar(x_track,y_track,&range,&amzi); - //目标距离 - Tracking_beam->Range=range; - //目标方位 - Tracking_beam->Azi=amzi/PI*180; - //目标俯仰角 - double elev=asin(H_track/range)/PI*180; + //目标距离 + Tracking_beam->Range=range; + //目标方位 + Tracking_beam->Azi=amzi/PI*180; + //目标俯仰角 + double elev=asin(H_track/range)/PI*180; - if(elev<=0) - elev=0; - else if(elev>=40) - elev=40; - else - elev=elev; + if(elev<=0) + elev=0; + else if(elev>=40) + elev=40; + else + elev=elev; - Tracking_beam->Elev=elev; + Tracking_beam->Elev=elev; - //跟踪波束类型 - Tracking_beam->type=1; + //跟踪波束类型 + Tracking_beam->type=1; - //跟踪目标批号 - Tracking_beam->TAS_track_index = tas_target_queue[0].Index; + //跟踪目标批号 + Tracking_beam->TAS_track_index = tas_target_queue[0].Index; - //跟踪波束开关开启 - Tracking_beam->open_flag=1; - } - else - { - //跟踪波束开关关闭 - Tracking_beam->open_flag=0; - } + //跟踪波束开关开启 + Tracking_beam->open_flag=1; + } + else + { + //跟踪波束开关关闭 + Tracking_beam->open_flag=0; + } diff --git a/data_process_class_dll/tas_ctrl.h b/data_process_class_dll/tas_ctrl.h index eb551eb..497b1ae 100644 --- a/data_process_class_dll/tas_ctrl.h +++ b/data_process_class_dll/tas_ctrl.h @@ -12,45 +12,45 @@ class TAS_Ctrl { public: - TAS_Ctrl(); + TAS_Ctrl(); - void tas_ctrl_process( QVector *trust_track, - struct TrackingBeam *Tracking_beam, - struct Track Trust_Track_Output[][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter); + void tas_ctrl_process( QVector *trust_track, + struct TrackingBeam *Tracking_beam, + struct Track Trust_Track_Output[][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter); private: - //添加TAS目标 - void tas_target_add(QVector *trust_track, - struct Track Trust_Track_Output[][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter); + //添加TAS目标 + void tas_target_add(QVector *trust_track, + struct Track Trust_Track_Output[][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter); - //删除TAS目标 - void tas_target_del(QVector *trust_track, - struct Track Trust_Track_Output[][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter); + //删除TAS目标 + void tas_target_del(QVector *trust_track, + struct Track Trust_Track_Output[][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter); - //TAS队列信息输出 - void tas_beam_output(QVector *trust_track, - struct TrackingBeam *Tracking_beam); + //TAS队列信息输出 + void tas_beam_output(QVector *trust_track, + struct TrackingBeam *Tracking_beam); - //TAS 自动开启条件 - int tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter); + //TAS 自动开启条件 + int tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter); - //TAS 自动退出条件 - int tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter); + //TAS 自动退出条件 + int tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter); - // 禁止跟踪区域判断 - int tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter); + // 禁止跟踪区域判断 + int tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter); - struct Tracking_Target tas_target_queue[TAS_QUEUE_LENGTH]; //跟踪目标信息队列 + struct Tracking_Target tas_target_queue[TAS_QUEUE_LENGTH]; //跟踪目标信息队列 - int tas_target_num; //跟踪目标数量 + int tas_target_num; //跟踪目标数量 }; #endif // TAS_CTRL_H diff --git a/data_process_class_dll/track_asso.cpp b/data_process_class_dll/track_asso.cpp index dbe9db6..dcff6dd 100644 --- a/data_process_class_dll/track_asso.cpp +++ b/data_process_class_dll/track_asso.cpp @@ -13,240 +13,240 @@ using namespace std; // //////////////////////////// 航迹更新结果通过Trust_Track_Output[MAX_TRACK_NUM][10]和 Trust_track_num_Output输出 ///////////// int Track_Asso:: track_asso_process(QVector *point_recv, //输入点迹 - QVector *trust_track, //航迹文件 - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 - int *Trust_track_num_Output, //更新航迹数 - struct RadarPara Work_Parameter //雷达参数 - ) + QVector *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; - } + //取出对应点迹区的点 + 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); + //IMM算法 + model_interaction(trust_track); - model_filter(trust_track, Work_Parameter); + model_filter(trust_track, Work_Parameter); - model_output(trust_track); + model_output(trust_track); - //删除关联上的点 - QVector ::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 ::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().swap((*point_recv)); - for (int i=0;i().swap((*point_recv)); + for (int i=0;i().swap(point_process); + //point_process清空 + QVector().swap(point_process); - //输出航迹 - for (int i=0;isize();i++) - { // 输出更新航迹条件： + //输出航迹 + for (int i=0;isize();i++) + { // 输出更新航迹条件： // if((*trust_track)[i].manual_tracking_flag==0) // { - *Trust_track_num_Output= *Trust_track_num_Output+1; + *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; + 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].Height0&&(*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; + 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; + //关联点信息 + 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; + //直接输出点迹高度-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].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; + 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)); + 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; + return 0; } //遍历所有航迹 进行多模型交互 -void Track_Asso:: model_interaction(QVector *trust_track) +void Track_Asso:: model_interaction(QVector *trust_track) { - for (int loop_of_track=0;loop_of_tracksize();loop_of_track++) - { + for (int loop_of_track=0;loop_of_tracksize();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_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]; + 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]; + 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]; - } + 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]; - } + 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]; + 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++) - { + 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]; + 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]; - } + 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]; - } + 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]; - } + 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]; + } // } - } + } } @@ -254,138 +254,138 @@ void Track_Asso:: model_interaction(QVector *trust_track) 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 + 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); + 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; + //模型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; + 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; + //模型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; - } + } + 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)); + 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; + 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; + 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; + //模型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 if(v_track<=100&&v_track>50) + { + q3=2*Work_Parameter.Model3_Q_fast; - } - else - { - q3=10*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)); + 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; + 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; + 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; } @@ -393,21 +393,21 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub // 计算三个 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 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); + 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); + 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); } @@ -417,57 +417,57 @@ void Track_Asso::IMM_d_cal(double v_track, void Track_Asso:: model_filter(QVector *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 associated_info; + //存储关联信息 + struct asso_info + { + double d1; + double d2; + double d3; + double d_min; + int point_index; + int track_index; + }; + QVector associated_info; - //遍历所有点迹航迹 计算点迹和航迹的统计距离 - for (int loop_of_track = 0; loop_of_tracksize();loop_of_track++) - { + //遍历所有点迹航迹 计算点迹和航迹的统计距离 + for (int loop_of_track = 0; loop_of_tracksize();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 + (*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1 - for (int loop_of_point = 0; loop_of_point *trust_track,struct RadarP //计算俯仰门限 bool d_p = false; - double p_track = atan2(h_track, r_track); //航迹俯仰 - double p_point = asin(h_point/point_process[loop_of_point].Range); //点迹俯仰 + double p_track = atan2(h_track, r_track); //航迹俯仰 + double p_point = asin(h_point/point_process[loop_of_point].Range); //点迹俯仰 if(abs(p_track - p_point)*180/PI <= 5){ - d_p = true; + d_p = true; } - //计算距离门限，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; - } + //计算距离门限，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; + 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; + 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(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*d1500&&r_track<1000) && (d1*d1=1000 && (d1*d1500&&r_track<1000) && (d1*d1=1000 && (d1*d1size();i++) - { - //寻找最小d - if(associated_num>0) - { - int min_index=1; - double min_d=associated_info[0].d_min; - for (int ii=0;iisize();i++) + { + //寻找最小d + if(associated_num>0) + { + int min_index=1; + double min_d=associated_info[0].d_min; + for (int ii=0;ii0) - { - 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 ); + 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); + 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 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]; + 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]); + (*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]; - } + //更新 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].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].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].RCS = point_process[point_index-1].RCS; - (*trust_track)[track_index-1].pitch_num = point_process[point_index-1].pitch_num; + (*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)); + 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); + //更新高度 + 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 + } + (*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 *trust_track) +void Track_Asso:: model_output(QVector *trust_track) { - for(int loop_of_track=0; loop_of_tracksize(); loop_of_track++) - { + for(int loop_of_track=0; loop_of_tracksize(); 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 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 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 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]); + 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]; + //本地航迹文件更新 + (*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); + // //航迹区更新 + // 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_1_START && azi/PI*180=TRACK_SECTION_2_START && azi/PI*180=TRACK_SECTION_2_START && azi/PI*180=TRACK_SECTION_3_START && azi/PI*180=TRACK_SECTION_3_START && azi/PI*180=TRACK_SECTION_4_START && azi/PI*180=TRACK_SECTION_4_START && azi/PI*180=TRACK_SECTION_5_START && azi/PI*180=TRACK_SECTION_5_START && azi/PI*180=TRACK_SECTION_6_START && azi/PI*180=TRACK_SECTION_6_START && azi/PI*180=TRACK_SECTION_7_START && azi/PI*180=TRACK_SECTION_7_START && azi/PI*180=TRACK_SECTION_8_START && azi/PI*180=TRACK_SECTION_8_START && azi/PI*180=TRACK_SECTION_9_START || azi/PI*180=TRACK_SECTION_9_START || azi/PI*180 *trust_track) //高度维更新 void Track_Asso:: track_hight_update(int updata_track_index, //更新的航迹号 - int asso_point_index, //点迹号 - QVector *trust_track //航迹 - ) + int asso_point_index, //点迹号 + QVector *trust_track //航迹 + ) { - if(updata_track_index<=trust_track->size() && asso_point_index<=point_process.size()) - { + 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); + (*trust_track)[updata_track_index-1].Hight_smooth.push_back(point_process[asso_point_index-1].Height); - int height_win_length; + 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; - } + 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) - { + } + 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 *point_recv, //点迹文件 - QVector *trust_track, //航迹文件 - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 - int *Trust_track_num_Output, //更新航迹数 - struct RadarPara Work_Parameter //工作参数 - ); + int track_asso_process(QVector *point_recv, //点迹文件 + QVector *trust_track, //航迹文件 + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 + int *Trust_track_num_Output, //更新航迹数 + struct RadarPara Work_Parameter //工作参数 + ); - Track_Asso(); + Track_Asso(); private: - //要处理的点迹 - QVector point_process; + //要处理的点迹 + QVector point_process; - //IMM - void model_interaction(QVector *trust_track); //模型交互 + //IMM + void model_interaction(QVector *trust_track); //模型交互 - void model_filter(QVector *trust_track, //滤波 - struct RadarPara Work_Parameter - ); + void model_filter(QVector *trust_track, //滤波 + struct RadarPara Work_Parameter + ); - void model_output(QVector *trust_track ); //模型输出 + void model_output(QVector *trust_track ); //模型输出 - void 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); + void 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); - void 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); + void 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); - double Pt[3][3]; //模型转移概率 + double Pt[3][3]; //模型转移概率 - //高度维更新 - void track_hight_update(int updata_track_index, //更新的航迹号 - int asso_point_index, //点迹号 - QVector *trust_track //航迹 - ); + //高度维更新 + void track_hight_update(int updata_track_index, //更新的航迹号 + int asso_point_index, //点迹号 + QVector *trust_track //航迹 + ); }; #endif // TRACK_ASSO_H diff --git a/data_process_class_dll/track_asso_direct_tracking.cpp b/data_process_class_dll/track_asso_direct_tracking.cpp index 6570994..9f7dcc0 100644 --- a/data_process_class_dll/track_asso_direct_tracking.cpp +++ b/data_process_class_dll/track_asso_direct_tracking.cpp @@ -10,208 +10,208 @@ using namespace std; int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector *point_recv, //点迹 - Trust_Track *trust_track, //航迹 - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 - int *Trust_track_num_Output, //更新航迹数 - struct RadarPara Work_Parameter //工作参数 - ) + 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]); - } + //取出点迹 + for (int i=0;i< point_recv->size();i++) + { + point_process.push_back((*point_recv)[i]); + } - //IMM算法 - model_interaction(trust_track); + //IMM算法 + model_interaction(trust_track); - model_filter(trust_track, Work_Parameter); + model_filter(trust_track, Work_Parameter); - model_output(trust_track); + model_output(trust_track); - //point_process清空 - QVector().swap(point_process); + //point_process清空 + QVector().swap(point_process); - //输出航迹 + //输出航迹 - *Trust_track_num_Output= *Trust_track_num_Output+1; + *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).X[0]+(*trust_track).X[1]*Work_Parameter.Sys_delay; - y=(*trust_track).X[3]+(*trust_track).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; - Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin((*trust_track).Height/r)/PI*180; - Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track).Track_Index; - Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V = - sqrt((*trust_track).X[1]*(*trust_track).X[1]+(*trust_track).X[4]*(*trust_track).X[4]); - Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = (*trust_track).Amplitude; - Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track).Track_Mode; - Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track).T_track/1000.0; - Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track).point_flag; - Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track).Height+Work_Parameter.Height; + Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1; + double x,y; + x=(*trust_track).X[0]+(*trust_track).X[1]*Work_Parameter.Sys_delay; + y=(*trust_track).X[3]+(*trust_track).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; + Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin((*trust_track).Height/r)/PI*180; + Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track).Track_Index; + Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V = + sqrt((*trust_track).X[1]*(*trust_track).X[1]+(*trust_track).X[4]*(*trust_track).X[4]); + Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = (*trust_track).Amplitude; + Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track).Track_Mode; + Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track).T_track/1000.0; + Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track).point_flag; + Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track).Height+Work_Parameter.Height; - double Direction_Angle; - Direction_Angle=atan((*trust_track).X[4]/(*trust_track).X[1]); - if((*trust_track).X[1]<0){Direction_Angle=Direction_Angle+PI;} - if((*trust_track).X[1]>0&&(*trust_track).X[4]<0){Direction_Angle=Direction_Angle+2*PI;} - Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180; + double Direction_Angle; + Direction_Angle=atan((*trust_track).X[4]/(*trust_track).X[1]); + if((*trust_track).X[1]<0){Direction_Angle=Direction_Angle+PI;} + if((*trust_track).X[1]>0&&(*trust_track).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).range_point; - Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track).azi_point; - Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track).elev_point; - Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track).vr_point; - Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=1; - Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = (*trust_track).prf_point; - Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track).snr_point; - Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track).X[0]; - Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track).X[3]; - Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track).X[1]; - Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track).X[4]; - Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*trust_track).RCS; + //关联点信息 + Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=(*trust_track).range_point; + Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track).azi_point; + Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track).elev_point; + Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track).vr_point; + Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=1; + Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = (*trust_track).prf_point; + Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track).snr_point; + Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track).X[0]; + Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track).X[3]; + Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track).X[1]; + Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track).X[4]; + Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*trust_track).RCS; - return 0; + return 0; } -void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track) //模型交互 +void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track) //模型交互 { - double u_last[3]; - u_last[0]=(*trust_track).u[0]; - u_last[1]=(*trust_track).u[1]; - u_last[2]=(*trust_track).u[2]; + double u_last[3]; + u_last[0]=(*trust_track).u[0]; + u_last[1]=(*trust_track).u[1]; + u_last[2]=(*trust_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]; + 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]; + 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).X1[i]; - X2[i]=(*trust_track).X2[i]; - X3[i]=(*trust_track).X3[i]; - } + 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).X1[i]; + X2[i]=(*trust_track).X2[i]; + X3[i]=(*trust_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]; - } + 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]; + 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++) - { + 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]; + 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]; - } + 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).P1[i][j]+X11[i][j])*u_t[0][0]+((*trust_track).P2[i][j]+X21[i][j])*u_t[1][0]+((*trust_track).P3[i][j]+X31[i][j])*u_t[2][0]; - Po2_last[i][j]=((*trust_track).P1[i][j]+X12[i][j])*u_t[0][1]+((*trust_track).P2[i][j]+X22[i][j])*u_t[1][1]+((*trust_track).P3[i][j]+X32[i][j])*u_t[2][1]; - Po3_last[i][j]=((*trust_track).P1[i][j]+X13[i][j])*u_t[0][2]+((*trust_track).P2[i][j]+X23[i][j])*u_t[1][2]+((*trust_track).P3[i][j]+X33[i][j])*u_t[2][2]; - } + 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).P1[i][j]+X11[i][j])*u_t[0][0]+((*trust_track).P2[i][j]+X21[i][j])*u_t[1][0]+((*trust_track).P3[i][j]+X31[i][j])*u_t[2][0]; + Po2_last[i][j]=((*trust_track).P1[i][j]+X12[i][j])*u_t[0][1]+((*trust_track).P2[i][j]+X22[i][j])*u_t[1][1]+((*trust_track).P3[i][j]+X32[i][j])*u_t[2][1]; + Po3_last[i][j]=((*trust_track).P1[i][j]+X13[i][j])*u_t[0][2]+((*trust_track).P2[i][j]+X23[i][j])*u_t[1][2]+((*trust_track).P3[i][j]+X33[i][j])*u_t[2][2]; + } - for(int i=0;i<6;i++) - { - (*trust_track).X1[i]=Xo1_last[i]; - (*trust_track).X2[i]=Xo2_last[i]; - (*trust_track).X3[i]=Xo3_last[i]; - } - for(int i=0;i<6;i++) - for (int j=0;j<6;j++) - { - (*trust_track).P1[i][j]=Po1_last[i][j]; - (*trust_track).P2[i][j]=Po2_last[i][j]; - (*trust_track).P3[i][j]=Po3_last[i][j]; - } + for(int i=0;i<6;i++) + { + (*trust_track).X1[i]=Xo1_last[i]; + (*trust_track).X2[i]=Xo2_last[i]; + (*trust_track).X3[i]=Xo3_last[i]; + } + for(int i=0;i<6;i++) + for (int j=0;j<6;j++) + { + (*trust_track).P1[i][j]=Po1_last[i][j]; + (*trust_track).P2[i][j]=Po2_last[i][j]; + (*trust_track).P3[i][j]=Po3_last[i][j]; + } } void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, //滤波 - struct RadarPara Work_Parameter) + struct RadarPara Work_Parameter) { - //存储关联信息 - struct asso_info - { - double d1; - double d2; - double d3; - double d_min; - int point_index; - }; - QVector associated_info; + //存储关联信息 + struct asso_info + { + double d1; + double d2; + double d3; + double d_min; + int point_index; + }; + QVector associated_info; - //航迹信息 - double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6]; - double T_track; - double v_track; - double r_track; - double h_track; + //航迹信息 + double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6]; + double T_track; + double v_track; + double r_track; + double h_track; (*trust_track).point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1 memcpy(X1,(*trust_track).X1,6*sizeof(double)); memcpy(X2,(*trust_track).X2,6*sizeof(double)); @@ -227,228 +227,228 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, // 计算量测和航迹统计距离 for (int loop_of_point = 0; loop_of_point=1000) - { - if(abs(h_track-h_point)<=200) - { - 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; - } + } + 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; + } - //小于关联门限 保存关联信息 - if((d1*d10) { - //找最近点 - int min_index=1; - double min_d=associated_info[0].d_min; - for (int ii=0;ii50) - { - q2=10.0*Work_Parameter.Model2_Q_fast; - } - else - { - q2=Work_Parameter.Model2_Q_fast; - } + //模型2 + //Q + double q2; + if(v_track<=5) + { + q2=Work_Parameter.Model2_Q_slow/20; + } + else if(v_track<=100&&v_track>50) + { + q2=10.0*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)); + 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; + 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; + 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<=100&&v_track>50) - { - q3=10.0*Work_Parameter.Model3_Q_fast; - } - else - { - q3=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)); + //模型3 + //Q + double q3; + if(v_track<=5) + { + q3=Work_Parameter.Model3_Q_slow/20; + } + else if(v_track<=100&&v_track>50) + { + q3=10.0*Work_Parameter.Model3_Q_fast; + } + else + { + q3=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; + 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; + 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; } @@ -637,24 +637,24 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub // 计算三个 d void Track_Asso_Direct_Tracking::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 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); + 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; + kalman Kalman; // *d1=Kalman.d_cal_with_doppler(F,Q1,Z,X1,P1,v_point,prt,freq_ind); // *d2=Kalman.d_cal_with_doppler(F,Q2,Z,X2,P2,v_point,prt,freq_ind); // *d3=Kalman.d_cal_with_doppler(F,Q3,Z,X3,P3,v_point,prt,freq_ind); - *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); + *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); } @@ -662,8 +662,8 @@ void Track_Asso_Direct_Tracking::IMM_d_cal(double v_track, //高度维更新 void Track_Asso_Direct_Tracking::track_hight_update(int asso_point_index, //点迹号 - Trust_Track *trust_track //航迹 - ) + Trust_Track *trust_track //航迹 + ) { (*trust_track).Hight_smooth.push_back(point_process[asso_point_index-1].Height); @@ -672,19 +672,19 @@ void Track_Asso_Direct_Tracking::track_hight_update(int asso_point int height_win_length; if((*trust_track).range_point <= 1000) { - height_win_length = H_F_WIN_LEN+2; + height_win_length = H_F_WIN_LEN+2; } else if((*trust_track).range_point <= 2000 && (*trust_track).range_point > 1000) { - height_win_length = H_F_WIN_LEN+3; + height_win_length = H_F_WIN_LEN+3; } else if((*trust_track).range_point <= 4000 && (*trust_track).range_point > 2000) { - height_win_length = H_F_WIN_LEN+4; + height_win_length = H_F_WIN_LEN+4; } else { - height_win_length = H_F_WIN_LEN+6; + height_win_length = H_F_WIN_LEN+6; } @@ -692,33 +692,33 @@ void Track_Asso_Direct_Tracking::track_hight_update(int asso_point if((*trust_track).Hight_smooth.size()=height_win_length) { - int N=(*trust_track).Hight_smooth.size(); - for (int ii=0;ii *point_recv, //点迹 - Trust_Track *trust_track, //航迹 - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 - int *Trust_track_num_Output, //更新航迹数 - struct RadarPara Work_Parameter //工作参数 - ); + public: + int track_asso_process_direct_tracking(QVector *point_recv, //点迹 + Trust_Track *trust_track, //航迹 + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 + int *Trust_track_num_Output, //更新航迹数 + struct RadarPara Work_Parameter //工作参数 + ); - Track_Asso_Direct_Tracking(); + Track_Asso_Direct_Tracking(); - private: + private: - //要处理的点迹 - QVector point_process; + //要处理的点迹 + QVector point_process; - //IMM - void model_interaction(Trust_Track *trust_track); //模型交互 + //IMM + void model_interaction(Trust_Track *trust_track); //模型交互 - void model_filter(Trust_Track *trust_track, //滤波 - struct RadarPara Work_Parameter - ); + void model_filter(Trust_Track *trust_track, //滤波 + struct RadarPara Work_Parameter + ); - void model_output(Trust_Track *trust_track); //模型输出 + void model_output(Trust_Track *trust_track); //模型输出 - void 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); + void 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); - void 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); + void 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); - double Pt[3][3]; //模型转移概率 + double Pt[3][3]; //模型转移概率 - //高度维更新 - void track_hight_update(int asso_point_index, //点迹号 - Trust_Track *trust_track //航迹 - ); + //高度维更新 + void track_hight_update(int asso_point_index, //点迹号 + Trust_Track *trust_track //航迹 + ); }; diff --git a/data_process_class_dll/track_asso_tas.cpp b/data_process_class_dll/track_asso_tas.cpp index 0f10f8e..9589dc8 100644 --- a/data_process_class_dll/track_asso_tas.cpp +++ b/data_process_class_dll/track_asso_tas.cpp @@ -10,93 +10,93 @@ using namespace std; int Track_Asso_Tas::track_asso_process_tas(QVector *point_recv_tas, //点迹文件 - QVector *trust_track, //航迹文件 - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 - int *Trust_track_num_Output, //更新航迹数 - struct RadarPara Work_Parameter, //工作参数 - int tas_track_idx - ) + QVector *trust_track, //航迹文件 + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 + int *Trust_track_num_Output, //更新航迹数 + struct RadarPara Work_Parameter, //工作参数 + int tas_track_idx + ) { - //取出点迹 - for (int i=0;i< point_recv_tas->size();i++) - { - point_process.push_back((*point_recv_tas)[i]); - } + //取出点迹 + for (int i=0;i< point_recv_tas->size();i++) + { + point_process.push_back((*point_recv_tas)[i]); + } - //IMM算法 - model_interaction(trust_track,tas_track_idx); + //IMM算法 + model_interaction(trust_track,tas_track_idx); - model_filter(trust_track, Work_Parameter,tas_track_idx); + model_filter(trust_track, Work_Parameter,tas_track_idx); - model_output(trust_track,tas_track_idx); + model_output(trust_track,tas_track_idx); - //point_process清空 - QVector().swap(point_process); + //point_process清空 + QVector().swap(point_process); - //输出航迹 - for (int i=0;isize();i++) - { // 输出更新航迹条件： - if((*trust_track)[i].Track_Index == tas_track_idx) - { - *Trust_track_num_Output= *Trust_track_num_Output+1; + //输出航迹 + for (int i=0;isize();i++) + { // 输出更新航迹条件： + if((*trust_track)[i].Track_Index == tas_track_idx) + { + *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; - Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin((*trust_track)[i].Height/r)/PI*180; - 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; + 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; + Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin((*trust_track)[i].Height/r)/PI*180; + 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; + 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=1; - 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; - 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; + //关联点信息 + 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=1; + 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; + 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)); - } + 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; + return 0; } //进行多模型交互 -void Track_Asso_Tas:: model_interaction( QVector *trust_track, int tas_track_idx) +void Track_Asso_Tas:: model_interaction( QVector *trust_track, int tas_track_idx) { @@ -104,8 +104,8 @@ void Track_Asso_Tas:: model_interaction( QVector *trust_track, int { - if((*trust_track)[loop_of_track].Track_Index == tas_track_idx) - { + if((*trust_track)[loop_of_track].Track_Index == tas_track_idx) + { // if(point_process.size()>0) @@ -115,413 +115,413 @@ void Track_Asso_Tas:: model_interaction( QVector *trust_track, int - 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_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]; + 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]; + 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]; - } + 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]; - } + 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]; + 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++) - { + 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]; + 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]; - } + 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]; - } + 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]; - } - } - } + 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]; + } + } + } } void Track_Asso_Tas::model_filter( QVector *trust_track, //滤波 - struct RadarPara Work_Parameter, - int tas_track_idx - ) + struct RadarPara Work_Parameter, + int tas_track_idx + ) { - //存储关联信息 - struct asso_info - { - double d1; - double d2; - double d3; - double d_min; - int point_index; - }; - QVector associated_info; + //存储关联信息 + struct asso_info + { + double d1; + double d2; + double d3; + double d_min; + int point_index; + }; + QVector associated_info; - //航迹信息 - double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6]; - double T_track; - double v_track; - double r_track; - double h_track; - for (int loop_of_track=0;loop_of_tracksize();loop_of_track++) - { - if((*trust_track)[loop_of_track].Track_Index == tas_track_idx) - { + //航迹信息 + double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6]; + double T_track; + double v_track; + double r_track; + double h_track; + for (int loop_of_track=0;loop_of_tracksize();loop_of_track++) + { + if((*trust_track)[loop_of_track].Track_Index == tas_track_idx) + { // if(point_process.size()>0) // { // qDebug() << "model_filter point_process :" <=1000) - { - if(abs(h_track-h_point)<=200) - { - 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; - } + } + 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() << "T d" <0) - { + //有关联 滤波 + if (associated_info.size()>0) + { // qDebug() << "TAS asoooooo222" ; - //找最近点 - int min_index=1; - double min_d=associated_info[0].d_min; - for (int ii=0;iisize();i++) + //更新航迹 + for(int i=0;isize();i++) - if((*trust_track)[i].Track_Index == tas_track_idx) - { - //更新模型概率 - double u_last[3]; - u_last[0]=(*trust_track)[i].u[0]; - u_last[1]=(*trust_track)[i].u[1]; - u_last[2]=(*trust_track)[i].u[2]; + if((*trust_track)[i].Track_Index == tas_track_idx) + { + //更新模型概率 + double u_last[3]; + u_last[0]=(*trust_track)[i].u[0]; + u_last[1]=(*trust_track)[i].u[1]; + u_last[2]=(*trust_track)[i].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]; + 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)[i].u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); - (*trust_track)[i].u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); - (*trust_track)[i].u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); + (*trust_track)[i].u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); + (*trust_track)[i].u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); + (*trust_track)[i].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)[i].X1[ii]=X1_filter[ii]; - (*trust_track)[i].X2[ii]=X2_filter[ii]; - (*trust_track)[i].X3[ii]=X3_filter[ii]; - } - for(int ii=0;ii<6;ii++) - for (int jj=0;jj<6;jj++) - { - (*trust_track)[i].P1[ii][jj]=P1_filter[ii][jj]; - (*trust_track)[i].P2[ii][jj]=P2_filter[ii][jj]; - (*trust_track)[i].P3[ii][jj]=P3_filter[ii][jj]; - } + //更新 X1 X2 X3 P1 P2 P3 + for(int ii=0;ii<6;ii++) + { + (*trust_track)[i].X1[ii]=X1_filter[ii]; + (*trust_track)[i].X2[ii]=X2_filter[ii]; + (*trust_track)[i].X3[ii]=X3_filter[ii]; + } + for(int ii=0;ii<6;ii++) + for (int jj=0;jj<6;jj++) + { + (*trust_track)[i].P1[ii][jj]=P1_filter[ii][jj]; + (*trust_track)[i].P2[ii][jj]=P2_filter[ii][jj]; + (*trust_track)[i].P3[ii][jj]=P3_filter[ii][jj]; + } - //更新航迹时间 - (*trust_track)[i].T_track = point_process[point_index-1].CPI_Time; + //更新航迹时间 + (*trust_track)[i].T_track = point_process[point_index-1].CPI_Time; - //更新关联上的点迹信息 - (*trust_track)[i].range_point=point_process[point_index-1].Range; - (*trust_track)[i].azi_point=point_process[point_index-1].Azimuth/PI*180; - (*trust_track)[i].elev_point= asin(point_process[point_index-1].Height/point_process[point_index-1].Range)/PI*180; - (*trust_track)[i].vr_point=point_process[point_index-1].Velocity; - (*trust_track)[i].point_type = 1; - (*trust_track)[i].prf_point = point_process[point_index-1].PRF_index; - (*trust_track)[i].snr_point = point_process[point_index-1].snr; - (*trust_track)[i].Extrapolate_round = 0; //连续未用实点更新时间 - (*trust_track)[i].point_flag=1; //实点 - (*trust_track)[i].Amplitude= point_process[point_index-1].Amplitude; //幅度 - (*trust_track)[i].associate_point_number = (*trust_track)[i].associate_point_number+1; //关联点数+1 - (*trust_track)[i].RCS = point_process[point_index-1].RCS; + //更新关联上的点迹信息 + (*trust_track)[i].range_point=point_process[point_index-1].Range; + (*trust_track)[i].azi_point=point_process[point_index-1].Azimuth/PI*180; + (*trust_track)[i].elev_point= asin(point_process[point_index-1].Height/point_process[point_index-1].Range)/PI*180; + (*trust_track)[i].vr_point=point_process[point_index-1].Velocity; + (*trust_track)[i].point_type = 1; + (*trust_track)[i].prf_point = point_process[point_index-1].PRF_index; + (*trust_track)[i].snr_point = point_process[point_index-1].snr; + (*trust_track)[i].Extrapolate_round = 0; //连续未用实点更新时间 + (*trust_track)[i].point_flag=1; //实点 + (*trust_track)[i].Amplitude= point_process[point_index-1].Amplitude; //幅度 + (*trust_track)[i].associate_point_number = (*trust_track)[i].associate_point_number+1; //关联点数+1 + (*trust_track)[i].RCS = point_process[point_index-1].RCS; - (*trust_track)[i].pitch_num = point_process[point_index-1].pitch_num; + (*trust_track)[i].pitch_num = point_process[point_index-1].pitch_num; - std::memcpy((*trust_track)[i].speed_dim, point_process[point_index-1].speed_dim, sizeof((*trust_track)[i].speed_dim)); - std::memcpy((*trust_track)[i].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[i].range_dim)); + std::memcpy((*trust_track)[i].speed_dim, point_process[point_index-1].speed_dim, sizeof((*trust_track)[i].speed_dim)); + std::memcpy((*trust_track)[i].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[i].range_dim)); - //更新高度 - track_hight_update(tas_track_idx,point_index,trust_track); - } + //更新高度 + track_hight_update(tas_track_idx,point_index,trust_track); + } - } - //未关联上，航迹外推 - else - { + } + //未关联上，航迹外推 + else + { - for(int i=0;isize();i++) - if((*trust_track)[i].Track_Index == tas_track_idx) - { - double delta_T = DATA_RATE_TAS; - 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); + for(int i=0;isize();i++) + if((*trust_track)[i].Track_Index == tas_track_idx) + { + double delta_T = DATA_RATE_TAS; + 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); - 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)); + 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].point_flag=0; //虚点 - (*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; - } + (*trust_track)[i].T_track = (*trust_track)[i].T_track+delta_T*1000.0; //更新航迹时间 + (*trust_track)[i].point_flag=0; //虚点 + (*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; + } - } + } @@ -531,73 +531,73 @@ void Track_Asso_Tas::model_filter( QVector *trust void Track_Asso_Tas::model_output(QVector *trust_track, int tas_track_idx ) { - for(int i=0; isize(); i++) - if((*trust_track)[i].Track_Index==tas_track_idx) - { + for(int i=0; isize(); i++) + if((*trust_track)[i].Track_Index==tas_track_idx) + { - double u_now[3]; - u_now[0]=(*trust_track)[i].u[0]; - u_now[1]=(*trust_track)[i].u[1]; - u_now[2]=(*trust_track)[i].u[2]; + double u_now[3]; + u_now[0]=(*trust_track)[i].u[0]; + u_now[1]=(*trust_track)[i].u[1]; + u_now[2]=(*trust_track)[i].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)[i].X1[ii]; - X2_filter[ii]=(*trust_track)[i].X2[ii]; - X3_filter[ii]=(*trust_track)[i].X3[ii]; - } - for (int ii=0;ii<6;ii++) - for(int jj=0;jj<6;jj++) - { - P1_filter[ii][jj]=(*trust_track)[i].P1[ii][jj]; - P2_filter[ii][jj]=(*trust_track)[i].P2[ii][jj]; - P3_filter[ii][jj]=(*trust_track)[i].P3[ii][jj]; - } + 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)[i].X1[ii]; + X2_filter[ii]=(*trust_track)[i].X2[ii]; + X3_filter[ii]=(*trust_track)[i].X3[ii]; + } + for (int ii=0;ii<6;ii++) + for(int jj=0;jj<6;jj++) + { + P1_filter[ii][jj]=(*trust_track)[i].P1[ii][jj]; + P2_filter[ii][jj]=(*trust_track)[i].P2[ii][jj]; + P3_filter[ii][jj]=(*trust_track)[i].P3[ii][jj]; + } - double X_filter[6], P_filter[6][6]; - for (int ii=0;ii<6;ii++) - X_filter[ii]=u_now[0]*X1_filter[ii]+u_now[1]*X2_filter[ii]+u_now[2]*X3_filter[ii]; + double X_filter[6], P_filter[6][6]; + for (int ii=0;ii<6;ii++) + X_filter[ii]=u_now[0]*X1_filter[ii]+u_now[1]*X2_filter[ii]+u_now[2]*X3_filter[ii]; - 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 ii=0;ii<6;ii++) - { - X1_sub_X[ii]=X1_filter[ii]-X_filter[ii]; - X2_sub_X[ii]=X2_filter[ii]-X_filter[ii]; - X3_sub_X[ii]=X3_filter[ii]-X_filter[ii]; - } - for(int ii=0;ii<6;ii++) - for (int jj=0;jj<6;jj++) - { - X1X[ii][jj]=X1_sub_X[ii]*X1_sub_X[jj]; - X2X[ii][jj]=X2_sub_X[ii]*X2_sub_X[jj]; - X3X[ii][jj]=X3_sub_X[ii]*X3_sub_X[jj]; - } - for(int ii=0;ii<6;ii++) - for (int jj=0;jj<6;jj++) - P_filter[ii][jj]=u_now[0]*(P1_filter[ii][jj]+X1X[ii][jj])+u_now[1]*(P2_filter[ii][jj]+X2X[ii][jj])+u_now[2]*(P3_filter[ii][jj]+X3X[ii][jj]); + 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 ii=0;ii<6;ii++) + { + X1_sub_X[ii]=X1_filter[ii]-X_filter[ii]; + X2_sub_X[ii]=X2_filter[ii]-X_filter[ii]; + X3_sub_X[ii]=X3_filter[ii]-X_filter[ii]; + } + for(int ii=0;ii<6;ii++) + for (int jj=0;jj<6;jj++) + { + X1X[ii][jj]=X1_sub_X[ii]*X1_sub_X[jj]; + X2X[ii][jj]=X2_sub_X[ii]*X2_sub_X[jj]; + X3X[ii][jj]=X3_sub_X[ii]*X3_sub_X[jj]; + } + for(int ii=0;ii<6;ii++) + for (int jj=0;jj<6;jj++) + P_filter[ii][jj]=u_now[0]*(P1_filter[ii][jj]+X1X[ii][jj])+u_now[1]*(P2_filter[ii][jj]+X2X[ii][jj])+u_now[2]*(P3_filter[ii][jj]+X3X[ii][jj]); - //本地航迹文件更新 - (*trust_track)[i].X[0]=X_filter[0]; //位置 速度 - (*trust_track)[i].X[1]=X_filter[1]; - (*trust_track)[i].X[2]=X_filter[2]; - (*trust_track)[i].X[3]=X_filter[3]; - (*trust_track)[i].X[4]=X_filter[4]; - (*trust_track)[i].X[5]=X_filter[5]; + //本地航迹文件更新 + (*trust_track)[i].X[0]=X_filter[0]; //位置 速度 + (*trust_track)[i].X[1]=X_filter[1]; + (*trust_track)[i].X[2]=X_filter[2]; + (*trust_track)[i].X[3]=X_filter[3]; + (*trust_track)[i].X[4]=X_filter[4]; + (*trust_track)[i].X[5]=X_filter[5]; - for(int ii=0;ii<6;ii++) //协方差矩阵 - for (int jj=0;jj<6;jj++) - (*trust_track)[i].P[ii][jj]=P_filter[ii][jj]; + for(int ii=0;ii<6;ii++) //协方差矩阵 + for (int jj=0;jj<6;jj++) + (*trust_track)[i].P[ii][jj]=P_filter[ii][jj]; - //航迹区更新 - double r, azi; - coor_trans Coor_trans; - Coor_trans.cart2polar((*trust_track)[i].X[0], (*trust_track)[i].X[3], &r, &azi); + //航迹区更新 + double r, azi; + coor_trans Coor_trans; + Coor_trans.cart2polar((*trust_track)[i].X[0], (*trust_track)[i].X[3], &r, &azi); - } + } } @@ -609,114 +609,114 @@ void Track_Asso_Tas::model_output(QVector *trust_track, int tas_ void Track_Asso_Tas::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 + 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); + 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; + //模型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; + 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<=100&&v_track>50) - { - q2=10.0*Work_Parameter.Model2_Q_fast; - } - else - { - q2=Work_Parameter.Model2_Q_fast; - } + //模型2 + //Q + double q2; + if(v_track<=5) + { + q2=Work_Parameter.Model2_Q_slow/20; + } + else if(v_track<=100&&v_track>50) + { + q2=10.0*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)); + 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; + 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; + 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<=100&&v_track>50) - { - q3=10.0*Work_Parameter.Model3_Q_fast; - } - else - { - q3=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)); + //模型3 + //Q + double q3; + if(v_track<=5) + { + q3=Work_Parameter.Model3_Q_slow/20; + } + else if(v_track<=100&&v_track>50) + { + q3=10.0*Work_Parameter.Model3_Q_fast; + } + else + { + q3=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; + 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; + 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; } @@ -724,24 +724,24 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], // 计算三个 d void Track_Asso_Tas::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 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); + 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; + kalman Kalman; // *d1=Kalman.d_cal_with_doppler(F,Q1,Z,X1,P1,v_point,prt,freq_ind); // *d2=Kalman.d_cal_with_doppler(F,Q2,Z,X2,P2,v_point,prt,freq_ind); // *d3=Kalman.d_cal_with_doppler(F,Q3,Z,X3,P3,v_point,prt,freq_ind); - *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); + *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); } @@ -749,71 +749,71 @@ void Track_Asso_Tas::IMM_d_cal(double v_track, //高度维更新 void Track_Asso_Tas::track_hight_update(int tas_track_idx, //更新的航迹号 - int asso_point_index, //点迹号 - QVector *trust_track //航迹 - ) + int asso_point_index, //点迹号 + QVector *trust_track //航迹 + ) { - for(int i=0; isize(); i++) - if((*trust_track)[i].Track_Index==tas_track_idx) - { - (*trust_track)[i].Hight_smooth.push_back(point_process[asso_point_index-1].Height); + for(int i=0; isize(); i++) + if((*trust_track)[i].Track_Index==tas_track_idx) + { + (*trust_track)[i].Hight_smooth.push_back(point_process[asso_point_index-1].Height); - int height_win_length; - if((*trust_track)[i].range_point <= 1000) - { - height_win_length = H_F_WIN_LEN+2; - } - else if((*trust_track)[i].range_point <= 2000 && (*trust_track)[i].range_point > 1000) - { - height_win_length = H_F_WIN_LEN+3; - } - else if((*trust_track)[i].range_point <= 4000 && (*trust_track)[i].range_point > 2000) - { - height_win_length = H_F_WIN_LEN+4; - } - else - { - height_win_length = H_F_WIN_LEN+6; - } + int height_win_length; + if((*trust_track)[i].range_point <= 1000) + { + height_win_length = H_F_WIN_LEN+2; + } + else if((*trust_track)[i].range_point <= 2000 && (*trust_track)[i].range_point > 1000) + { + height_win_length = H_F_WIN_LEN+3; + } + else if((*trust_track)[i].range_point <= 4000 && (*trust_track)[i].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)[i].Hight_smooth.size()=height_win_length) - { + } + else if((*trust_track)[i].Hight_smooth.size()>=height_win_length) + { - int N=(*trust_track)[i].Hight_smooth.size(); - for (int ii=0;ii *point_recv_tas, //点迹文件 - QVector *trust_track, //航迹文件 - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 - int *Trust_track_num_Output, //更新航迹数 - struct RadarPara Work_Parameter, //工作参数 - int tas_track_idx - ); + int track_asso_process_tas(QVector *point_recv_tas, //点迹文件 + QVector *trust_track, //航迹文件 + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 + int *Trust_track_num_Output, //更新航迹数 + struct RadarPara Work_Parameter, //工作参数 + int tas_track_idx + ); - Track_Asso_Tas(); + Track_Asso_Tas(); private: - //要处理的点迹 - QVector point_process; + //要处理的点迹 + QVector point_process; - //IMM - void model_interaction(QVector *trust_track, int tas_track_idx); //模型交互 + //IMM + void model_interaction(QVector *trust_track, int tas_track_idx); //模型交互 - void model_filter(QVector *trust_track, //滤波 - struct RadarPara Work_Parameter, - int tas_track_idx - ); + void model_filter(QVector *trust_track, //滤波 + struct RadarPara Work_Parameter, + int tas_track_idx + ); - void model_output(QVector *trust_track, int tas_track_idx ); //模型输出 + void model_output(QVector *trust_track, int tas_track_idx ); //模型输出 - void 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); + void 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); - void 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); + void 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); - double Pt[3][3]; //模型转移概率 + double Pt[3][3]; //模型转移概率 - //高度维更新 - void track_hight_update(int tas_track_index, //更新的航迹号 - int asso_point_index, //点迹号 - QVector *trust_track //航迹 - ); + //高度维更新 + void track_hight_update(int tas_track_index, //更新的航迹号 + int asso_point_index, //点迹号 + QVector *trust_track //航迹 + ); diff --git a/data_process_class_dll/track_die.cpp b/data_process_class_dll/track_die.cpp index b62c631..6f26c72 100644 --- a/data_process_class_dll/track_die.cpp +++ b/data_process_class_dll/track_die.cpp @@ -8,30 +8,30 @@ using namespace std; void Track_Die::track_die_process( QVector *trust_track, - int Track_die_Index_Output[], - int *Track_die_num_Output) + int Track_die_Index_Output[], + int *Track_die_num_Output) { - QVector ::iterator Iter; - for (Iter=trust_track->begin(); Iter!=trust_track->end();) - { - if( ((*Iter).Track_Mode == 0 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND) - // ||((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND_TAS) - ||(*Iter).manual_delete_flag == 1) + QVector ::iterator Iter; + for (Iter=trust_track->begin(); Iter!=trust_track->end();) + { + if( ((*Iter).Track_Mode == 0 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND) + // ||((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND_TAS) + ||(*Iter).manual_delete_flag == 1) - { - //输出消亡信息 - *Track_die_num_Output=*Track_die_num_Output+1; - Track_die_Index_Output[*Track_die_num_Output-1]=(*Iter).Track_Index; + { + //输出消亡信息 + *Track_die_num_Output=*Track_die_num_Output+1; + Track_die_Index_Output[*Track_die_num_Output-1]=(*Iter).Track_Index; - trust_track->erase(Iter); - Iter=trust_track->begin(); - } - else - { - Iter++; - } - } + trust_track->erase(Iter); + Iter=trust_track->begin(); + } + else + { + Iter++; + } + } }; diff --git a/data_process_class_dll/track_die.h b/data_process_class_dll/track_die.h index 86df228..da9fac3 100644 --- a/data_process_class_dll/track_die.h +++ b/data_process_class_dll/track_die.h @@ -11,9 +11,9 @@ using namespace std; class Track_Die { public: - void track_die_process( QVector *trust_track, - int Track_die_Index_Output[], - int *Track_die_num_Output); + void track_die_process( QVector *trust_track, + int Track_die_Index_Output[], + int *Track_die_num_Output); diff --git a/data_process_class_dll/track_die_tas.cpp b/data_process_class_dll/track_die_tas.cpp index 3cd6227..06aa438 100644 --- a/data_process_class_dll/track_die_tas.cpp +++ b/data_process_class_dll/track_die_tas.cpp @@ -8,29 +8,29 @@ using namespace std; void Track_Die_Tas::track_die_process_tas( QVector *trust_track, - int Track_die_Index_Output[], - int *Track_die_num_Output) - { + int Track_die_Index_Output[], + int *Track_die_num_Output) + { - QVector ::iterator Iter; - for (Iter=trust_track->begin(); Iter!=trust_track->end();) - { - if( ((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND_TAS) - ||(*Iter).manual_delete_flag == 1) + QVector ::iterator Iter; + for (Iter=trust_track->begin(); Iter!=trust_track->end();) + { + if( ((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND_TAS) + ||(*Iter).manual_delete_flag == 1) - { - //输出消亡信息 - *Track_die_num_Output=*Track_die_num_Output+1; - Track_die_Index_Output[*Track_die_num_Output-1]=(*Iter).Track_Index; + { + //输出消亡信息 + *Track_die_num_Output=*Track_die_num_Output+1; + Track_die_Index_Output[*Track_die_num_Output-1]=(*Iter).Track_Index; - trust_track->erase(Iter); - Iter=trust_track->begin(); - } - else - { - Iter++; - } - } + trust_track->erase(Iter); + Iter=trust_track->begin(); + } + else + { + Iter++; + } + } }; diff --git a/data_process_class_dll/track_die_tas.h b/data_process_class_dll/track_die_tas.h index f399a56..83b4926 100644 --- a/data_process_class_dll/track_die_tas.h +++ b/data_process_class_dll/track_die_tas.h @@ -11,9 +11,9 @@ using namespace std; class Track_Die_Tas { public: - void track_die_process_tas( QVector *trust_track, - int Track_die_Index_Output[], - int *Track_die_num_Output); + void track_die_process_tas( QVector *trust_track, + int Track_die_Index_Output[], + int *Track_die_num_Output); diff --git a/data_process_class_dll/track_index_mangement.cpp b/data_process_class_dll/track_index_mangement.cpp index 080bd3e..97f5f60 100644 --- a/data_process_class_dll/track_index_mangement.cpp +++ b/data_process_class_dll/track_index_mangement.cpp @@ -7,69 +7,69 @@ using namespace std; -int Track_Ind_Mangement ::track_ind_get( QVector *trust_track) +int Track_Ind_Mangement ::track_ind_get( QVector *trust_track) { - int isempty = 1; - if((*trust_track).size()>0) - { + int isempty = 1; + if((*trust_track).size()>0) + { - isempty = 0; - } + isempty = 0; + } - if (isempty == 1) - { - lastest_index = 1; - return lastest_index; - } - else - { - //建立航迹号列表 - int List[MAX_TRACK_INDEX]={0}; - for (int i=0;isize();i++) - { - List[(*trust_track)[i].Track_Index-1]=1; - } + if (isempty == 1) + { + lastest_index = 1; + return lastest_index; + } + else + { + //建立航迹号列表 + int List[MAX_TRACK_INDEX]={0}; + for (int i=0;isize();i++) + { + List[(*trust_track)[i].Track_Index-1]=1; + } - //分配航迹号 - if(lastest_index *trust_track); + int track_ind_get( QVector *trust_track); private: - int lastest_index; + int lastest_index; }; #endif // TRACK_INDEX_MANGEMENT_H diff --git a/data_process_class_dll/track_init.cpp b/data_process_class_dll/track_init.cpp index ffaea99..d9b0a84 100644 --- a/data_process_class_dll/track_init.cpp +++ b/data_process_class_dll/track_init.cpp @@ -9,13 +9,13 @@ using namespace std; -int Track_Init::track_init_process_logic( QVector *point_recv, //输入点迹 - QVector *trust_track, //可靠航迹 - QVector > *temp_track, - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter - ) +int Track_Init::track_init_process_logic( QVector *point_recv, //输入点迹 + QVector *trust_track, //可靠航迹 + QVector > *temp_track, + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter + ) { @@ -36,74 +36,74 @@ int Track_Init::track_init_process_logic( QVector *poi - //取出待起航的点迹数据 - for (int i=0;i<(*point_recv).size();i++) - point_process.push_back((*point_recv)[i]); + //取出待起航的点迹数据 + for (int i=0;i<(*point_recv).size();i++) + point_process.push_back((*point_recv)[i]); - //临时航迹的buff_round+1 - for (int i=0;isize();i++) - { - for (int j=0;j<(*temp_track)[i].size();j++) - { - (*temp_track)[i][j].buff_round = (*temp_track)[i][j].buff_round+1; - } - } + //临时航迹的buff_round+1 + for (int i=0;isize();i++) + { + for (int j=0;j<(*temp_track)[i].size();j++) + { + (*temp_track)[i][j].buff_round = (*temp_track)[i][j].buff_round+1; + } + } - if(point_process.size()>0) - { - //点迹与临时航迹关联 - point_temp_track_asso(temp_track,Work_Parameter); + if(point_process.size()>0) + { + //点迹与临时航迹关联 + point_temp_track_asso(temp_track,Work_Parameter); - //点迹与航迹头关联 - point_track_head_asso(temp_track,Work_Parameter); + //点迹与航迹头关联 + point_track_head_asso(temp_track,Work_Parameter); - //删除关联上的点迹 - QVector ::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 ::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().swap((*point_recv)); - for (int i=0;i().swap((*point_recv)); + for (int i=0;ipush_back( QVector ()); - int n=temp_track->size(); - (*temp_track)[n-1].push_back(temp_track_tmp); + temp_track->push_back( QVector ()); + int n=temp_track->size(); + (*temp_track)[n-1].push_back(temp_track_tmp); // if(point_process[i].CPI_Time == 30675948 || point_process[i].CPI_Time == 30682320) @@ -116,23 +116,23 @@ int Track_Init::track_init_process_logic( QVector *poi // << "point_process_time=" << point_process[i].CPI_Time; // } - } + } - //清空点迹 - QVector().swap(point_process); - QVector().swap((*point_recv)); + //清空点迹 + QVector().swap(point_process); + QVector().swap((*point_recv)); - //临时航迹满足起始长度 转为可靠航迹 - tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter); + //临时航迹满足起始长度 转为可靠航迹 + tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter); - } + } - //消亡临时航迹 - tmp_track_die(temp_track); + //消亡临时航迹 + tmp_track_die(temp_track); // qDebug() << "-----------------------------------------------"; @@ -149,74 +149,74 @@ int Track_Init::track_init_process_logic( QVector *poi // } - return 0; + return 0; } void Track_Init::point_temp_track_asso(QVector > *temp_track, - struct RadarPara Work_Parameter) + struct RadarPara Work_Parameter) { - //关联信息 - struct Asso_info - { - int track_idx; - int point_idx; - double d; - }; - QVector asso_info; + //关联信息 + struct Asso_info + { + int track_idx; + int point_idx; + double d; + }; + QVector asso_info; - for ( int i=0;i 1 && (*temp_track)[j][L-1].buff_round >= 2) - { + if( (*temp_track)[j].size() > 1 && (*temp_track)[j][L-1].buff_round >= 2) + { - //点迹信息 + //点迹信息 // double Z[2]={ point_process[i].Range*cos(point_process[i].Azimuth),point_process[i].Range*sin(point_process[i].Azimuth)} ; - double Z[3]={point_process[i].Range, - point_process[i].Azimuth, - point_process[i].Velocity}; - double vr_point=point_process[i].Velocity; - double r_point=point_process[i].Range; - double prt = point_process[i].PRF_index; - double freq_ind = point_process[i].Freq_index; - double h_point = point_process[i].Height; - double T_point = point_process[i].CPI_Time; + double Z[3]={point_process[i].Range, + point_process[i].Azimuth, + point_process[i].Velocity}; + double vr_point=point_process[i].Velocity; + double r_point=point_process[i].Range; + double prt = point_process[i].PRF_index; + double freq_ind = point_process[i].Freq_index; + double h_point = point_process[i].Height; + double T_point = point_process[i].CPI_Time; - //航迹信息 - double X[4]; - double P[4][4]; - memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(double)); - memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(double)); - double v_temp_track=(*temp_track)[j][L-1].vr; - double r_temp_track=(*temp_track)[j][L-1].r; - double h_temp_track=(*temp_track)[j][L-1].height; - double T_track_head = (*temp_track)[j][L-1].T; + //航迹信息 + double X[4]; + double P[4][4]; + memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(double)); + memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(double)); + double v_temp_track=(*temp_track)[j][L-1].vr; + double r_temp_track=(*temp_track)[j][L-1].r; + double h_temp_track=(*temp_track)[j][L-1].height; + double T_track_head = (*temp_track)[j][L-1].T; - double delta_T = (T_point - T_track_head)/1000.0; + double delta_T = (T_point - T_track_head)/1000.0; - if( fabs(r_point-r_temp_track)>=5 ) - { - //计算d - kalman Kalman; - double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind); - //double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind); - //double d = Kalman.d_cal_track_init(Z,X,P,delta_T); - //计算夹角 - double x0=(*temp_track)[j][L-2].X[0]; - double y0=(*temp_track)[j][L-2].X[2]; - double x1=(*temp_track)[j][L-1].X[0]; - double y1=(*temp_track)[j][L-1].X[2]; - double x2=Z[0]*cos(Z[1]); - double y2=Z[0]*sin(Z[1]); - double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2); + if( fabs(r_point-r_temp_track)>=5 ) + { + //计算d + kalman Kalman; + double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind); + //double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind); + //double d = Kalman.d_cal_track_init(Z,X,P,delta_T); + //计算夹角 + double x0=(*temp_track)[j][L-2].X[0]; + double y0=(*temp_track)[j][L-2].X[2]; + double x1=(*temp_track)[j][L-1].X[0]; + double y1=(*temp_track)[j][L-1].X[2]; + double x2=Z[0]*cos(Z[1]); + double y2=Z[0]*sin(Z[1]); + double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2); @@ -237,88 +237,88 @@ void Track_Init::point_temp_track_asso(QVector > *temp_t - if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8 - { - struct Asso_info asso_info_tmp; - asso_info_tmp.point_idx = i+1; - asso_info_tmp.track_idx = j+1; - asso_info_tmp.d = d; - asso_info.push_back(asso_info_tmp); - (*temp_track)[j][L-1].asso_flag = 1; - point_process[i].Use_Flag = 1; - } - } - } - } - } + if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8 + { + struct Asso_info asso_info_tmp; + asso_info_tmp.point_idx = i+1; + asso_info_tmp.track_idx = j+1; + asso_info_tmp.d = d; + asso_info.push_back(asso_info_tmp); + (*temp_track)[j][L-1].asso_flag = 1; + point_process[i].Use_Flag = 1; + } + } + } + } + } - // temp_track中加入新关联上的临时航迹 - for (int i = 0 ; i ()); + // temp_track中加入新关联上的临时航迹 + for (int i = 0 ; i ()); - //前L个点 - int L = (*temp_track)[asso_info[i].track_idx-1].size(); - for (int j=0;j < L;j++ ) - { - (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][j]); - (*temp_track)[(*temp_track).size()-1][j].asso_flag = 0; - } + //前L个点 + int L = (*temp_track)[asso_info[i].track_idx-1].size(); + for (int j=0;j < L;j++ ) + { + (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][j]); + (*temp_track)[(*temp_track).size()-1][j].asso_flag = 0; + } - //关联上的点 - Temp_track asso_track_info_tmp; - asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; - asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; - asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; - asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].Amplitude; - asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_Time; - asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].Velocity; - asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].snr; - asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; - asso_track_info_tmp.asso_flag=0; - asso_track_info_tmp.buff_round = 1; - asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num; - std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); - std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim)); - asso_track_info_tmp.d = asso_info[i].d; + //关联上的点 + Temp_track asso_track_info_tmp; + asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; + asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; + asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; + asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].Amplitude; + asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_Time; + asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].Velocity; + asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].snr; + asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; + asso_track_info_tmp.asso_flag=0; + asso_track_info_tmp.buff_round = 1; + asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num; + std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); + std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim)); + asso_track_info_tmp.d = asso_info[i].d; // asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1; - double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][L-1].r*cos((*temp_track)[asso_info[i].track_idx-1][L-1].azi), - (*temp_track)[asso_info[i].track_idx-1][L-1].r*sin((*temp_track)[asso_info[i].track_idx-1][L-1].azi)}; - double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth), - point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)}; - double X[4]; - double P[4][4]; - kalman Kalman; - double T = ( point_process[asso_info[i].point_idx-1].CPI_Time - (*temp_track)[asso_info[i].track_idx-1][L-1].T)/1000.0; + double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][L-1].r*cos((*temp_track)[asso_info[i].track_idx-1][L-1].azi), + (*temp_track)[asso_info[i].track_idx-1][L-1].r*sin((*temp_track)[asso_info[i].track_idx-1][L-1].azi)}; + double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth), + point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)}; + double X[4]; + double P[4][4]; + kalman Kalman; + double T = ( point_process[asso_info[i].point_idx-1].CPI_Time - (*temp_track)[asso_info[i].track_idx-1][L-1].T)/1000.0; - Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P); - memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); - memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double)); - (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp); - } + Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P); + memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); + memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double)); + (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp); + } - //temp_track中删除关联上的临时航迹 - QVector >::iterator Iter; - for (Iter=temp_track->begin(); Iter!=temp_track->end();) - { - if((*Iter).size()>=2) - { - if((*Iter)[(*Iter).size()-1].asso_flag==1) - { - temp_track->erase(Iter); - Iter=temp_track->begin(); - } - else - { - Iter++; - } - } - else - { - Iter++; - } - } + //temp_track中删除关联上的临时航迹 + QVector >::iterator Iter; + for (Iter=temp_track->begin(); Iter!=temp_track->end();) + { + if((*Iter).size()>=2) + { + if((*Iter)[(*Iter).size()-1].asso_flag==1) + { + temp_track->erase(Iter); + Iter=temp_track->begin(); + } + else + { + Iter++; + } + } + else + { + Iter++; + } + } } @@ -326,45 +326,45 @@ void Track_Init::point_temp_track_asso(QVector > *temp_t void Track_Init::point_track_head_asso( QVector > *temp_track, - struct RadarPara Work_Parameter) + struct RadarPara Work_Parameter) { - //关联上的信息 - struct Asso_info - { - int track_idx; - int point_idx; - }; - QVector asso_info; + //关联上的信息 + struct Asso_info + { + int track_idx; + int point_idx; + }; + QVector asso_info; - //关联 - for ( int i=0;isize();j++) - { + for ( int j=0;jsize();j++) + { - if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2) - { + if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2) + { - //点迹信息 - double x_point, y_point, vr_point; - coor_trans Coor_trans; - Coor_trans.polar2cart(&x_point,&y_point,point_process[i].Range,point_process[i].Azimuth); - vr_point=point_process[i].Velocity; - double r_point=point_process[i].Range; - double h_point = point_process[i].Height; - double T_point = point_process[i].CPI_Time; + //点迹信息 + double x_point, y_point, vr_point; + coor_trans Coor_trans; + Coor_trans.polar2cart(&x_point,&y_point,point_process[i].Range,point_process[i].Azimuth); + vr_point=point_process[i].Velocity; + double r_point=point_process[i].Range; + double h_point = point_process[i].Height; + double T_point = point_process[i].CPI_Time; - //航迹信息 - double x_track_head, y_track_head; - x_track_head=(*temp_track)[j][0].X[0]; - y_track_head=(*temp_track)[j][0].X[2]; - double v_track_head=(*temp_track)[j][0].vr; - double h_track_head = (*temp_track)[j][0].height; - double T_track_head = (*temp_track)[j][0].T; + //航迹信息 + double x_track_head, y_track_head; + x_track_head=(*temp_track)[j][0].X[0]; + y_track_head=(*temp_track)[j][0].X[2]; + double v_track_head=(*temp_track)[j][0].vr; + double h_track_head = (*temp_track)[j][0].height; + double T_track_head = (*temp_track)[j][0].T; // if(T_track_head == 30675948) // { @@ -390,32 +390,32 @@ void Track_Init::point_track_head_asso( QVector > *temp_ // } - //距离差 - double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); + //距离差 + double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); - double vmax; - if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点 - { - vmax = Work_Parameter.V_MAX; - } - else //中远程模式 最大速度正常用 - { - vmax = Work_Parameter.V_MAX; - } + double vmax; + if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点 + { + vmax = Work_Parameter.V_MAX; + } + else //中远程模式 最大速度正常用 + { + vmax = Work_Parameter.V_MAX; + } - double delta_T = (T_point - T_track_head)/1000.0; + double delta_T = (T_point - T_track_head)/1000.0; - //满足关联条件的点航 -// if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=V_MIN*delta_T/2.0) ) + //满足关联条件的点航 +// if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=V_MIN*delta_T/2.0) ) // && vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E - if( - ( (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )) - || (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )) - ) - && vr_point*v_track_head>0 ) - { + if( + ( (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )) + || (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )) + ) + && vr_point*v_track_head>0 ) + { // if(T_track_head == 30675948) // { @@ -428,79 +428,79 @@ void Track_Init::point_track_head_asso( QVector > *temp_ // << "T_track_head=" << T_track_head; // } - struct Asso_info asso_info_tmp; - asso_info_tmp.point_idx = i+1; - asso_info_tmp.track_idx = j+1; - asso_info.push_back(asso_info_tmp); - (*temp_track)[j][0].asso_flag = 1; - point_process[i].Use_Flag = 1; - } + struct Asso_info asso_info_tmp; + asso_info_tmp.point_idx = i+1; + asso_info_tmp.track_idx = j+1; + asso_info.push_back(asso_info_tmp); + (*temp_track)[j][0].asso_flag = 1; + point_process[i].Use_Flag = 1; + } - } - } - } + } + } + } - // temp_track中加入新关联上的临时航迹 - for (int i = 0 ; i ()); + // temp_track中加入新关联上的临时航迹 + for (int i = 0 ; i ()); - //第一个点 - (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]); - (*temp_track)[(*temp_track).size()-1][0].asso_flag = 0; + //第一个点 + (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]); + (*temp_track)[(*temp_track).size()-1][0].asso_flag = 0; - //第二个点 - struct Temp_track asso_track_info_tmp; - asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; - asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; - asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; - asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].Amplitude; - asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].Velocity; - asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].snr; - asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; - asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_Time; - asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num; - std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); - std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim)); - asso_track_info_tmp.asso_flag = 0; - asso_track_info_tmp.buff_round = 1; + //第二个点 + struct Temp_track asso_track_info_tmp; + asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; + asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; + asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; + asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].Amplitude; + asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].Velocity; + asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].snr; + asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; + asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_Time; + asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num; + std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); + std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim)); + asso_track_info_tmp.asso_flag = 0; + asso_track_info_tmp.buff_round = 1; // asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;; - double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][0].r*cos((*temp_track)[asso_info[i].track_idx-1][0].azi), - (*temp_track)[asso_info[i].track_idx-1][0].r*sin((*temp_track)[asso_info[i].track_idx-1][0].azi)}; - double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth), - point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)}; - double X[4]; - double P[4][4]; - kalman Kalman; - double T = (point_process[asso_info[i].point_idx-1].CPI_Time - (*temp_track)[asso_info[i].track_idx-1][0].T)/1000.0; + double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][0].r*cos((*temp_track)[asso_info[i].track_idx-1][0].azi), + (*temp_track)[asso_info[i].track_idx-1][0].r*sin((*temp_track)[asso_info[i].track_idx-1][0].azi)}; + double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth), + point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)}; + double X[4]; + double P[4][4]; + kalman Kalman; + double T = (point_process[asso_info[i].point_idx-1].CPI_Time - (*temp_track)[asso_info[i].track_idx-1][0].T)/1000.0; - Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P); - memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); - memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double)); - (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp); + Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P); + memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); + memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double)); + (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp); - } + } - //temp_track中删除关联上的航迹头 - QVector >::iterator Iter; - for (Iter=temp_track->begin(); Iter!=temp_track->end();) - { - if((*Iter)[0].asso_flag==1) - { - temp_track->erase(Iter); - Iter=temp_track->begin(); - } - else - { - Iter++; - } + //temp_track中删除关联上的航迹头 + QVector >::iterator Iter; + for (Iter=temp_track->begin(); Iter!=temp_track->end();) + { + if((*Iter)[0].asso_flag==1) + { + temp_track->erase(Iter); + Iter=temp_track->begin(); + } + else + { + Iter++; + } - } + } } @@ -509,11 +509,11 @@ void Track_Init::point_track_head_asso( QVector > *temp_ ////////////////////////////////////////////计算三点间的夹角//////////////////////////////////////////////// double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2) { - double D12[2]={x1-x0,y1-y0}; - double D23[2]={x2-x1,y2-y1}; - double alpha=acos((D12[0]*D23[0]+D12[1]*D23[1])/(sqrt(D12[0]*D12[0]+D12[1]*D12[1])*sqrt(D23[0]*D23[0]+D23[1]*D23[1])))/PI*180; + double D12[2]={x1-x0,y1-y0}; + double D23[2]={x2-x1,y2-y1}; + double alpha=acos((D12[0]*D23[0]+D12[1]*D23[1])/(sqrt(D12[0]*D12[0]+D12[1]*D12[1])*sqrt(D23[0]*D23[0]+D23[1]*D23[1])))/PI*180; - return alpha; + return alpha; } @@ -522,87 +522,87 @@ double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1, void Track_Init::tmp_track_to_trust_track(QVector *trust_track, - QVector > *temp_track, - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter) + QVector > *temp_track, + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter) { - QVector > Track_to_start; + QVector > Track_to_start; - // 1. 将temp_track中满足条件的航迹取出, 放到Track_to_start中 - QVector >::iterator Iter; - for (Iter=temp_track->begin(); Iter!=temp_track->end();) - { - int L=(*Iter).size(); - double range=(*Iter)[L-1].r; - double azi=(*Iter)[L-1].azi/PI*180; - if( ( L==Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0) //按长度查找TRUST_TRACK_POINT - { - //加入Track_to_start中 - Track_to_start.push_back(QVector ()); - for (int i = 0; i>::iterator Iter; + for (Iter=temp_track->begin(); Iter!=temp_track->end();) + { + int L=(*Iter).size(); + double range=(*Iter)[L-1].r; + double azi=(*Iter)[L-1].azi/PI*180; + if( ( L==Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0) //按长度查找TRUST_TRACK_POINT + { + //加入Track_to_start中 + Track_to_start.push_back(QVector ()); + for (int i = 0; ierase(Iter); - Iter=temp_track->begin(); - } - else - { - Iter++; - } - } + } + temp_track->erase(Iter); + Iter=temp_track->begin(); + } + else + { + Iter++; + } + } - //2.两两比较Track_to_start中的航迹信息，删除重复的航迹 - int L = Work_Parameter.track_start_point_num; - for (int i=0; i>::iterator Iter1; - for (Iter1=Track_to_start.begin(); Iter1!=Track_to_start.end();) - { - if( (*Iter1)[0].asso_flag == 2 ) - { - Track_to_start.erase(Iter1); - Iter1=Track_to_start.begin(); - } - else - { - Iter1++; - } - } + QVector >::iterator Iter1; + for (Iter1=Track_to_start.begin(); Iter1!=Track_to_start.end();) + { + if( (*Iter1)[0].asso_flag == 2 ) + { + Track_to_start.erase(Iter1); + Iter1=Track_to_start.begin(); + } + else + { + Iter1++; + } + } @@ -610,116 +610,116 @@ void Track_Init::tmp_track_to_trust_track(QVector *trust_track, - //3.Track_to_start中剩余的航迹起始为可靠航迹 - for (int i=0; isize()0) - { + //3.Track_to_start中剩余的航迹起始为可靠航迹 + for (int i=0; isize()0) + { - //加入本地航迹文件 - double Z0[2]={(Track_to_start[i][L-3].r)*cos(Track_to_start[i][L-3].azi),(Track_to_start[i][L-3].r)*sin(Track_to_start[i][L-3].azi) }; - double Z1[2]={(Track_to_start[i][L-2].r)*cos(Track_to_start[i][L-2].azi),(Track_to_start[i][L-2].r)*sin(Track_to_start[i][L-2].azi) }; - double Z2[2]={(Track_to_start[i][L-1].r)*cos(Track_to_start[i][L-1].azi),(Track_to_start[i][L-1].r)*sin(Track_to_start[i][L-1].azi) }; - double X[6]; - double P[6][6]; - double T1=(Track_to_start[i][L-2].T-Track_to_start[i][L-3].T)/1000.0; - double T2=(Track_to_start[i][L-1].T-Track_to_start[i][L-2].T)/1000.0; - kalman Kalman; - Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P); + //加入本地航迹文件 + double Z0[2]={(Track_to_start[i][L-3].r)*cos(Track_to_start[i][L-3].azi),(Track_to_start[i][L-3].r)*sin(Track_to_start[i][L-3].azi) }; + double Z1[2]={(Track_to_start[i][L-2].r)*cos(Track_to_start[i][L-2].azi),(Track_to_start[i][L-2].r)*sin(Track_to_start[i][L-2].azi) }; + double Z2[2]={(Track_to_start[i][L-1].r)*cos(Track_to_start[i][L-1].azi),(Track_to_start[i][L-1].r)*sin(Track_to_start[i][L-1].azi) }; + double X[6]; + double P[6][6]; + double T1=(Track_to_start[i][L-2].T-Track_to_start[i][L-3].T)/1000.0; + double T2=(Track_to_start[i][L-1].T-Track_to_start[i][L-2].T)/1000.0; + kalman Kalman; + Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P); - Trust_Track trust_track_tmp; - memcpy(trust_track_tmp.X, X, 6*sizeof(double)); - memcpy(trust_track_tmp.P, P, 6*6*sizeof(double)); + Trust_Track trust_track_tmp; + memcpy(trust_track_tmp.X, X, 6*sizeof(double)); + memcpy(trust_track_tmp.P, P, 6*6*sizeof(double)); - trust_track_tmp.Amplitude=Track_to_start[i][L-1].Amp; - trust_track_tmp.snr_point = Track_to_start[i][L-1].snr; - trust_track_tmp.RCS = Track_to_start[i][L-1].RCS; - trust_track_tmp.pitch_num = Track_to_start[i][L-1].pitch_num; - std::memcpy(trust_track_tmp.speed_dim, Track_to_start[i][L-1].speed_dim, sizeof(trust_track_tmp.speed_dim)); - std::memcpy(trust_track_tmp.range_dim, Track_to_start[i][L-1].range_dim, sizeof(trust_track_tmp.range_dim)); - trust_track_tmp.T_track = Track_to_start[i][L-1].T; - trust_track_tmp.point_flag=1; //实点 - trust_track_tmp.Track_Mode=0; //跟踪模式 TWS 0 - trust_track_tmp.Target_Type=UNCONF_TARGET; - trust_track_tmp.Track_Index=index; - trust_track_tmp.Height=Track_to_start[i][L-1].height; - trust_track_tmp.Extrapolate_round=0; - trust_track_tmp.approach_flag = -1; - trust_track_tmp.manual_delete_flag = 0; - trust_track_tmp.associate_point_number = 0; - trust_track_tmp.manual_tracking_flag = 0; - trust_track_tmp.point_type = 0; - trust_track_tmp.u[0]=0.3333; - trust_track_tmp.u[1]=0.3333; - trust_track_tmp.u[2]=0.3333; - memcpy(trust_track_tmp.X1, X, 6*sizeof(double)); - memcpy(trust_track_tmp.P1, P, 6*6*sizeof(double)); - memcpy(trust_track_tmp.X2, X, 6*sizeof(double)); - memcpy(trust_track_tmp.P2, P, 6*6*sizeof(double)); - memcpy(trust_track_tmp.X3, X, 6*sizeof(double)); - memcpy(trust_track_tmp.P3, P, 6*6*sizeof(double)); + trust_track_tmp.Amplitude=Track_to_start[i][L-1].Amp; + trust_track_tmp.snr_point = Track_to_start[i][L-1].snr; + trust_track_tmp.RCS = Track_to_start[i][L-1].RCS; + trust_track_tmp.pitch_num = Track_to_start[i][L-1].pitch_num; + std::memcpy(trust_track_tmp.speed_dim, Track_to_start[i][L-1].speed_dim, sizeof(trust_track_tmp.speed_dim)); + std::memcpy(trust_track_tmp.range_dim, Track_to_start[i][L-1].range_dim, sizeof(trust_track_tmp.range_dim)); + trust_track_tmp.T_track = Track_to_start[i][L-1].T; + trust_track_tmp.point_flag=1; //实点 + trust_track_tmp.Track_Mode=0; //跟踪模式 TWS 0 + trust_track_tmp.Target_Type=UNCONF_TARGET; + trust_track_tmp.Track_Index=index; + trust_track_tmp.Height=Track_to_start[i][L-1].height; + trust_track_tmp.Extrapolate_round=0; + trust_track_tmp.approach_flag = -1; + trust_track_tmp.manual_delete_flag = 0; + trust_track_tmp.associate_point_number = 0; + trust_track_tmp.manual_tracking_flag = 0; + trust_track_tmp.point_type = 0; + trust_track_tmp.u[0]=0.3333; + trust_track_tmp.u[1]=0.3333; + trust_track_tmp.u[2]=0.3333; + memcpy(trust_track_tmp.X1, X, 6*sizeof(double)); + memcpy(trust_track_tmp.P1, P, 6*6*sizeof(double)); + memcpy(trust_track_tmp.X2, X, 6*sizeof(double)); + memcpy(trust_track_tmp.P2, P, 6*6*sizeof(double)); + memcpy(trust_track_tmp.X3, X, 6*sizeof(double)); + memcpy(trust_track_tmp.P3, P, 6*6*sizeof(double)); - double r, azi; - coor_trans Coor_trans; - Coor_trans.cart2polar(X[0], X[3], &r, &azi); + double r, azi; + coor_trans Coor_trans; + Coor_trans.cart2polar(X[0], X[3], &r, &azi); - (*trust_track).push_back(trust_track_tmp); + (*trust_track).push_back(trust_track_tmp); - //输出航迹更新信息 - *Trust_track_num_Output=*Trust_track_num_Output+1; - for (int j=0;j>().swap(Track_to_start); + QVector >().swap(Track_to_start); } @@ -728,39 +728,39 @@ void Track_Init::tmp_track_to_trust_track(QVector *trust_track, void Track_Init::tmp_track_die(QVector > *temp_track) { - QVector >::iterator Iter; - for (Iter=temp_track->begin(); Iter!=temp_track->end();) - { - int n=(*Iter).size(); - if( (*Iter)[n-1].buff_round >2 || n>=10) - { + QVector >::iterator Iter; + for (Iter=temp_track->begin(); Iter!=temp_track->end();) + { + int n=(*Iter).size(); + if( (*Iter)[n-1].buff_round >2 || n>=10) + { - temp_track->erase(Iter); - Iter=temp_track->begin(); - } - else - { - Iter++; - } - } + temp_track->erase(Iter); + Iter=temp_track->begin(); + } + else + { + Iter++; + } + } }; -int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter) +int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter) { - for (int i=0;iWork_Parameter.R_min_track_prohibited[i] && aziWork_Parameter.Azimuth_min_track_prohibited[i]) - { - return 1; - } - } + for (int i=0;iWork_Parameter.R_min_track_prohibited[i] && aziWork_Parameter.Azimuth_min_track_prohibited[i]) + { + return 1; + } + } - return 0; + return 0; } diff --git a/data_process_class_dll/track_init.h b/data_process_class_dll/track_init.h index f706afd..f129cb4 100644 --- a/data_process_class_dll/track_init.h +++ b/data_process_class_dll/track_init.h @@ -16,43 +16,43 @@ class Track_Init { public: - //航迹起始逻辑法 - int track_init_process_logic( QVector *point_recv, //输入点迹 - QVector *trust_track, //可靠航迹 - QVector > *temp_track, - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter - ); + //航迹起始逻辑法 + int track_init_process_logic( QVector *point_recv, //输入点迹 + QVector *trust_track, //可靠航迹 + QVector > *temp_track, + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter + ); - Track_Init(); + Track_Init(); private: - QVector point_process; //要处理的点迹 + QVector point_process; //要处理的点迹 - void point_temp_track_asso(QVector > *temp_track, - struct RadarPara Work_Parameter); //临时航迹与点迹关联 + void point_temp_track_asso(QVector > *temp_track, + struct RadarPara Work_Parameter); //临时航迹与点迹关联 - void point_track_head_asso( QVector > *temp_track, - struct RadarPara Work_Parameter); //航迹头与点迹关联 + void point_track_head_asso( QVector > *temp_track, + struct RadarPara Work_Parameter); //航迹头与点迹关联 - void tmp_track_to_trust_track( QVector *trust_track, //临时航迹转可靠航迹 - QVector > *temp_track, - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter); + void tmp_track_to_trust_track( QVector *trust_track, //临时航迹转可靠航迹 + QVector > *temp_track, + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter); - void tmp_track_die(QVector > *temp_track);//临时航迹消亡 + void tmp_track_die(QVector > *temp_track);//临时航迹消亡 - double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角 + double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角 - int track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0 + int track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0 - Track_Ind_Mangement track_index_mangement; + Track_Ind_Mangement track_index_mangement; }; #endif // TRACK_INIT_H diff --git a/data_process_class_dll/track_init_direct_tracking.cpp b/data_process_class_dll/track_init_direct_tracking.cpp index dd4c4c9..477545e 100644 --- a/data_process_class_dll/track_init_direct_tracking.cpp +++ b/data_process_class_dll/track_init_direct_tracking.cpp @@ -8,422 +8,422 @@ #include using namespace std; -int Track_Init_Direct_Tracking::track_init_process_logic( QVector *point_recv, //输入点迹 - QVector *trust_track, //可靠航迹 - QVector > *temp_track, - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter, - int track_ID - ) +int Track_Init_Direct_Tracking::track_init_process_logic( QVector *point_recv, //输入点迹 + QVector *trust_track, //可靠航迹 + QVector > *temp_track, + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter, + int track_ID + ) { - return 0; + return 0; } void Track_Init_Direct_Tracking::point_temp_track_asso(QVector > *temp_track, - struct RadarPara Work_Parameter) //临时航迹与点迹关联 + struct RadarPara Work_Parameter) //临时航迹与点迹关联 { - //关联信息 - struct Asso_info - { - int track_idx; - int point_idx; - }; - QVector asso_info; + //关联信息 + struct Asso_info + { + int track_idx; + int point_idx; + }; + QVector asso_info; - for ( int i=0;i 1 && (*temp_track)[j][L-1].buff_round >= 2) - { + if( (*temp_track)[j].size() > 1 && (*temp_track)[j][L-1].buff_round >= 2) + { - //点迹信息 + //点迹信息 // double Z[2]={ point_process[i].Range*cos(point_process[i].Azimuth),point_process[i].Range*sin(point_process[i].Azimuth)} ; - double Z[3]={point_process[i].Range, - point_process[i].Azimuth, - point_process[i].Velocity}; - double vr_point=point_process[i].Velocity; - double r_point=point_process[i].Range; - double prt = point_process[i].PRF_index; - double freq_ind = point_process[i].Freq_index; - double h_point = point_process[i].Height; - double T_point = point_process[i].CPI_Time; + double Z[3]={point_process[i].Range, + point_process[i].Azimuth, + point_process[i].Velocity}; + double vr_point=point_process[i].Velocity; + double r_point=point_process[i].Range; + double prt = point_process[i].PRF_index; + double freq_ind = point_process[i].Freq_index; + double h_point = point_process[i].Height; + double T_point = point_process[i].CPI_Time; - //航迹信息 - double X[4]; - double P[4][4]; - memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(double)); - memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(double)); - double v_temp_track=(*temp_track)[j][L-1].vr; - double r_temp_track=(*temp_track)[j][L-1].r; - double h_temp_track=(*temp_track)[j][L-1].height; - double T_track_head = (*temp_track)[j][L-1].T; + //航迹信息 + double X[4]; + double P[4][4]; + memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(double)); + memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(double)); + double v_temp_track=(*temp_track)[j][L-1].vr; + double r_temp_track=(*temp_track)[j][L-1].r; + double h_temp_track=(*temp_track)[j][L-1].height; + double T_track_head = (*temp_track)[j][L-1].T; - double delta_T = (T_point - T_track_head)/1000.0; + double delta_T = (T_point - T_track_head)/1000.0; - if( fabs(r_point-r_temp_track)>=5 ) - { - //计算d - kalman Kalman; - double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind); - //double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind); - //double d = Kalman.d_cal_track_init(Z,X,P,delta_T); - //计算夹角 - double x0=(*temp_track)[j][L-2].X[0]; - double y0=(*temp_track)[j][L-2].X[2]; - double x1=(*temp_track)[j][L-1].X[0]; - double y1=(*temp_track)[j][L-1].X[2]; - double x2=Z[0]*cos(Z[1]); - double y2=Z[0]*sin(Z[1]); - double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2); + if( fabs(r_point-r_temp_track)>=5 ) + { + //计算d + kalman Kalman; + double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind); + //double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind); + //double d = Kalman.d_cal_track_init(Z,X,P,delta_T); + //计算夹角 + double x0=(*temp_track)[j][L-2].X[0]; + double y0=(*temp_track)[j][L-2].X[2]; + double x1=(*temp_track)[j][L-1].X[0]; + double y1=(*temp_track)[j][L-1].X[2]; + double x2=Z[0]*cos(Z[1]); + double y2=Z[0]*sin(Z[1]); + double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2); - if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8 - { - struct Asso_info asso_info_tmp; - asso_info_tmp.point_idx = i+1; - asso_info_tmp.track_idx = j+1; - asso_info.push_back(asso_info_tmp); - (*temp_track)[j][L-1].asso_flag = 1; - point_process[i].Use_Flag = 1; - } - } - } - } - } + if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8 + { + struct Asso_info asso_info_tmp; + asso_info_tmp.point_idx = i+1; + asso_info_tmp.track_idx = j+1; + asso_info.push_back(asso_info_tmp); + (*temp_track)[j][L-1].asso_flag = 1; + point_process[i].Use_Flag = 1; + } + } + } + } + } - // temp_track中加入新关联上的临时航迹 - for (int i = 0 ; i ()); + // temp_track中加入新关联上的临时航迹 + for (int i = 0 ; i ()); - //前L个点 - int L = (*temp_track)[asso_info[i].track_idx-1].size(); - for (int j=0;j < L;j++ ) - { - (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][j]); - (*temp_track)[(*temp_track).size()-1][j].asso_flag = 0; - } + //前L个点 + int L = (*temp_track)[asso_info[i].track_idx-1].size(); + for (int j=0;j < L;j++ ) + { + (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][j]); + (*temp_track)[(*temp_track).size()-1][j].asso_flag = 0; + } - //关联上的点 - Temp_track asso_track_info_tmp; - asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; - asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; - asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; - asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].Amplitude; - asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_Time; - asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].Velocity; - asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].snr; - asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; - asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num; - std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); - std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim)); - asso_track_info_tmp.asso_flag=0; - asso_track_info_tmp.buff_round = 1; + //关联上的点 + Temp_track asso_track_info_tmp; + asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; + asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; + asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; + asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].Amplitude; + asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_Time; + asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].Velocity; + asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].snr; + asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; + asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num; + std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); + std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim)); + asso_track_info_tmp.asso_flag=0; + asso_track_info_tmp.buff_round = 1; // asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1; - double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][L-1].r*cos((*temp_track)[asso_info[i].track_idx-1][L-1].azi), - (*temp_track)[asso_info[i].track_idx-1][L-1].r*sin((*temp_track)[asso_info[i].track_idx-1][L-1].azi)}; - double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth), - point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)}; - double X[4]; - double P[4][4]; - kalman Kalman; - double T = ( point_process[asso_info[i].point_idx-1].CPI_Time - (*temp_track)[asso_info[i].track_idx-1][L-1].T)/1000.0; + double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][L-1].r*cos((*temp_track)[asso_info[i].track_idx-1][L-1].azi), + (*temp_track)[asso_info[i].track_idx-1][L-1].r*sin((*temp_track)[asso_info[i].track_idx-1][L-1].azi)}; + double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth), + point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)}; + double X[4]; + double P[4][4]; + kalman Kalman; + double T = ( point_process[asso_info[i].point_idx-1].CPI_Time - (*temp_track)[asso_info[i].track_idx-1][L-1].T)/1000.0; - Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P); - memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); - memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double)); - (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp); - } + Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P); + memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); + memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double)); + (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp); + } - //temp_track中删除关联上的临时航迹 - QVector >::iterator Iter; - for (Iter=temp_track->begin(); Iter!=temp_track->end();) - { - if((*Iter).size()>=2) - { - if((*Iter)[(*Iter).size()-1].asso_flag==1) - { - temp_track->erase(Iter); - Iter=temp_track->begin(); - } - else - { - Iter++; - } - } - else - { - Iter++; - } - } + //temp_track中删除关联上的临时航迹 + QVector >::iterator Iter; + for (Iter=temp_track->begin(); Iter!=temp_track->end();) + { + if((*Iter).size()>=2) + { + if((*Iter)[(*Iter).size()-1].asso_flag==1) + { + temp_track->erase(Iter); + Iter=temp_track->begin(); + } + else + { + Iter++; + } + } + else + { + Iter++; + } + } } void Track_Init_Direct_Tracking::point_track_head_asso( QVector > *temp_track, - struct RadarPara Work_Parameter) //航迹头与点迹关联 + struct RadarPara Work_Parameter) //航迹头与点迹关联 { - //关联上的信息 - struct Asso_info - { - int track_idx; - int point_idx; - }; - QVector asso_info; + //关联上的信息 + struct Asso_info + { + int track_idx; + int point_idx; + }; + QVector asso_info; - //关联 - for ( int i=0;isize();j++) - { + for ( int j=0;jsize();j++) + { - if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2) - { + if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2) + { - //点迹信息 - double x_point, y_point, vr_point; - coor_trans Coor_trans; - Coor_trans.polar2cart(&x_point,&y_point,point_process[i].Range,point_process[i].Azimuth); - vr_point=point_process[i].Velocity; - double r_point=point_process[i].Range; - double h_point = point_process[i].Height; - double T_point = point_process[i].CPI_Time; + //点迹信息 + double x_point, y_point, vr_point; + coor_trans Coor_trans; + Coor_trans.polar2cart(&x_point,&y_point,point_process[i].Range,point_process[i].Azimuth); + vr_point=point_process[i].Velocity; + double r_point=point_process[i].Range; + double h_point = point_process[i].Height; + double T_point = point_process[i].CPI_Time; - //航迹信息 - double x_track_head, y_track_head; - x_track_head=(*temp_track)[j][0].X[0]; - y_track_head=(*temp_track)[j][0].X[2]; - double v_track_head=(*temp_track)[j][0].vr; - double h_track_head = (*temp_track)[j][0].height; - double T_track_head = (*temp_track)[j][0].T; + //航迹信息 + double x_track_head, y_track_head; + x_track_head=(*temp_track)[j][0].X[0]; + y_track_head=(*temp_track)[j][0].X[2]; + double v_track_head=(*temp_track)[j][0].vr; + double h_track_head = (*temp_track)[j][0].height; + double T_track_head = (*temp_track)[j][0].T; - //距离差 - double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); + //距离差 + double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); - double vmax; - if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点 - { - //vmax = V_MAX/5; - vmax = Work_Parameter.V_MAX; + double vmax; + if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点 + { + //vmax = V_MAX/5; + vmax = Work_Parameter.V_MAX; - } - else //中远程模式 最大速度正常用 - { - // vmax = V_MAX; - vmax = Work_Parameter.V_MAX; + } + else //中远程模式 最大速度正常用 + { + // vmax = V_MAX; + vmax = Work_Parameter.V_MAX; - } + } - double delta_T = (T_point - T_track_head)/1000.0; + double delta_T = (T_point - T_track_head)/1000.0; - //满足关联条件的点航 - if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ) - && vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E - { - struct Asso_info asso_info_tmp; - asso_info_tmp.point_idx = i+1; - asso_info_tmp.track_idx = j+1; - asso_info.push_back(asso_info_tmp); - (*temp_track)[j][0].asso_flag = 1; - point_process[i].Use_Flag = 1; - } + //满足关联条件的点航 + if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ) + && vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E + { + struct Asso_info asso_info_tmp; + asso_info_tmp.point_idx = i+1; + asso_info_tmp.track_idx = j+1; + asso_info.push_back(asso_info_tmp); + (*temp_track)[j][0].asso_flag = 1; + point_process[i].Use_Flag = 1; + } - } - } - } + } + } + } - // temp_track中加入新关联上的临时航迹 - for (int i = 0 ; i ()); + // temp_track中加入新关联上的临时航迹 + for (int i = 0 ; i ()); - //第一个点 - (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]); - (*temp_track)[(*temp_track).size()-1][0].asso_flag = 0; + //第一个点 + (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]); + (*temp_track)[(*temp_track).size()-1][0].asso_flag = 0; - //第二个点 - struct Temp_track asso_track_info_tmp; - asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; - asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; - asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; - asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].Amplitude; - asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].Velocity; - asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].snr; - asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; - asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_Time; - asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num; - std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); - std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim)); - asso_track_info_tmp.asso_flag = 0; - asso_track_info_tmp.buff_round = 1; + //第二个点 + struct Temp_track asso_track_info_tmp; + asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; + asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; + asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; + asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].Amplitude; + asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].Velocity; + asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].snr; + asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; + asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_Time; + asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num; + std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); + std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim)); + asso_track_info_tmp.asso_flag = 0; + asso_track_info_tmp.buff_round = 1; // asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;; - double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][0].r*cos((*temp_track)[asso_info[i].track_idx-1][0].azi), - (*temp_track)[asso_info[i].track_idx-1][0].r*sin((*temp_track)[asso_info[i].track_idx-1][0].azi)}; - double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth), - point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)}; - double X[4]; - double P[4][4]; - kalman Kalman; - double T = (point_process[asso_info[i].point_idx-1].CPI_Time - (*temp_track)[asso_info[i].track_idx-1][0].T)/1000.0; + double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][0].r*cos((*temp_track)[asso_info[i].track_idx-1][0].azi), + (*temp_track)[asso_info[i].track_idx-1][0].r*sin((*temp_track)[asso_info[i].track_idx-1][0].azi)}; + double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth), + point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)}; + double X[4]; + double P[4][4]; + kalman Kalman; + double T = (point_process[asso_info[i].point_idx-1].CPI_Time - (*temp_track)[asso_info[i].track_idx-1][0].T)/1000.0; - Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P); - memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); - memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double)); - (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp); + Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P); + memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); + memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double)); + (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp); - } + } - //temp_track中删除关联上的航迹头 - QVector >::iterator Iter; - for (Iter=temp_track->begin(); Iter!=temp_track->end();) - { - if((*Iter)[0].asso_flag==1) - { - temp_track->erase(Iter); - Iter=temp_track->begin(); - } - else - { - Iter++; - } + //temp_track中删除关联上的航迹头 + QVector >::iterator Iter; + for (Iter=temp_track->begin(); Iter!=temp_track->end();) + { + if((*Iter)[0].asso_flag==1) + { + temp_track->erase(Iter); + Iter=temp_track->begin(); + } + else + { + Iter++; + } - } + } } -void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector *trust_track, //临时航迹转可靠航迹 - QVector > *temp_track, - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter, - int track_ID) +void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector *trust_track, //临时航迹转可靠航迹 + QVector > *temp_track, + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter, + int track_ID) { - //temp_track中的临时航迹转为可靠航迹 - QVector >::iterator Iter; - for (Iter=temp_track->begin(); Iter!=temp_track->end();) - { - int L=(*Iter).size(); - double range=(*Iter)[L-1].r; - double azi=(*Iter)[L-1].azi/PI*180; - if( L==Work_Parameter.track_start_point_num) //按长度查找TRUST_TRACK_POINT - { + //temp_track中的临时航迹转为可靠航迹 + QVector >::iterator Iter; + for (Iter=temp_track->begin(); Iter!=temp_track->end();) + { + int L=(*Iter).size(); + double range=(*Iter)[L-1].r; + double azi=(*Iter)[L-1].azi/PI*180; + if( L==Work_Parameter.track_start_point_num) //按长度查找TRUST_TRACK_POINT + { - //加入本地航迹文件 - double Z0[2]={((*Iter)[L-3].r)*cos((*Iter)[L-3].azi),((*Iter)[L-3].r)*sin((*Iter)[L-3].azi) }; - double Z1[2]={((*Iter)[L-2].r)*cos((*Iter)[L-2].azi),((*Iter)[L-2].r)*sin((*Iter)[L-2].azi) }; - double Z2[2]={((*Iter)[L-1].r)*cos((*Iter)[L-1].azi),((*Iter)[L-1].r)*sin((*Iter)[L-1].azi) }; - double X[6]; - double P[6][6]; - double T1=((*Iter)[L-2].T-(*Iter)[L-3].T)/1000.0; - double T2=((*Iter)[L-1].T-(*Iter)[L-2].T)/1000.0; - kalman Kalman; - Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P); - Trust_Track trust_track_tmp; - memcpy(trust_track_tmp.X, X, 6*sizeof(double)); - memcpy(trust_track_tmp.P, P, 6*6*sizeof(double)); + //加入本地航迹文件 + double Z0[2]={((*Iter)[L-3].r)*cos((*Iter)[L-3].azi),((*Iter)[L-3].r)*sin((*Iter)[L-3].azi) }; + double Z1[2]={((*Iter)[L-2].r)*cos((*Iter)[L-2].azi),((*Iter)[L-2].r)*sin((*Iter)[L-2].azi) }; + double Z2[2]={((*Iter)[L-1].r)*cos((*Iter)[L-1].azi),((*Iter)[L-1].r)*sin((*Iter)[L-1].azi) }; + double X[6]; + double P[6][6]; + double T1=((*Iter)[L-2].T-(*Iter)[L-3].T)/1000.0; + double T2=((*Iter)[L-1].T-(*Iter)[L-2].T)/1000.0; + kalman Kalman; + Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P); + Trust_Track trust_track_tmp; + memcpy(trust_track_tmp.X, X, 6*sizeof(double)); + memcpy(trust_track_tmp.P, P, 6*6*sizeof(double)); - trust_track_tmp.Track_Index = track_ID; - trust_track_tmp.Amplitude=(*Iter)[L-1].Amp; - trust_track_tmp.snr_point = (*Iter)[L-1].snr; - trust_track_tmp.RCS = (*Iter)[L-1].RCS; - trust_track_tmp.T_track = (*Iter)[L-1].T; - trust_track_tmp.point_flag=1; //实点 - trust_track_tmp.Track_Mode=1; //跟踪模式 TWS 0 - trust_track_tmp.Target_Type=UNCONF_TARGET; - trust_track_tmp.Height=(*Iter)[L-1].height; - trust_track_tmp.Extrapolate_round=0; - trust_track_tmp.approach_flag = -1; - trust_track_tmp.manual_delete_flag = 0; - trust_track_tmp.associate_point_number = 0; - trust_track_tmp.manual_tracking_flag = 0; - trust_track_tmp.point_type = 0; - trust_track_tmp.u[0]=0.3333; - trust_track_tmp.u[1]=0.3333; - trust_track_tmp.u[2]=0.3333; - memcpy(trust_track_tmp.X1, X, 6*sizeof(double)); - memcpy(trust_track_tmp.P1, P, 6*6*sizeof(double)); - memcpy(trust_track_tmp.X2, X, 6*sizeof(double)); - memcpy(trust_track_tmp.P2, P, 6*6*sizeof(double)); - memcpy(trust_track_tmp.X3, X, 6*sizeof(double)); - memcpy(trust_track_tmp.P3, P, 6*6*sizeof(double)); - (*trust_track).push_back(trust_track_tmp); + trust_track_tmp.Track_Index = track_ID; + trust_track_tmp.Amplitude=(*Iter)[L-1].Amp; + trust_track_tmp.snr_point = (*Iter)[L-1].snr; + trust_track_tmp.RCS = (*Iter)[L-1].RCS; + trust_track_tmp.T_track = (*Iter)[L-1].T; + trust_track_tmp.point_flag=1; //实点 + trust_track_tmp.Track_Mode=1; //跟踪模式 TWS 0 + trust_track_tmp.Target_Type=UNCONF_TARGET; + trust_track_tmp.Height=(*Iter)[L-1].height; + trust_track_tmp.Extrapolate_round=0; + trust_track_tmp.approach_flag = -1; + trust_track_tmp.manual_delete_flag = 0; + trust_track_tmp.associate_point_number = 0; + trust_track_tmp.manual_tracking_flag = 0; + trust_track_tmp.point_type = 0; + trust_track_tmp.u[0]=0.3333; + trust_track_tmp.u[1]=0.3333; + trust_track_tmp.u[2]=0.3333; + memcpy(trust_track_tmp.X1, X, 6*sizeof(double)); + memcpy(trust_track_tmp.P1, P, 6*6*sizeof(double)); + memcpy(trust_track_tmp.X2, X, 6*sizeof(double)); + memcpy(trust_track_tmp.P2, P, 6*6*sizeof(double)); + memcpy(trust_track_tmp.X3, X, 6*sizeof(double)); + memcpy(trust_track_tmp.P3, P, 6*6*sizeof(double)); + (*trust_track).push_back(trust_track_tmp); - trust_track_tmp.pitch_num = (*Iter)[L-1].pitch_num; + trust_track_tmp.pitch_num = (*Iter)[L-1].pitch_num; - std::memcpy(trust_track_tmp.speed_dim, (*Iter)[L-1].speed_dim, sizeof(trust_track_tmp.speed_dim)); - std::memcpy(trust_track_tmp.range_dim, (*Iter)[L-1].range_dim, sizeof(trust_track_tmp.range_dim)); + std::memcpy(trust_track_tmp.speed_dim, (*Iter)[L-1].speed_dim, sizeof(trust_track_tmp.speed_dim)); + std::memcpy(trust_track_tmp.range_dim, (*Iter)[L-1].range_dim, sizeof(trust_track_tmp.range_dim)); - //输出航迹更新信息 - *Trust_track_num_Output=*Trust_track_num_Output+1; - for (int j=0;jerase(Iter); - Iter=temp_track->begin(); - } - else - { - Iter++; - } - } + temp_track->erase(Iter); + Iter=temp_track->begin(); + } + else + { + Iter++; + } + } } @@ -432,20 +432,20 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector void Track_Init_Direct_Tracking::tmp_track_die(QVector > *temp_track) { - QVector >::iterator Iter; - for (Iter=temp_track->begin(); Iter!=temp_track->end();) - { - int n=(*Iter).size(); - if( (*Iter)[n-1].buff_round >1 || n>=10) - { - temp_track->erase(Iter); - Iter=temp_track->begin(); - } - else - { - Iter++; - } - } + QVector >::iterator Iter; + for (Iter=temp_track->begin(); Iter!=temp_track->end();) + { + int n=(*Iter).size(); + if( (*Iter)[n-1].buff_round >1 || n>=10) + { + temp_track->erase(Iter); + Iter=temp_track->begin(); + } + else + { + Iter++; + } + } } @@ -453,11 +453,11 @@ void Track_Init_Direct_Tracking::tmp_track_die(QVector > double Track_Init_Direct_Tracking::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2) { - double D12[2]={x1-x0,y1-y0}; - double D23[2]={x2-x1,y2-y1}; - double alpha=acos((D12[0]*D23[0]+D12[1]*D23[1])/(sqrt(D12[0]*D12[0]+D12[1]*D12[1])*sqrt(D23[0]*D23[0]+D23[1]*D23[1])))/PI*180; + double D12[2]={x1-x0,y1-y0}; + double D23[2]={x2-x1,y2-y1}; + double alpha=acos((D12[0]*D23[0]+D12[1]*D23[1])/(sqrt(D12[0]*D12[0]+D12[1]*D12[1])*sqrt(D23[0]*D23[0]+D23[1]*D23[1])))/PI*180; - return alpha; + return alpha; } diff --git a/data_process_class_dll/track_init_direct_tracking.h b/data_process_class_dll/track_init_direct_tracking.h index e623cf0..1132e2d 100644 --- a/data_process_class_dll/track_init_direct_tracking.h +++ b/data_process_class_dll/track_init_direct_tracking.h @@ -9,43 +9,43 @@ class Track_Init_Direct_Tracking { public: - //航迹起始逻辑法 - int track_init_process_logic( QVector *point_recv, //输入点迹 - QVector *trust_track, //可靠航迹 - QVector > *temp_track, - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter, - int track_ID - ); + //航迹起始逻辑法 + int track_init_process_logic( QVector *point_recv, //输入点迹 + QVector *trust_track, //可靠航迹 + QVector > *temp_track, + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter, + int track_ID + ); - Track_Init_Direct_Tracking(); + Track_Init_Direct_Tracking(); private: - QVector point_process; //要处理的点迹 + QVector point_process; //要处理的点迹 - void point_temp_track_asso(QVector > *temp_track, - struct RadarPara Work_Parameter); //临时航迹与点迹关联 + void point_temp_track_asso(QVector > *temp_track, + struct RadarPara Work_Parameter); //临时航迹与点迹关联 - void point_track_head_asso( QVector > *temp_track, - struct RadarPara Work_Parameter); //航迹头与点迹关联 + void point_track_head_asso( QVector > *temp_track, + struct RadarPara Work_Parameter); //航迹头与点迹关联 - void tmp_track_to_trust_track( QVector *trust_track, //临时航迹转可靠航迹 - QVector > *temp_track, - struct Track Trust_Track_Output[MAX_TRACK_NUM][10], - int *Trust_track_num_Output, - struct RadarPara Work_Parameter, - int track_ID - ); + void tmp_track_to_trust_track( QVector *trust_track, //临时航迹转可靠航迹 + QVector > *temp_track, + struct Track Trust_Track_Output[MAX_TRACK_NUM][10], + int *Trust_track_num_Output, + struct RadarPara Work_Parameter, + int track_ID + ); - void tmp_track_die(QVector > *temp_track);//临时航迹消亡 + void tmp_track_die(QVector > *temp_track);//临时航迹消亡 - double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角 + double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角 };