waiwaylee/radar_data_process
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新：1、使用VS code+cmake重新编译，编译器保持不变；

Browse Source 
2、修复若干逻辑bug，具体参考BUG_FIX_REPORT.md

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-08-18 11:26:19 +08:00
parent 60ad5c136f
commit 01d28e0d28
49 changed files with 1452 additions and 957 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/coor_trans.cpp
+3 -3
View File
@@ -1,7 +1,7 @@
#include "coor_trans.h"
#include <qmath.h>
#include"memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
using namespace std;
//极坐标转直角坐标
data_process_class_dll/coor_trans.h
-1
View File
@@ -10,6 +10,5 @@ public:
void cart2polar(double x, double y, double *Range, double *Azimuth);
};
#endif // COOR_TRANS_H
data_process_class_dll/data_process.cpp
+34 -39
View File
@@ -1,7 +1,7 @@
#include "data_process.h"
#include "memory.h"
#include <QVector>
#include <qmath.h>
#include <cstring>
#include <vector>
#include <cmath>
#include <iostream>
using namespace std;
@@ -11,7 +11,6 @@ using namespace std;
/*******************************************************************************/
/*******************************************************************************/
//数据预处理
int Data_Process::data_preprocess(struct DataRev Data_Input[150])
{
@@ -22,7 +21,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
if(Data_Input[0].point_type == 0) //tws数据
{
data_num=min(Data_Input[0].Point_Sum, 150);
//qDebug() << "TARGET azi beam idx:" <<Data_Input[0].Beam_index_aiz;
//std::cout << "TARGET azi beam idx:" <<Data_Input[0].Beam_index_aiz;
for (int i=0; i<data_num;i++)
{
PointRecv Data_buffer_temp;
@@ -61,10 +60,10 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
else if(Data_Input[0].point_type == 1) //tas数据
{
// qDebug() << "TAS TARGET :" <<Data_Input[0].TAS_track_index;
//qDebug() << "TAS time :" <<Data_Input[0].CPI_time;
// std::cout << "TAS TARGET :" <<Data_Input[0].TAS_track_index;
//std::cout << "TAS time :" <<Data_Input[0].CPI_time;
data_num=Data_Input[0].Point_Sum;
data_num = min(max(Data_Input[0].Point_Sum, 0), 150);
TAS_track_idx = Data_Input[0].TAS_track_index;
latest_timestamp = Data_Input[0].CPI_time;
for (int i=0; i<data_num;i++)
@@ -88,7 +87,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
Data_buffer_tas.push_back(Data_buffer_temp);
// qDebug() << "Azimuth:" << Data_Input[i].Azimuth
// std::cout << "Azimuth:" << Data_Input[i].Azimuth
// << "Range:" << Data_Input[i].Range
// << "EL:" << Data_Input[i].Elevation
// << "CPI_Time"<< Data_Input[i].CPI_time
@@ -106,8 +105,6 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
}
//数据处理
int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
@@ -121,12 +118,10 @@ int model)
{
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,Work_Parameter);
//qDebug() << "point_recv :" <<point_recv.size();
QVector<PointRecv>().swap(Data_buffer);
//std::cout << "point_recv :" <<point_recv.size();
std::vector<PointRecv>().swap(Data_buffer);
}
if(model==1)
{
*Trust_track_num_Output=0; //航迹更新数
@@ -149,11 +144,11 @@ int model)
Trust_Track_Output,
Trust_track_num_Output,
Work_Parameter);
// qDebug() << "point_recv: " <<point_recv.size();
// qDebug() << "trust_track: " <<trust_track.size();
// qDebug() << "temp_track: " <<temp_track.size();
// qDebug() << "Trust_track_num_Output: " <<*Trust_track_num_Output;
// qDebug() << "Track_die_num_Output: " <<*Track_die_num_Output;
// std::cout << "point_recv: " <<point_recv.size();
// std::cout << "trust_track: " <<trust_track.size();
// std::cout << "temp_track: " <<temp_track.size();
// std::cout << "Trust_track_num_Output: " <<*Trust_track_num_Output;
// std::cout << "Track_die_num_Output: " <<*Track_die_num_Output;
}
@@ -167,11 +162,11 @@ int model)
// if(point_recv_tas.size()>0)
// {
// qDebug() << "point_recv_tas :" <<point_recv_tas[0].Azimuth/PI*180<<" "<<point_recv_tas[0].Range;
// std::cout << "point_recv_tas :" <<point_recv_tas[0].Azimuth/PI*180<<" "<<point_recv_tas[0].Range;
// }
//qDebug() << "TAS process :" << TAS_track_idx << "p a r:"<<point_recv_tas[0].Point_Sum<<" " <<point_recv_tas[0].Azimuth/PI*180<<" "<<point_recv_tas[0].Range;
//std::cout << "TAS process :" << TAS_track_idx << "p a r:"<<point_recv_tas[0].Point_Sum<<" " <<point_recv_tas[0].Azimuth/PI*180<<" "<<point_recv_tas[0].Range;
QVector<PointRecv>().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer
std::vector<PointRecv>().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer
track_asso_tas.track_asso_process_tas(&point_recv_tas, //点迹文件
&trust_track, //航迹文件
@@ -182,22 +177,18 @@ int model)
latest_timestamp //最新时间戳
);
QVector<PointRecv>().swap(point_recv_tas); //清除 point_recv_tas
std::vector<PointRecv>().swap(point_recv_tas); //清除 point_recv_tas
track_die_tas.track_die_process_tas( &trust_track,
Track_die_Index_Output,
Track_die_num_Output);
//qDebug() << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
//std::cout << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
}
return 1;
}
//波束控制
void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
@@ -212,6 +203,8 @@ int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parame
{
//工作参数设置
if (Radar_Parameter.track_start_point_num < 3 || Radar_Parameter.track_start_point_num > 9)
return -1;
memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara));
return 0;
@@ -220,11 +213,12 @@ int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parame
//数据处理参数修改
int Data_Process::track_process_parameters_modify(struct RadarPara Radar_Parameter)
{
if (Radar_Parameter.track_start_point_num < 3 || Radar_Parameter.track_start_point_num > 9)
return -1;
memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara));
return 0;
}
//航迹清空函数
int Data_Process::track_clear_all(void)
{
@@ -236,10 +230,17 @@ int Data_Process::track_clear_all(void)
trust_track.clear();
temp_track.clear();
// 同步清理内部缓存和状态，避免重连后输出幽灵航迹/幽灵波束
dot_coh.reset();
track_asso.reset();
track_asso_tas.reset();
track_init.reset();
tas_ctrl.reset();
last_beam_num = INT_MAX;
return 0;
}
//手动航迹删除函数
int Data_Process:: track_delete(int delete_track_num, //手动删除的航迹数目
int delete_track_index[]) //手动删除的航迹号
@@ -252,13 +253,12 @@ int Data_Process:: track_delete(int delete_track_num, //手动
{
trust_track[j].manual_delete_flag=1;
qDebug() << "Delete :" << trust_track[j].Track_Index;
std::cout << "Delete :" << trust_track[j].Track_Index << std::endl;
}
}
}
return 0;
}
@@ -272,7 +272,6 @@ int Data_Process:: tracking_start(int track_index) //需要手动
trust_track[i].manual_tracking_flag = 1;
}
return 0;
}
@@ -292,12 +291,8 @@ int Data_Process:: tracking_stop(int track_index) //需要手动转
}
//手动打跟踪波束
int Data_Process::tracking_point(float Azimuth)//方位角
int Data_Process::tracking_point(float Azimuth)//方位角（当前接口未启用，保留空实现）
{
return 0;
}
data_process_class_dll/data_process.h
+10 -15
View File
@@ -12,9 +12,10 @@
#include "track_die.h"
#include "track_die_tas.h"
#include "tas_ctrl.h"
#include <QVector>
#include <vector>
#include <climits>
#include <Eigen/Dense>
#include <QDebug>
using namespace Eigen;
using namespace std;
@@ -29,7 +30,6 @@ public:
}
//数据预处理函数
int data_preprocess(struct DataRev Data_Input[150]);
@@ -49,7 +49,6 @@ public:
//航迹清空函数
int track_clear_all(void);
//手动航迹删除函数
int track_delete(int delete_track_num, //手动删除的航迹数目
int delete_track_index[]); //手动删除的航迹号
@@ -69,17 +68,15 @@ public:
//参数设置修改函数
int track_process_parameters_modify(struct RadarPara Radar_Parameter);
private:
QVector <PointRecv> Data_buffer; //接收点迹 TWS 输入凝聚
QVector <PointRecv> Data_buffer_tas; //接收点迹 TAS 输入凝聚
QVector <PointRecv> point_recv; //凝聚后输出的点迹 按点迹区存入 输入航迹关联、起始
QVector <PointRecv> point_recv_tas; //凝聚后输出的点迹 TAS
QVector <Trust_Track> trust_track; //可靠航迹 按航迹区存入
QVector <QVector<Temp_track>> temp_track; //临时航迹 按临时航迹区存入
std::vector <PointRecv> Data_buffer; //接收点迹 TWS 输入凝聚
std::vector <PointRecv> Data_buffer_tas; //接收点迹 TAS 输入凝聚
std::vector <PointRecv> point_recv; //凝聚后输出的点迹 按点迹区存入 输入航迹关联、起始
std::vector <PointRecv> point_recv_tas; //凝聚后输出的点迹 TAS
std::vector <Trust_Track> trust_track; //可靠航迹 按航迹区存入
std::vector <std::vector<Temp_track>> temp_track; //临时航迹 按临时航迹区存入
int Beam_num; //波位数
int beam_count; //波位计数
@@ -93,7 +90,7 @@ private:
int last_beam_num = INT_MAX; //上一个波位号，用于判断是否搜索完一圈
qint64 latest_timestamp = 0; //最新时间戳
long long latest_timestamp = 0; //最新时间戳
struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入
@@ -115,6 +112,4 @@ private:
};
#endif // DATA_PROCESS_H
data_process_class_dll/data_process_class_dll.cpp
+2 -1
View File
@@ -1,4 +1,6 @@
#ifndef DATA_PROCESS_CLASS_DLL_LIBRARY
#define DATA_PROCESS_CLASS_DLL_LIBRARY
#endif
#include "data_process_class_dll.h"
#include "data_process.h"
@@ -8,7 +10,6 @@ Data_process_class_dll::Data_process_class_dll()
}
*/
Data_process_class_dll *Data_Process_Factory::p = 0;
Data_process_class_dll *Data_Process_Factory::GetB()
data_process_class_dll/data_process_class_dll.h
+7 -27
View File
@@ -17,12 +17,12 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT DataRev
float Snr; //目标信噪比
float RCS; //目标RCS
float Encoder_value; //码盘值
qint64 CPI_time; //CPI时间 该波位无点迹输入时也需要给出当前时间
qint64 GNSS_time; //GNSS时间
long long CPI_time; //CPI时间 该波位无点迹输入时也需要给出当前时间
long long GNSS_time; //GNSS时间
int Beam_index_azi_0; //上一个cpi波位
int Beam_index_aiz; //方位波位号（0-24） 该波位无输入点迹时 也需要给出当前波位
int Beam_index_elev; //俯仰波位号（0-13）
int PRI; //PRI(us) 给0
int PRI; //PRI(us) 不允许为0
int Freq_index; //频点号 给0
int Point_Sum; //输入点迹总数 若该波位无点迹输入 置为0
int Point_Num; //点迹号(1,2,3,4...)
@@ -34,7 +34,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT DataRev
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
};
//输出航迹的数据结构 用户可见
struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Track
{
@@ -76,13 +75,11 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Track
float track_rcs; //rcs
int pitch_num; // 俯仰波位号
qint64 GNSS_time; //GNSS时间
long long GNSS_time; //GNSS时间
float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
};
// 跟踪波束信息的结构体
struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT TrackingBeam
{
@@ -106,8 +103,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Target_direct_tracking
int open_flag; // 1 引导跟踪 0 结束引导跟踪
};
// 雷达参数结构体
struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
{
@@ -128,14 +123,12 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
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 TAS_prohibite_area_num; //禁止TAS的区域数目
float R_max_TAS_prohibited[30]; //最大距离 （m）
float R_min_TAS_prohibited[30]; //最小距离 (m)
@@ -144,7 +137,7 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
int cfar_th; //cfar门限
int work_mode; //工作模式 0进程 1中程 3远程
int work_mode; //工作模式 0进程 1中程 2远程
int north_angle; //北偏角
float V_MAX; //最大速度
@@ -154,7 +147,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
float DATA_RATE_MIDDLE;
float DATA_RATE_FAR;
//数据关联参数
int track_start_point_num; //起批点数(典型值 3或4)
float track_start_threshold; //起航波门大小(典型值 3)
@@ -173,8 +165,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
};
class DATA_PROCESS_CLASS_DLLSHARED_EXPORT Data_process_class_dll
{
@@ -187,7 +177,6 @@ public:
return 0;
}
//数据处理函数
virtual int track_process(struct Track Trust_Track_Output[][10], //数据处理输出的航迹更新信息 主程序创建全局变量 数据处理函数更新其中数据
int *Trust_track_num_Output, //数据处理后输出的航迹数目 主程序创建全局变量 数据处理函数更新其中数据
@@ -208,7 +197,7 @@ public:
}
//引导跟踪处理函数
virtual int direct_tracking_process(struct Target_direct_tracking Target_direct_info, //引导信息
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, //数据处理后输出的航迹数目 主程序创建全局变量 数据处理函数更新其中数据
@@ -220,7 +209,6 @@ public:
return 0;
}
//航迹清空函数
virtual int track_clear_all(void)
{
@@ -247,31 +235,25 @@ public:
}
//手动打跟踪波束
virtual int tracking_point(float Azimuth) //方位角
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_modify(struct RadarPara Radar_Parameter)
{
return 0;
}
};
//工厂类 用户可见
class DATA_PROCESS_CLASS_DLLSHARED_EXPORT Data_Process_Factory
{
@@ -283,6 +265,4 @@ private:
static Data_process_class_dll *p;
};
#endif // DATA_PROCESS_CLASS_DLL_H
data_process_class_dll/data_process_class_dll.pro
-49
View File
@@ -1,49 +0,0 @@
#-------------------------------------------------
#
# Project created by QtCreator 2021-06-07T22:55:21
#
#-------------------------------------------------
QT -= gui
TARGET = data_process_class_dll
TEMPLATE = lib
DEFINES += DATA_PROCESS_CLASS_DLL_LIBRARY
SOURCES += data_process_class_dll.cpp \
data_process.cpp \
dot_coh.cpp \
kalman.cpp \
coor_trans.cpp \
track_asso.cpp \
track_init.cpp \
track_index_mangement.cpp \
tas_ctrl.cpp \
track_die.cpp \
dot_coh_tas.cpp \
track_asso_tas.cpp \
track_init_direct_tracking.cpp \
track_die_tas.cpp
HEADERS += data_process_class_dll.h\
data_process_class_dll_global.h \
data_process.h \
struct.h \
parameters.h \
dot_coh.h \
kalman.h \
coor_trans.h \
track_asso.h \
track_init.h \
track_index_mangement.h \
tas_ctrl.h \
track_die.h \
dot_coh_tas.h \
track_asso_tas.h \
track_init_direct_tracking.h \
track_die_tas.h
unix {
target.path = /usr/lib
INSTALLS += target
}
data_process_class_dll/data_process_class_dll.pro.user
-263
View File
@@ -1,263 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.0.2, 2026-08-14T14:09:13. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{bda90df5-9664-4f48-ab0c-f665f06226f6}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
<value type="int">0</value>
</data>
<data>
<variable>ProjectExplorer.Project.EditorSettings</variable>
<valuemap type="QVariantMap">
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
</valuemap>
</valuemap>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
<value type="QString" key="language">QmlJS</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
</valuemap>
</valuemap>
<value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
<value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
<value type="int" key="EditorConfiguration.IndentSize">4</value>
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
<value type="int" key="EditorConfiguration.TabSize">8</value>
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.PluginSettings</variable>
<valuemap type="QVariantMap">
<valuelist type="QVariantList" key="ClangStaticAnalyzer.SuppressedDiagnostics"/>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.Target.0</variable>
<valuemap type="QVariantMap">
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop Qt 5.7.0 MSVC2013 32bit</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop Qt 5.7.0 MSVC2013 32bit</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">qt.57.win32_msvc2013_kit</value>
<value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
<value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">E:/Demo/QT/simianzhen256-v1.5.5-ms/build/X256_PS</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
<value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments">&quot;DEFINES += PHASE_SCANNING&quot;</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">构建</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">清理</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">X256_PS</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
<value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
<value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">E:/Demo/QT/simianzhen256-v1.5.5-ms/build/X256_MS</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
<value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments">&quot;DEFINES += MECHANICAL_SCANNING&quot;</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">构建</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">清理</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">X256_MS</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
<value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
<value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
</valuemap>
<value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">2</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">部署</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">在本地部署</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
</valuemap>
<value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
<value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
<value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
<value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
<value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
<value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
<value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
<value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
<value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
<valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
<value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
<value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
<value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
<valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
<value type="int">0</value>
<value type="int">1</value>
<value type="int">2</value>
<value type="int">3</value>
<value type="int">4</value>
<value type="int">5</value>
<value type="int">6</value>
<value type="int">7</value>
<value type="int">8</value>
<value type="int">9</value>
<value type="int">10</value>
<value type="int">11</value>
<value type="int">12</value>
<value type="int">13</value>
<value type="int">14</value>
</valuelist>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Arguments"></value>
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Executable"></value>
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.WorkingDirectory">%{buildDir}</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">自定义执行档</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
<value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
<value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap>
<value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.TargetCount</variable>
<value type="int">1</value>
</data>
<data>
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
<value type="int">18</value>
</data>
<data>
<variable>Version</variable>
<value type="int">18</value>
</data>
</qtcreator>
data_process_class_dll/data_process_class_dll_global.h
+2 -16
View File
@@ -1,26 +1,12 @@
#ifndef DATA_PROCESS_CLASS_DLL_GLOBAL_H
#define DATA_PROCESS_CLASS_DLL_GLOBAL_H
#include <QtCore/qglobal.h>
#if defined(DATA_PROCESS_CLASS_DLL_LIBRARY)
# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT Q_DECL_EXPORT
# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllexport)
#else
# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT Q_DECL_IMPORT
# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllimport)
#endif
//#if defined(DATA_PROCESS_CLASS_DLL_LIBRARY)
//# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllexport)
//#else
//# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllexport)
//#endif
//#if defined(DATA_PROCESS_CLASS_DLL_LIBRARY)
//#define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllexport)
//#else
//#define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllimport)
//#endif
#endif // DATA_PROCESS_CLASS_DLL_GLOBAL_H
data_process_class_dll/dot_coh.cpp
+27 -39
View File
@@ -1,12 +1,11 @@
#include "dot_coh.h"
#include <qmath.h>
#include"memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
using namespace std;
int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
QVector <PointRecv> *point_recv,
int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
std::vector <PointRecv> *point_recv,
struct RadarPara Work_Parameter)
{
@@ -23,7 +22,7 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
float point_0_A=(*data_input)[loop_of_point].Amplitude;
for (unsigned int i=loop_of_point+1;i<data_input->size();i++)
{
if((*data_input)[loop_of_point].Use_Flag!=1)
if((*data_input)[loop_of_point].Use_Flag!=1 && (*data_input)[i].Use_Flag!=1)
{
float point_1_R=(*data_input)[i].Range;
float point_1_V=(*data_input)[i].Velocity;
@@ -33,7 +32,8 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
//凝聚条件： 距离、方位接近
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)//
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 && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
&& point_0_A <point_1_A )//
{
point_0_R=(*data_input)[i].Range;
@@ -42,7 +42,7 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
point_0_F=(*data_input)[i].Azimuth;
(*data_input)[loop_of_point].Use_Flag=1;
}
else 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)//
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_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;
@@ -66,8 +66,6 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
(*data_input)[i].Use_Flag=1;
}
}
}
}
@@ -76,7 +74,7 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
}
//删除凝聚点 和 量程范围外点
QVector <PointRecv>::iterator Iter;
std::vector <PointRecv>::iterator Iter;
for (Iter=data_input->begin(); Iter!=data_input->end();)
{
if((*Iter).Use_Flag==1 || (*Iter).Range <R_MIN || (*Iter).Range > R_MAX )
@@ -98,21 +96,16 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
return 0;
}
int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input, //输入的点迹
QVector <PointRecv> *point_recv, //输出点迹
int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input, //输入的点迹
std::vector <PointRecv> *point_recv, //输出点迹
struct RadarPara Work_Parameter //工作参数
)
{
//1. data_input与data_input_buff进行凝聚
//1.1 data_input、data_input_buff中的数据放在一起
QVector <PointRecv> data_tmp;
std::vector <PointRecv> data_tmp;
for (int i=0;i<data_input_buff.size();i++)
{
data_tmp.push_back(data_input_buff[i]);
@@ -120,7 +113,6 @@ int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input,
}
for (int i=0;i<data_input->size();i++)
{
data_tmp.push_back((*data_input)[i]);
@@ -137,9 +129,8 @@ int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input,
//1.2 把data_input、data_input_buff清空
QVector<PointRecv>().swap(data_input_buff);
QVector<PointRecv>().swap(*data_input);
std::vector<PointRecv>().swap(data_input_buff);
std::vector<PointRecv>().swap(*data_input);
//1.3 对data_tmp进行凝聚
if(data_tmp.size()>1)
@@ -155,7 +146,7 @@ int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input,
float point_0_A=data_tmp[loop_of_point].Amplitude;
for (unsigned int i=loop_of_point+1;i<data_tmp.size();i++)
{
if(data_tmp[loop_of_point].Use_Flag!=1)
if(data_tmp[loop_of_point].Use_Flag!=1 && data_tmp[i].Use_Flag!=1)
{
float point_1_R=data_tmp[i].Range;
float point_1_V=data_tmp[i].Velocity;
@@ -165,7 +156,8 @@ int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input,
//凝聚条件： 距离、方位接近
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)//
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 && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
&& point_0_A <point_1_A )//
{
point_0_R=data_tmp[i].Range;
@@ -174,7 +166,7 @@ int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input,
point_0_F=data_tmp[i].Azimuth;
data_tmp[loop_of_point].Use_Flag=1;
}
else 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)//
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_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;
@@ -206,7 +198,7 @@ int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input,
}
//1.4 删除凝聚点 和 量程范围外点
QVector <PointRecv>::iterator Iter;
std::vector <PointRecv>::iterator Iter;
for (Iter=data_tmp.begin(); Iter!=data_tmp.end();)
{
if((*Iter).Use_Flag==1 || (*Iter).Range <R_MIN || (*Iter).Range > R_MAX )
@@ -233,9 +225,7 @@ int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input,
(*data_input).push_back(data_tmp[i]);
}
}
QVector<PointRecv>().swap(data_tmp);
std::vector<PointRecv>().swap(data_tmp);
//2.data_input_buff数据输出给point_recv
@@ -244,8 +234,7 @@ int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input,
(*point_recv).push_back(data_input_buff[i]);
}
QVector<PointRecv>().swap(data_input_buff);
std::vector<PointRecv>().swap(data_input_buff);
//3.data_input数据输出给data_input_buff
for (int i=0;i<data_input->size();i++)
@@ -253,16 +242,15 @@ int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input,
data_input_buff.push_back((*data_input)[i]);
}
return 0;
}
Dot_Coh::Dot_Coh(void)
{
}
void Dot_Coh::reset()
{
data_input_buff.clear();
}
data_process_class_dll/dot_coh.h
+7 -14
View File
@@ -3,8 +3,7 @@
#include "data_process_class_dll.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <QDebug>
#include <vector>
using namespace std;
@@ -14,29 +13,23 @@ class Dot_Coh
public:
// 点迹凝聚函数
int dot_coh_process( QVector <PointRecv> *data_input, //输入的点迹
QVector <PointRecv> *point_recv, //输出点迹
int dot_coh_process( std::vector <PointRecv> *data_input, //输入的点迹
std::vector <PointRecv> *point_recv, //输出点迹
struct RadarPara Work_Parameter //工作参数
);
//点迹凝聚函数，缓存一帧，一边输出，一边进行滑窗凝聚
int dot_coh_process_buff( QVector <PointRecv> *data_input, //输入的点迹
QVector <PointRecv> *point_recv, //输出点迹
int dot_coh_process_buff( std::vector <PointRecv> *data_input, //输入的点迹
std::vector <PointRecv> *point_recv, //输出点迹
struct RadarPara Work_Parameter //工作参数
);
//构造函数
Dot_Coh();
void reset();
private:
QVector <PointRecv> data_input_buff;
std::vector <PointRecv> data_input_buff;
};
data_process_class_dll/dot_coh_tas.cpp
+10 -17
View File
@@ -1,13 +1,11 @@
#include "dot_coh_tas.h"
#include <qmath.h>
#include"memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
using namespace std;
int Dot_Coh_TAS::dot_coh_tas_process(QVector <PointRecv> *data_input, //输入点迹
QVector <PointRecv> *point_recv_tas //输出点迹
int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *data_input, //输入点迹
std::vector <PointRecv> *point_recv_tas //输出点迹
)
{
if (data_input->size() == 0) {
@@ -24,7 +22,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector <PointRecv> *data_in
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)
if((*data_input)[loop_of_point].Use_Flag!=1 && (*data_input)[i].Use_Flag!=1)
{
float point_1_R=(*data_input)[i].Range;
float point_1_V=(*data_input)[i].Velocity;
@@ -32,7 +30,8 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector <PointRecv> *data_in
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)
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 && fabs(point_0_V-point_1_V)<=DOT_COH_V)
&& point_0_A <point_1_A )
{
point_0_R=(*data_input)[i].Range;
@@ -41,7 +40,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector <PointRecv> *data_in
point_0_F=(*data_input)[i].Azimuth;
(*data_input)[loop_of_point].Use_Flag=1;
}
else 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)
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_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;
@@ -51,10 +50,8 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector <PointRecv> *data_in
}
}
//删除凝聚点 和 量程范围外点
QVector <PointRecv>::iterator Iter;
std::vector <PointRecv>::iterator Iter;
for (Iter=data_input->begin(); Iter!=data_input->end();)
{
if((*Iter).Use_Flag==1 || (*Iter).Range <R_MIN || (*Iter).Range > R_MAX )
@@ -68,14 +65,10 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector <PointRecv> *data_in
}
}
for (int i=0;i<data_input->size();i++)
{
(*point_recv_tas).push_back((*data_input)[i]);
}
return 1;
}
data_process_class_dll/dot_coh_tas.h
+3 -6
View File
@@ -2,18 +2,15 @@
#define DOT_COH_TAS_H
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <vector>
class Dot_Coh_TAS
{
public:
// 点迹凝聚函数
int dot_coh_tas_process(QVector <PointRecv> *data_input, //输入点迹
QVector <PointRecv> *point_recv_tas //输出点迹
int dot_coh_tas_process(std::vector <PointRecv> *data_input, //输入点迹
std::vector <PointRecv> *point_recv_tas //输出点迹
);
};
#endif // DOT_COH_TAS_H
data_process_class_dll/kalman.cpp
+19 -51
View File
@@ -1,14 +1,18 @@
#include "kalman.h"
#include "coor_trans.h"
#include "parameters.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
using namespace std;
static double wrapAnglePi(double a)
{
while (a > PI) a -= 2*PI;
while (a < -PI) a += 2*PI;
return a;
}
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4])
@@ -31,7 +35,6 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub
R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R[1][0]=R[0][1];
P[0][0]=R[0][0];
P[0][1]=R[0][0]/T;
P[0][2]=R[0][1];
@@ -47,7 +50,6 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub
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;
@@ -55,7 +57,6 @@ 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)
{
@@ -111,8 +112,6 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
Matrix2d S;
S=H*P_pred*H.transpose()+R;
//d
Vector2d Z_presnet= Vector2d(Z[0],Z[1]);
Vector2d delta_z;
@@ -123,8 +122,6 @@ 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];
@@ -141,8 +138,6 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d
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];
@@ -157,14 +152,12 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d
R0[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R0[1][0]=R0[0][1];
Coor_trans.cart2polar(Z1[0],Z1[1],&rho,&theta);
R1[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R1[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R1[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R1[1][0]=R1[0][1];
Coor_trans.cart2polar(Z2[0],Z2[1],&rho,&theta);
R2[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R2[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
@@ -200,9 +193,6 @@ 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);
@@ -241,13 +231,11 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6])
Matrix2d S;
S=H*P_pred*H.transpose()+R;
//d
Vector2d delta_z;
delta_z=Z_mea-Z_pred;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
};
@@ -273,7 +261,6 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
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;
@@ -318,15 +305,14 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
Vector3d Z_mea(Z[0],Z[1],Z[2]);
Vector3d delta_z;
delta_z=Z_mea-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
double d=delta_z.transpose()*S.inverse()*delta_z;
return d;
}
double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind)
{
@@ -348,7 +334,6 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
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);
@@ -356,7 +341,6 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
X_pred = F1*X1;
P_pred = F1*P1*F1.transpose()+Q1;
//Z(k+1|k)
double x=X_pred[0];
double vx=X_pred[1];
@@ -396,18 +380,16 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
Matrix3d S;
S=H*P_pred*H.transpose()+R;
// bind_speed
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d delta_z;
delta_z=Z_mea-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
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;
};
@@ -435,7 +417,6 @@ void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6]
VectorXd X1_pred(6);
MatrixXd P1_pred(6,6);
X1_pred = F1*X1;
P1_pred = F1*P1*F1.transpose()+Q1;
@@ -446,11 +427,10 @@ void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6]
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 X_filter[6],double P_filter[6][6],double S_filter[3][3],
double prt,double freq_ind )
{
MatrixXd F1(6,6);
@@ -474,12 +454,9 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
X_pred = F1*X1;
P_pred = F1*P1*F1.transpose()+Q1;
Vector3d Z_mea(Z[0],Z[1],Z[2]);
Vector3d Z_pred;
@@ -521,10 +498,10 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
//bind_speed
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d delta_z;
delta_z=Z_mea-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
//X(k+1|k+1)
@@ -536,10 +513,8 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
MatrixXd I;
I.setIdentity(6, 6);
P=(I-K*H)*P_pred;
for (int i=0;i<6;i++)
X_filter[i]=X(i);
@@ -547,13 +522,12 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
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++)
for (int i=0;i<3;i++)
for (int j=0;j<3;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])
@@ -579,16 +553,11 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
X_pred = F1*X1;
P_pred = F1*P1*F1.transpose()+Q1;
Vector2d Z_mea(Z[0],Z[1]);
//Z(k+1|k)
MatrixXd H(2,6);
H<< 1,0,0,0,0,0,
@@ -614,7 +583,6 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
Matrix2d S;
S=H*P_pred*H.transpose()+R;
//kalmam gain
MatrixXd K;
@@ -629,10 +597,8 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
MatrixXd I;
I.setIdentity(6, 6);
P=(I-K*H)*P_pred;
for (int i=0;i<6;i++)
X_filter[i]=X(i);
@@ -718,6 +684,7 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
Vector3d Z_presnet= Vector3d(Z[0],Z[1],Z[2]);
Vector3d delta_z;
delta_z=Z_presnet-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
double d=delta_z.transpose()*S.inverse()*delta_z;
@@ -754,7 +721,6 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
Vector3d Z_pred;
Z_pred=H*X_pred;
//P(k+1|k)
Matrix2d Q;
Q<< 0.03*0.03, 0,
@@ -803,6 +769,7 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr);
Vector3d delta_z;
delta_z=Z_presnet-Z_pred;
delta_z(1)=wrapAnglePi(delta_z(1));
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
double d=delta_z.transpose()*S.inverse()*delta_z;
@@ -810,11 +777,12 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
};
double kalman::Bind_speed(double prt,double freq_ind)
{
double freq=FREQ0+freq_ind*0.02;
if (prt <= 0.0 || freq <= 0.0)
return 1.0; // 避免除零；正常流程 PRI 不允许为 0
return 150000.0/(freq*prt);
data_process_class_dll/kalman.h
+2 -4
View File
@@ -2,12 +2,11 @@
#define KALMAN_H
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <vector>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
class kalman
{
public:
@@ -16,7 +15,6 @@ public:
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_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind);
@@ -34,7 +32,7 @@ public:
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 X_filter[6],double P_filter[6][6],double S_filter[3][3],
double prt,double freq_ind );
double Bind_speed(double prt,double freq_ind); //根据PRT 和 频率 计算不模糊速度
data_process_class_dll/struct.h
+8 -20
View File
@@ -2,7 +2,7 @@
#define STRUCT_H
#pragma once
#include <QVector>
#include <vector>
using namespace std;
/********************************* 数据处理结构体 **********************************************/
@@ -16,8 +16,8 @@ struct PointRecv
double Height; //高度
double snr; //信噪比
double RCS; //目标RCS
qint64 CPI_Time; //CPI时间
qint64 GNSS_time; //GNSS时间
long long CPI_Time; //CPI时间
long long GNSS_time; //GNSS时间
int Point_index; //点迹号 1~50
int Point_Sum; //点迹总数
int Use_Flag; //点迹使用标志 1使用 0未使用
@@ -32,7 +32,6 @@ struct PointRecv
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
};
// 可靠航迹数据结构
struct Trust_Track
{
@@ -41,8 +40,8 @@ struct Trust_Track
double Amplitude; //幅度
double Height; //高度
qint64 T_track; //航迹时间
qint64 GNSS_time; //GNSS时间
long long T_track; //航迹时间
long long GNSS_time; //GNSS时间
int Track_Sum; //航迹总数
int Point_Index; //该航迹上的第几个点
@@ -62,8 +61,7 @@ struct Trust_Track
int Track_section_idx; //航迹区号
QVector <double> Hight_smooth; //高度平滑
std::vector <double> Hight_smooth; //高度平滑
double range_point; //关联上的点信息（距离、方位、俯仰）
double azi_point;
@@ -92,8 +90,6 @@ struct Trust_Track
};
//临时航迹结构体
struct Temp_track
{
@@ -108,8 +104,8 @@ struct Temp_track
double snr; //信噪比
double RCS; //RCS
double Amp; //幅度
qint64 T; //时间戳
qint64 GNSS_time; //GNSS时间戳
long long T; //时间戳
long long GNSS_time; //GNSS时间戳
double d; //关联上的点的d
@@ -124,11 +120,6 @@ struct Temp_track
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
};
// TAS跟踪目标结构体
struct Tracking_Target
{
@@ -137,7 +128,6 @@ struct Tracking_Target
};
//引导跟踪目标结构体
struct Direct_Tracking_Target
{
@@ -150,6 +140,4 @@ struct Direct_Tracking_Target
};
#endif // STRUCT_H
data_process_class_dll/tas_ctrl.cpp
+35 -35
View File
@@ -1,17 +1,11 @@
#include <tas_ctrl.h>
#include "tas_ctrl.h"
#include "coor_trans.h"
#include <qmath.h>
#include"memory.h"
#include <QVector>
#include <QDebug>
#include <cmath>
#include <cstring>
#include <vector>
using namespace std;
TAS_Ctrl::TAS_Ctrl()
{
memset(tas_target_queue,0,TAS_QUEUE_LENGTH*sizeof(Tracking_Target));
@@ -19,13 +13,22 @@ TAS_Ctrl::TAS_Ctrl()
tas_target_num=0;
}
void TAS_Ctrl::tas_ctrl_process(QVector<Trust_Track> *trust_track,
void TAS_Ctrl::reset()
{
memset(tas_target_queue,0,sizeof(tas_target_queue));
tas_target_num=0;
}
void TAS_Ctrl::tas_ctrl_process(std::vector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter,
qint64 latest_timestamp)
long long latest_timestamp)
{
if (Trust_track_num_Output == 0) return;
*Trust_track_num_Output = 0;
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,latest_timestamp);
@@ -40,8 +43,7 @@ void TAS_Ctrl::tas_ctrl_process(QVector<Trust_Track> *trust_track,
memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target));
};
void TAS_Ctrl::tas_target_add(QVector<Trust_Track> *trust_track,
void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
@@ -67,6 +69,9 @@ void TAS_Ctrl::tas_target_add(QVector<Trust_Track> *trust_track,
{
if(tas_target_queue[j].empty_flag==0)// && j%2==0)
{
if (*Trust_track_num_Output >= MAX_TRACK_NUM)
break;
//插入跟踪队列
tas_target_queue[j].Index=(*trust_track)[i].Track_Index;
tas_target_queue[j].empty_flag=1;
@@ -102,12 +107,8 @@ void TAS_Ctrl::tas_target_add(QVector<Trust_Track> *trust_track,
}
}
};
//int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter)
//{
@@ -128,7 +129,6 @@ void TAS_Ctrl::tas_target_add(QVector<Trust_Track> *trust_track,
//};
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter)
{
@@ -142,10 +142,9 @@ int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct
return 0;
}
void TAS_Ctrl::tas_target_del(QVector<Trust_Track> *trust_track,
void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
@@ -224,7 +223,6 @@ void TAS_Ctrl::tas_target_del(QVector<Trust_Track> *trust_track,
// }
};
//int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter)
@@ -246,18 +244,17 @@ void TAS_Ctrl::tas_target_del(QVector<Trust_Track> *trust_track,
//};
void TAS_Ctrl::tas_beam_output(QVector<Trust_Track> *trust_track,
void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
int latest_timestamp)
long long latest_timestamp)
{
if(tas_target_queue[0].empty_flag==1)
{
double H_track;
double X_now[6];
int CPI_time = 0;
double H_track = 0;
double X_now[6] = {0};
long long CPI_time = 0;
bool found_track = false;
for (int i=0;i<trust_track->size();i++ )
{
if((*trust_track)[i].Track_Index == tas_target_queue[0].Index)
@@ -266,9 +263,16 @@ void TAS_Ctrl::tas_beam_output(QVector<Trust_Track> *trust_track,
CPI_time = (*trust_track)[i].T_track;
for(int ii=0;ii<6;ii++)
X_now[ii]=(*trust_track)[i].X[ii];
found_track = true;
}
}
if (!found_track)
{
Tracking_beam->open_flag = 0;
return;
}
float delta_T = (latest_timestamp - CPI_time) / 1000.0f;
//预测目标位置 计算跟踪波束波位号 俯仰角
@@ -286,7 +290,6 @@ void TAS_Ctrl::tas_beam_output(QVector<Trust_Track> *trust_track,
//目标俯仰角
double elev=asin(H_track/range)/PI*180;
// if(elev<=0)
// elev=0;
// else if(elev>=40)
@@ -294,7 +297,6 @@ void TAS_Ctrl::tas_beam_output(QVector<Trust_Track> *trust_track,
// else
// elev=elev;
Tracking_beam->Elev=elev;
//跟踪波束类型
@@ -306,7 +308,7 @@ void TAS_Ctrl::tas_beam_output(QVector<Trust_Track> *trust_track,
//跟踪波束开关开启
Tracking_beam->open_flag=1;
//qDebug() << "TAS: range: " <<Tracking_beam->Range << "azi: " <<Tracking_beam->Azi <<"pit: " <<Tracking_beam->Elev;
//std::cout << "TAS: range: " <<Tracking_beam->Range << "azi: " <<Tracking_beam->Azi <<"pit: " <<Tracking_beam->Elev;
}
else
{
@@ -314,6 +316,4 @@ void TAS_Ctrl::tas_beam_output(QVector<Trust_Track> *trust_track,
Tracking_beam->open_flag=0;
}
}
data_process_class_dll/tas_ctrl.h
+9 -10
View File
@@ -3,43 +3,42 @@
#include "data_process_class_dll.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <vector>
using namespace std;
class TAS_Ctrl
{
public:
TAS_Ctrl();
void tas_ctrl_process( QVector<Trust_Track> *trust_track,
void reset();
void tas_ctrl_process( std::vector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter,
qint64 latest_timestamp);
long long latest_timestamp);
private:
//添加TAS目标
void tas_target_add(QVector<Trust_Track> *trust_track,
void tas_target_add(std::vector<Trust_Track> *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> *trust_track,
void tas_target_del(std::vector<Trust_Track> *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> *trust_track,
void tas_beam_output(std::vector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
int latest_timestamp);
long long latest_timestamp);
//TAS 自动开启条件
int tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter);
data_process_class_dll/track_asso.cpp
+71 -70
View File
@@ -1,19 +1,18 @@
#include "track_asso.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
// //////////////////////////// 用point_recv中点迹与trust_track中的点迹进行关联， /////////////
// //////////////////////////// 航迹更新结果通过Trust_Track_Output[MAX_TRACK_NUM][10]和 Trust_track_num_Output输出 /////////////
int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv, //输入点迹
QVector <Trust_Track> *trust_track, //航迹文件
int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_recv, //输入点迹
std::vector <Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter //雷达参数
@@ -28,7 +27,6 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
point_process[n-1].point_section_asso = 1;
}
//IMM算法
model_interaction(trust_track);
@@ -36,9 +34,8 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
model_output(trust_track);
//删除关联上的点
QVector <PointRecv>::iterator Iter;
std::vector <PointRecv>::iterator Iter;
for (Iter= point_process.begin(); Iter!=point_process.end();)
{
if((*Iter).Use_Flag==1 )
@@ -53,7 +50,7 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
}
//剩余点重新存入点迹
QVector<PointRecv>().swap((*point_recv));
std::vector<PointRecv>().swap((*point_recv));
for (int i=0;i<point_process.size();i++)
{
@@ -62,7 +59,7 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
}
//point_process清空
QVector<PointRecv>().swap(point_process);
std::vector<PointRecv>().swap(point_process);
//输出航迹
for (int i=0;i<trust_track->size();i++)
@@ -93,18 +90,15 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
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].GNSS_time=(*trust_track)[i].GNSS_time;
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;}
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180;
//关联点信息
@@ -140,10 +134,9 @@ int Track_Asso:: track_asso_process(QVector<PointRecv> *point_recv
}
//遍历所有航迹 进行多模型交互
void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
void Track_Asso:: model_interaction(std::vector <Trust_Track> *trust_track)
{
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
@@ -159,6 +152,9 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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];
if (c[0] <= 1e-12) c[0] = 1e-12;
if (c[1] <= 1e-12) c[1] = 1e-12;
if (c[2] <= 1e-12) c[2] = 1e-12;
u_t[0][0]=Pt[0][0]*u_last[0]/c[0];
u_t[1][0]=Pt[1][0]*u_last[1]/c[0];
@@ -170,7 +166,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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++)
{
@@ -179,7 +174,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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];
@@ -187,7 +181,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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];
@@ -195,7 +188,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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++)
{
@@ -211,7 +203,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
}
for (int i=0;i<6;i++)
for(int j=0;j<6;j++)
{
@@ -226,7 +217,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
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++)
@@ -253,7 +243,6 @@ void Track_Asso:: model_interaction(QVector <Trust_Track> *trust_track)
}
}
// 产生 F Q 矩阵
void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter)
{
@@ -281,8 +270,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
F[5][4]=0;
F[5][5]=exp(-alpha*delta_T);
//模型1
//Q
double q1=Work_Parameter.Model1_Q_slow;
@@ -301,7 +288,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
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;
@@ -343,7 +329,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
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;
@@ -381,7 +366,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
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;
@@ -391,10 +375,8 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233;
Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333;
}
// 计算三个 d
void Track_Asso::IMM_d_cal(double v_track,
double X1[6], double P1[6][6],
@@ -415,10 +397,7 @@ void Track_Asso::IMM_d_cal(double v_track,
}
void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarPara Work_Parameter)
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct RadarPara Work_Parameter)
{
//存储关联信息
@@ -431,7 +410,7 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
int point_index;
int track_index;
};
QVector <asso_info> associated_info;
std::vector <asso_info> associated_info;
//遍历所有点迹航迹 计算点迹和航迹的统计距离
for (int loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
@@ -443,7 +422,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
for (int loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
{
//时间差
double delta_T = (point_process[loop_of_point].CPI_Time - (*trust_track)[loop_of_track].T_track)/1000.0;
//航迹X P
@@ -523,8 +501,8 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
// d_h = 1;
// }
// qDebug() << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// qDebug() << h_track<<" "<<h_point<<" "<<d_h;
// std::cout << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// std::cout << h_track<<" "<<h_point<<" "<<d_h;
// d_h = 1;
//计算俯仰门限
@@ -565,12 +543,12 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
//小于关联门限 保存关联信息
if(
( r_track<=500 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000) && d_p && d_r)//
||( (r_track>500&&r_track<1000) && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/100) && d_p && d_r)//
( r_track<=500 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000.0) && d_p && d_r)//
||( (r_track>500&&r_track<1000) && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/100.0) && d_p && d_r)//
||( r_track>=1000 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD) && d_p && d_r)//
)
{
//qDebug() << "asooooooooooo";
//std::cout << "asooooooooooo";
struct asso_info associated_info_tmp;
associated_info_tmp.d1=d1;
@@ -591,7 +569,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
// }
}
//最近邻法关联
int associated_num=associated_info.size();
for (int i=0;i<trust_track->size();i++)
@@ -632,30 +609,57 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
if (delta_T <= 0)
{
// 乱序/重复时间戳：不作为有效关联，不标记点迹已使用、不刷新航迹新鲜度
for (int ii=0;ii<associated_num;)
{
if(associated_info[ii].track_index==track_index)
{
associated_info.erase(associated_info.begin()+ii);
associated_num=associated_info.size();
}
else
ii++;
}
associated_num=associated_info.size();
for (int ii=0;ii<associated_num;)
{
if(associated_info[ii].point_index==point_index)
{
associated_info.erase(associated_info.begin()+ii);
associated_num=associated_info.size();
}
else
ii++;
}
associated_num=associated_info.size();
continue;
}
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];
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
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++)
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;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 Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0;
double det_S2=S2_out.determinant();
double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2);
double Possibility2=(det_S2>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S2)*exp(-0.5*d2)):0.0;
double det_S3=S3_out.determinant();
double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3);
double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0;
//更新模型概率
double u_last[3];
@@ -668,9 +672,14 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
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]);
double u_den = Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2];
if (u_den > 1e-12)
{
(*trust_track)[track_index-1].u[0]=Possibility1*c[0]/u_den;
(*trust_track)[track_index-1].u[1]=Possibility2*c[1]/u_den;
(*trust_track)[track_index-1].u[2]=Possibility3*c[2]/u_den;
}
// 若分母异常，保持上一拍模型概率
//更新 X1 X2 X3 P1 P2 P3
for(int ii=0;ii<6;ii++)
@@ -691,7 +700,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
(*trust_track)[track_index-1].T_track = point_process[point_index-1].CPI_Time;
(*trust_track)[track_index-1].GNSS_time = point_process[point_index-1].GNSS_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;
@@ -746,7 +754,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
}
}
//未关联上的航迹 进行外推 tws
for (int i =0; i<(*trust_track).size();i++ )
{
@@ -797,7 +804,6 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
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].GNSS_time = (*trust_track)[i].GNSS_time+delta_T*1000.0; //更新航迹时间
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间
@@ -805,12 +811,9 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct RadarP
}
}
}
void Track_Asso:: model_output(QVector <Trust_Track> *trust_track)
void Track_Asso:: model_output(std::vector <Trust_Track> *trust_track)
{
for(int loop_of_track=0; loop_of_track<trust_track->size(); loop_of_track++)
{
@@ -861,7 +864,6 @@ void Track_Asso:: model_output(QVector <Trust_Track> *trust_track)
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];
@@ -910,22 +912,18 @@ void Track_Asso:: model_output(QVector <Trust_Track> *trust_track)
}
//高度维更新
void Track_Asso:: track_hight_update(int updata_track_index, //更新的航迹号
int asso_point_index, //点迹号
QVector <Trust_Track> *trust_track //航迹
std::vector <Trust_Track> *trust_track //航迹
)
{
if(updata_track_index<=trust_track->size() && asso_point_index<=point_process.size())
{
(*trust_track)[updata_track_index-1].Hight_smooth.push_back(point_process[asso_point_index-1].Height);
int height_win_length;
if((*trust_track)[updata_track_index-1].range_point <= 1000)
@@ -971,12 +969,15 @@ void Track_Asso:: track_hight_update(int updata_
}
};
Track_Asso::Track_Asso()
{
Pt[0][0]=0.8;Pt[0][1]=0.15;Pt[0][2]=0.05; //IMM 转移概率
Pt[1][0]=0.3;Pt[1][1]=0.4;Pt[1][2]=0.3;
Pt[2][0]=0.05;Pt[2][1]=0.15;Pt[2][2]=0.8;
};
void Track_Asso::reset()
{
point_process.clear();
}
data_process_class_dll/track_asso.h
+10 -17
View File
@@ -1,44 +1,39 @@
#ifndef TRACK_ASSO_H
#define TRACK_ASSO_H
#include "data_process_class_dll.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <QDebug>
#include <vector>
using namespace std;
/******************************* 航迹关联类 **************************************************/
class Track_Asso
{
public:
int track_asso_process(QVector <PointRecv> *point_recv, //点迹文件
QVector <Trust_Track> *trust_track, //航迹文件
int track_asso_process(std::vector <PointRecv> *point_recv, //点迹文件
std::vector <Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter //工作参数
);
Track_Asso();
void reset();
private:
//要处理的点迹
QVector<PointRecv> point_process;
std::vector<PointRecv> point_process;
//IMM
void model_interaction(QVector <Trust_Track> *trust_track); //模型交互
void model_interaction(std::vector <Trust_Track> *trust_track); //模型交互
void model_filter(QVector <Trust_Track> *trust_track, //滤波
void model_filter(std::vector <Trust_Track> *trust_track, //滤波
struct RadarPara Work_Parameter
);
void model_output(QVector <Trust_Track> *trust_track ); //模型输出
void model_output(std::vector <Trust_Track> *trust_track ); //模型输出
void IMM_d_cal(double v_track,
double X1[6], double P1[6][6],
@@ -53,12 +48,10 @@ private:
double Pt[3][3]; //模型转移概率
//高度维更新
void track_hight_update(int updata_track_index, //更新的航迹号
int asso_point_index, //点迹号
QVector <Trust_Track> *trust_track //航迹
std::vector <Trust_Track> *trust_track //航迹
);
};
#endif // TRACK_ASSO_H
data_process_class_dll/track_asso_direct_tracking.cpp
+37 -53
View File
@@ -1,15 +1,14 @@
#include "track_asso_direct_tracking.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector<PointRecv> *point_recv, //点迹
int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<PointRecv> *point_recv, //点迹
Trust_Track *trust_track, //航迹
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
@@ -31,8 +30,7 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector<Point
model_output(trust_track);
//point_process清空
QVector<PointRecv>().swap(point_process);
std::vector<PointRecv>().swap(point_process);
//输出航迹
@@ -47,7 +45,12 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector<Point
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;
{
double elev_ratio = (r > 0.0) ? (*trust_track).Height / r : 0.0;
if (elev_ratio > 1.0) elev_ratio = 1.0;
if (elev_ratio < -1.0) elev_ratio = -1.0;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin(elev_ratio)/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]);
@@ -58,9 +61,8 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector<Point
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;}
Direction_Angle=atan2((*trust_track).X[4], (*trust_track).X[1]);
if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180;
//关联点信息
@@ -77,12 +79,9 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector<Point
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;
}
void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track) //模型交互
{
@@ -96,6 +95,9 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
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];
if (c[0] <= 1e-12) c[0] = 1e-12;
if (c[1] <= 1e-12) c[1] = 1e-12;
if (c[2] <= 1e-12) c[2] = 1e-12;
u_t[0][0]=Pt[0][0]*u_last[0]/c[0];
u_t[1][0]=Pt[1][0]*u_last[1]/c[0];
@@ -107,7 +109,6 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
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++)
{
@@ -116,7 +117,6 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
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];
@@ -124,7 +124,6 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
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];
@@ -132,7 +131,6 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
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++)
{
@@ -148,7 +146,6 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
}
for (int i=0;i<6;i++)
for(int j=0;j<6;j++)
{
@@ -163,7 +160,6 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
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++)
@@ -187,10 +183,8 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
(*trust_track).P3[i][j]=Po3_last[i][j];
}
}
void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, //滤波
struct RadarPara Work_Parameter)
{
@@ -203,8 +197,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
double d_min;
int point_index;
};
QVector <asso_info> associated_info;
std::vector <asso_info> associated_info;
//航迹信息
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
@@ -316,7 +309,6 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
}
}
//有关联 滤波
if (associated_info.size()>0)
{
@@ -348,7 +340,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
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];
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
kalman Kalman;
// Kalman.kalman_filter(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1);
// Kalman.kalman_filter(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2);
@@ -356,20 +348,20 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
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++)
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;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 Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0;
double det_S2=S2_out.determinant();
double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2);
double Possibility2=(det_S2>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S2)*exp(-0.5*d2)):0.0;
double det_S3=S3_out.determinant();
double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3);
double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0;
//更新航迹
@@ -384,9 +376,14 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
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).u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
(*trust_track).u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
(*trust_track).u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
double u_den = Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2];
if (u_den > 1e-12)
{
(*trust_track).u[0]=Possibility1*c[0]/u_den;
(*trust_track).u[1]=Possibility2*c[1]/u_den;
(*trust_track).u[2]=Possibility3*c[2]/u_den;
}
// 若分母异常，保持上一拍模型概率
//更新 X1 X2 X3 P1 P2 P3
for(int ii=0;ii<6;ii++)
@@ -406,8 +403,6 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
//更新航迹时间
(*trust_track).T_track = point_process[point_index-1].CPI_Time;
//更新关联上的点迹信息
(*trust_track).range_point=point_process[point_index-1].Range;
(*trust_track).azi_point=point_process[point_index-1].Azimuth/PI*180;
@@ -426,7 +421,6 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
track_hight_update(point_index,trust_track);
}
//未关联上，航迹外推
else
{
@@ -454,7 +448,6 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
};
void Track_Asso_Direct_Tracking::model_output(Trust_Track *trust_track)
{
@@ -502,7 +495,6 @@ void Track_Asso_Direct_Tracking::model_output(Trust_Track *trust_track)
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).X[0]=X_filter[0]; //位置 速度
(*trust_track).X[1]=X_filter[1];
@@ -515,7 +507,6 @@ void Track_Asso_Direct_Tracking::model_output(Trust_Track *trust_track)
for (int jj=0;jj<6;jj++)
(*trust_track).P[ii][jj]=P_filter[ii][jj];
}
// 产生 F Q 矩阵
@@ -545,8 +536,6 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
F[5][4]=0;
F[5][5]=exp(-alpha*delta_T);
//模型1
//Q
double q1=Work_Parameter.Model1_Q_slow;
@@ -565,7 +554,6 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
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;
@@ -598,7 +586,6 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
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;
@@ -621,7 +608,6 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
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;
@@ -631,10 +617,8 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233;
Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333;
}
// 计算三个 d
void Track_Asso_Direct_Tracking::IMM_d_cal(double v_track,
double X1[6], double P1[6][6],
@@ -658,8 +642,6 @@ 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 //航迹
@@ -668,7 +650,6 @@ void Track_Asso_Direct_Tracking::track_hight_update(int asso_point
(*trust_track).Hight_smooth.push_back(point_process[asso_point_index-1].Height);
int height_win_length;
if((*trust_track).range_point <= 1000)
{
@@ -687,7 +668,6 @@ void Track_Asso_Direct_Tracking::track_hight_update(int asso_point
height_win_length = H_F_WIN_LEN+6;
}
double sum=0;
if((*trust_track).Hight_smooth.size()<height_win_length)
{
@@ -720,5 +700,9 @@ Track_Asso_Direct_Tracking::Track_Asso_Direct_Tracking()
Pt[1][0]=0.3;Pt[1][1]=0.4;Pt[1][2]=0.3;
Pt[2][0]=0.05;Pt[2][1]=0.15;Pt[2][2]=0.8;
};
void Track_Asso_Direct_Tracking::reset()
{
point_process.clear();
}
data_process_class_dll/track_asso_direct_tracking.h
+4 -8
View File
@@ -3,14 +3,13 @@
#include "data_process_class_dll.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <vector>
using namespace std;
class Track_Asso_Direct_Tracking
{
public:
int track_asso_process_direct_tracking(QVector<PointRecv> *point_recv, //点迹
int track_asso_process_direct_tracking(std::vector<PointRecv> *point_recv, //点迹
Trust_Track *trust_track, //航迹
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
@@ -18,18 +17,16 @@ class Track_Asso_Direct_Tracking
);
Track_Asso_Direct_Tracking();
void reset();
private:
//要处理的点迹
QVector<PointRecv> point_process;
std::vector<PointRecv> point_process;
//IMM
void model_interaction(Trust_Track *trust_track); //模型交互
void model_filter(Trust_Track *trust_track, //滤波
struct RadarPara Work_Parameter
);
@@ -55,5 +52,4 @@ class Track_Asso_Direct_Tracking
);
};
#endif // TRACK_ASSO_DIRECT_TRACKING_H
data_process_class_dll/track_asso_tas.cpp
+63 -85
View File
@@ -1,21 +1,20 @@
#include "track_asso_tas.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
int Track_Asso_Tas::track_asso_process_tas(QVector<PointRecv> *point_recv_tas, //点迹文件
QVector<Trust_Track> *trust_track, //航迹文件
int Track_Asso_Tas::track_asso_process_tas(std::vector<PointRecv> *point_recv_tas, //点迹文件
std::vector<Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter, //工作参数
int tas_track_idx,
qint64 latest_timestamp //最新时间戳
long long latest_timestamp //最新时间戳
)
{
@@ -34,8 +33,7 @@ qint64 latest_timestamp //最新时间戳
model_output(trust_track,tas_track_idx);
//point_process清空
QVector<PointRecv>().swap(point_process);
std::vector<PointRecv>().swap(point_process);
//输出航迹
for (int i=0;i<trust_track->size();i++)
@@ -53,7 +51,12 @@ qint64 latest_timestamp //最新时间戳
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;
{
double elev_ratio = (r > 0.0) ? (*trust_track)[i].Height / r : 0.0;
if (elev_ratio > 1.0) elev_ratio = 1.0;
if (elev_ratio < -1.0) elev_ratio = -1.0;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin(elev_ratio)/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]);
@@ -65,9 +68,8 @@ qint64 latest_timestamp //最新时间戳
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;}
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180;
//关联点信息
@@ -93,30 +95,23 @@ qint64 latest_timestamp //最新时间戳
return 0;
}
//进行多模型交互
void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, int tas_track_idx)
void Track_Asso_Tas:: model_interaction( std::vector<Trust_Track> *trust_track, int tas_track_idx)
{
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
{
// if(point_process.size()>0)
// {
// qDebug() << "model_interaction point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
// std::cout << "model_interaction point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
// }
double u_last[3];
u_last[0]=(*trust_track)[loop_of_track].u[0];
u_last[1]=(*trust_track)[loop_of_track].u[1];
@@ -127,6 +122,9 @@ void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, in
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];
if (c[0] <= 1e-12) c[0] = 1e-12;
if (c[1] <= 1e-12) c[1] = 1e-12;
if (c[2] <= 1e-12) c[2] = 1e-12;
u_t[0][0]=Pt[0][0]*u_last[0]/c[0];
u_t[1][0]=Pt[1][0]*u_last[1]/c[0];
@@ -138,7 +136,6 @@ void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, in
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++)
{
@@ -147,7 +144,6 @@ void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, in
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];
@@ -155,7 +151,6 @@ void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, in
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];
@@ -163,7 +158,6 @@ void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, in
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++)
{
@@ -179,7 +173,6 @@ void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, in
}
for (int i=0;i<6;i++)
for(int j=0;j<6;j++)
{
@@ -194,7 +187,6 @@ void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, in
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++)
@@ -222,11 +214,10 @@ void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, in
}
void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust_track, //滤波
void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *trust_track, //滤波
struct RadarPara Work_Parameter,
int tas_track_idx,
qint64 latest_timestamp //最新时间戳
long long latest_timestamp //最新时间戳
)
{
//存储关联信息
@@ -238,24 +229,24 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
double d_min;
int point_index;
};
QVector <asso_info> associated_info;
std::vector <asso_info> 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] = {0},X2[6] = {0},X3[6] = {0},P1[6][6] = {{0}},P2[6][6] = {{0}},P3[6][6] = {{0}};
double T_track = 0;
double v_track = 0;
double r_track = 0;
double h_track = 0;
bool found_tas_track = false;
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
{
found_tas_track = true;
// if(point_process.size()>0)
// {
// qDebug() << "model_filter point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
// std::cout << "model_filter point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
// }
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
@@ -274,7 +265,8 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
}
if (!found_tas_track)
return;
// 计算量测和航迹统计距离
for (int loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
@@ -289,6 +281,8 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
double prt = point_process[loop_of_point].PRF_index;
double freq_ind = point_process[loop_of_point].Freq_index;
double delta_T = (point_process[loop_of_point].CPI_Time - T_track)/1000.0;
if (delta_T <= 0)
continue;
double h_point = point_process[loop_of_point].Height;
//计算统计距离d
@@ -348,17 +342,15 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
d_h = 1;
}
// qDebug() << "T d" <<point_process[loop_of_point].CPI_Time/1000<<" "<<T_track/1000<<" "<<delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// qDebug() << "h_track" <<h_track<<" h_point"<<h_point<<" d_h"<<d_h;
// std::cout << "T d" <<point_process[loop_of_point].CPI_Time/1000<<" "<<T_track/1000<<" "<<delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// std::cout << "h_track" <<h_track<<" h_point"<<h_point<<" d_h"<<d_h;
//小于关联门限 保存关联信息
if((d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD)
&& d_h == 1)//
{
// qDebug() << "TAS asoooooo111" ;
// std::cout << "TAS asoooooo111" ;
struct asso_info associated_info_tmp;
associated_info_tmp.d1=d1;
associated_info_tmp.d2=d2;
@@ -379,7 +371,7 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
if (associated_info.size()>0)
{
// qDebug() << "TAS asoooooo222" ;
// std::cout << "TAS asoooooo222" ;
//找最近点
int min_index=1;
@@ -409,7 +401,7 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
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];
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
kalman Kalman;
// Kalman.kalman_filter(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1);
// Kalman.kalman_filter(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2);
@@ -417,20 +409,20 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
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++)
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;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 Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0;
double det_S2=S2_out.determinant();
double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2);
double Possibility2=(det_S2>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S2)*exp(-0.5*d2)):0.0;
double det_S3=S3_out.determinant();
double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3);
double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0;
//更新航迹
for(int i=0;i<trust_track->size();i++)
@@ -448,9 +440,14 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
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]);
double u_den = Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2];
if (u_den > 1e-12)
{
(*trust_track)[i].u[0]=Possibility1*c[0]/u_den;
(*trust_track)[i].u[1]=Possibility2*c[1]/u_den;
(*trust_track)[i].u[2]=Possibility3*c[2]/u_den;
}
// 若分母异常，保持上一拍模型概率
//更新 X1 X2 X3 P1 P2 P3
for(int ii=0;ii<6;ii++)
@@ -471,8 +468,6 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
(*trust_track)[i].T_track = point_process[point_index-1].CPI_Time;
(*trust_track)[i].GNSS_time = point_process[point_index-1].GNSS_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;
@@ -505,6 +500,8 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
if((*trust_track)[i].Track_Index == tas_track_idx)
{
double delta_T = (latest_timestamp - T_track)/1000.0;
if (delta_T <= 0)
continue;
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);
@@ -528,13 +525,9 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
}
}
void Track_Asso_Tas::model_output(QVector<Trust_Track> *trust_track, int tas_track_idx )
void Track_Asso_Tas::model_output(std::vector<Trust_Track> *trust_track, int tas_track_idx )
{
for(int i=0; i<trust_track->size(); i++)
if((*trust_track)[i].Track_Index==tas_track_idx)
@@ -584,7 +577,6 @@ void Track_Asso_Tas::model_output(QVector<Trust_Track> *trust_track, int tas_
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];
@@ -604,12 +596,8 @@ void Track_Asso_Tas::model_output(QVector<Trust_Track> *trust_track, int tas_
}
}
// 产生 F Q 矩阵
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)
{
@@ -637,8 +625,6 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6],
F[5][4]=0;
F[5][5]=exp(-alpha*delta_T);
//模型1
//Q
double q1=Work_Parameter.Model1_Q_slow;
@@ -657,7 +643,6 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6],
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;
@@ -690,7 +675,6 @@ 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;
@@ -713,7 +697,6 @@ double q223=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*
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;
@@ -723,10 +706,8 @@ Q3[3][3]=q3*q113; Q3[3][4]=q3*q123; Q3[3][5]=q3*q133;
Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233;
Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333;
}
// 计算三个 d
void Track_Asso_Tas::IMM_d_cal(double v_track,
double X1[6], double P1[6][6],
@@ -750,12 +731,10 @@ struct RadarPara Work_Parameter)
}
//高度维更新
void Track_Asso_Tas::track_hight_update(int tas_track_idx, //更新的航迹号
int asso_point_index, //点迹号
QVector<Trust_Track> *trust_track //航迹
std::vector<Trust_Track> *trust_track //航迹
)
{
for(int i=0; i<trust_track->size(); i++)
@@ -763,7 +742,6 @@ void Track_Asso_Tas::track_hight_update(int tas_
{
(*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)
{
@@ -782,7 +760,6 @@ void Track_Asso_Tas::track_hight_update(int tas_
height_win_length = H_F_WIN_LEN+6;
}
double sum=0;
if((*trust_track)[i].Hight_smooth.size()<height_win_length)
{
@@ -807,11 +784,8 @@ void Track_Asso_Tas::track_hight_update(int tas_
(*trust_track)[i].Height=sum/height_win_length;
}
}
}
Track_Asso_Tas::Track_Asso_Tas()
@@ -820,5 +794,9 @@ Track_Asso_Tas::Track_Asso_Tas()
Pt[1][0]=0.3;Pt[1][1]=0.4;Pt[1][2]=0.3;
Pt[2][0]=0.05;Pt[2][1]=0.15;Pt[2][2]=0.8;
};
void Track_Asso_Tas::reset()
{
point_process.clear();
}
data_process_class_dll/track_asso_tas.h
+12 -18
View File
@@ -3,43 +3,40 @@
#include "data_process_class_dll.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <QDebug>
using namespace std;
#include <vector>
using namespace std;
/******************************* 航迹关联类 **************************************************/
class Track_Asso_Tas
{
public:
int track_asso_process_tas(QVector<PointRecv> *point_recv_tas, //点迹文件
QVector<Trust_Track> *trust_track, //航迹文件
int track_asso_process_tas(std::vector<PointRecv> *point_recv_tas, //点迹文件
std::vector<Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter, //工作参数
int tas_track_idx,
qint64 latest_timestamp //最新时间戳
long long latest_timestamp //最新时间戳
);
Track_Asso_Tas();
void reset();
private:
//要处理的点迹
QVector<PointRecv> point_process;
std::vector<PointRecv> point_process;
//IMM
void model_interaction(QVector<Trust_Track> *trust_track, int tas_track_idx); //模型交互
void model_interaction(std::vector<Trust_Track> *trust_track, int tas_track_idx); //模型交互
void model_filter(QVector<Trust_Track> *trust_track, //滤波
void model_filter(std::vector<Trust_Track> *trust_track, //滤波
struct RadarPara Work_Parameter,
int tas_track_idx,
qint64 latest_timestamp //最新时间戳
long long latest_timestamp //最新时间戳
);
void model_output(QVector<Trust_Track> *trust_track, int tas_track_idx ); //模型输出
void model_output(std::vector<Trust_Track> *trust_track, int tas_track_idx ); //模型输出
void IMM_d_cal(double v_track,
double X1[6], double P1[6][6],
@@ -57,12 +54,9 @@ private:
//高度维更新
void track_hight_update(int tas_track_index, //更新的航迹号
int asso_point_index, //点迹号
QVector<Trust_Track> *trust_track //航迹
std::vector<Trust_Track> *trust_track //航迹
);
};
#endif // TRACK_ASSO_TAS_H
data_process_class_dll/track_die.cpp
+5 -7
View File
@@ -1,25 +1,23 @@
#include "track_die.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include"memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
using namespace std;
void Track_Die::track_die_process( QVector <Trust_Track> *trust_track,
void Track_Die::track_die_process( std::vector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output)
{
QVector <Trust_Track>::iterator Iter;
std::vector <Trust_Track>::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;
data_process_class_dll/track_die.h
+2 -7
View File
@@ -3,20 +3,15 @@
#include "data_process_class_dll.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <vector>
using namespace std;
class Track_Die
{
public:
void track_die_process( QVector <Trust_Track> *trust_track,
void track_die_process( std::vector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output);
};
#endif // TRACK_DIE_H
data_process_class_dll/track_die_tas.cpp
+8 -9
View File
@@ -1,27 +1,26 @@
#include "track_die_tas.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include"memory.h"
#include <QVector>
#include <QDebug>
#include <iostream>
#include <cmath>
#include <cstring>
#include <vector>
using namespace std;
void Track_Die_Tas::track_die_process_tas( QVector <Trust_Track> *trust_track,
void Track_Die_Tas::track_die_process_tas( std::vector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output)
{
QVector <Trust_Track>::iterator Iter;
std::vector <Trust_Track>::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)
{
qDebug() << "remove tas target :" << (*Iter).Track_Index;
std::cout << "remove tas target :" << (*Iter).Track_Index << std::endl;
//输出消亡信息
*Track_die_num_Output=*Track_die_num_Output+1;
Track_die_Index_Output[*Track_die_num_Output-1]=(*Iter).Track_Index;
data_process_class_dll/track_die_tas.h
+2 -7
View File
@@ -3,20 +3,15 @@
#include "data_process_class_dll.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <vector>
using namespace std;
class Track_Die_Tas
{
public:
void track_die_process_tas( QVector <Trust_Track> *trust_track,
void track_die_process_tas( std::vector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output);
};
#endif // TRACK_DIE_TAS_H
data_process_class_dll/track_index_mangement.cpp
+13 -11
View File
@@ -1,13 +1,17 @@
#include "track_index_mangement.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
#include <iostream>
using namespace std;
void Track_Ind_Mangement::reset()
{
lastest_index = 0;
}
int Track_Ind_Mangement ::track_ind_get( QVector<Trust_Track> *trust_track)
int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track)
{
int isempty = 1;
@@ -17,8 +21,6 @@ int Track_Ind_Mangement ::track_ind_get( QVector<Trust_Track> *trust_track)
isempty = 0;
}
if (isempty == 1)
{
lastest_index = 1;
@@ -30,7 +32,9 @@ int Track_Ind_Mangement ::track_ind_get( QVector<Trust_Track> *trust_track)
int List[MAX_TRACK_INDEX]={0};
for (int i=0;i<trust_track->size();i++)
{
List[(*trust_track)[i].Track_Index-1]=1;
int ti = (*trust_track)[i].Track_Index;
if (ti >= 1 && ti <= MAX_TRACK_INDEX)
List[ti-1]=1;
}
//分配航迹号
@@ -45,7 +49,7 @@ int Track_Ind_Mangement ::track_ind_get( QVector<Trust_Track> *trust_track)
}
}
for (int i=1;i<MAX_TRACK_INDEX;i++)
for (int i=1;i<=MAX_TRACK_INDEX;i++)
{
if(List[i-1]==0)
{
@@ -54,11 +58,10 @@ int Track_Ind_Mangement ::track_ind_get( QVector<Trust_Track> *trust_track)
}
}
}
else if(lastest_index == MAX_TRACK_INDEX)
{
for (int i=1;i<MAX_TRACK_INDEX;i++)
for (int i=1;i<=MAX_TRACK_INDEX;i++)
{
if(List[i-1]==0)
{
@@ -70,6 +73,5 @@ int Track_Ind_Mangement ::track_ind_get( QVector<Trust_Track> *trust_track)
}
return 0;
}
data_process_class_dll/track_index_mangement.h
+4 -6
View File
@@ -3,21 +3,19 @@
#include "data_process_class_dll.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <vector>
using namespace std;
class Track_Ind_Mangement
{
public:
int track_ind_get( QVector<Trust_Track> *trust_track);
int track_ind_get( std::vector<Trust_Track> *trust_track);
void reset();
private:
int lastest_index;
int lastest_index = 0;
};
#endif // TRACK_INDEX_MANGEMENT_H
data_process_class_dll/track_init.cpp
+75 -112
View File
@@ -1,17 +1,16 @@
#include "track_init.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
#include <iostream>
#include<iomanip>
using namespace std;
int Track_Init::track_init_process_logic( QVector <PointRecv> *point_recv, //输入点迹
QVector<Trust_Track> *trust_track, //可靠航迹
QVector <QVector<Temp_track>> *temp_track,
int Track_Init::track_init_process_logic( std::vector <PointRecv> *point_recv, //输入点迹
std::vector<Trust_Track> *trust_track, //可靠航迹
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter
@@ -25,7 +24,7 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
// || (*point_recv)[i].CPI_Time == 30682132 || (*point_recv)[i].CPI_Time == 30688692
// || (*point_recv)[i].CPI_Time == 30691692 || (*point_recv)[i].CPI_Time == 30691880)
// {
// qDebug() << "azi=" << (*point_recv)[i].Azimuth
// std::cout << "azi=" << (*point_recv)[i].Azimuth
// << "dis=" << (*point_recv)[i].Range
// << "h=" << (*point_recv)[i].Height
// << "v=" << (*point_recv)[i].Velocity
@@ -34,13 +33,10 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
// }
// }
//取出待起航的点迹数据
for (int i=0;i<(*point_recv).size();i++)
point_process.push_back((*point_recv)[i]);
//临时航迹的buff_round+1
for (int i=0;i<temp_track->size();i++)
{
@@ -59,7 +55,7 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
point_track_head_asso(temp_track,Work_Parameter);
//删除关联上的点迹
QVector <PointRecv>::iterator Iter;
std::vector <PointRecv>::iterator Iter;
for (Iter=point_process.begin(); Iter!=point_process.end();)
{
if((*Iter).Use_Flag==1)
@@ -74,15 +70,14 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
}
//剩余点重新存入点迹
QVector<PointRecv>().swap((*point_recv));
std::vector<PointRecv>().swap((*point_recv));
for (int i=0;i<point_process.size();i++)
(*point_recv).push_back(point_process[i]);
//剩余点转为航迹头
for (unsigned int i=0;i<point_process.size();i++)
{
Temp_track temp_track_tmp;
Temp_track temp_track_tmp = {};
temp_track_tmp.r = point_process[i].Range;
temp_track_tmp.azi = point_process[i].Azimuth;
temp_track_tmp.height = point_process[i].Height;
@@ -102,14 +97,13 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
temp_track_tmp.GNSS_time = point_process[i].GNSS_time;
temp_track_tmp.buff_round = 1;
// temp_track_tmp.Temp_track_section_idx = floor(point_process[i].beam_index/BEAM_NUM_DOT_SECTION)+1;
temp_track->push_back( QVector <Temp_track> ());
temp_track->push_back( std::vector <Temp_track> ());
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)
// {
// qDebug() << "point_process_azi=" << point_process[i].Azimuth
// std::cout << "point_process_azi=" << point_process[i].Azimuth
// << "point_process_dis=" << point_process[i].Range
// << "point_process_h=" << point_process[i].Height
// << "point_process_v=" << point_process[i].Velocity
@@ -120,28 +114,23 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
}
//清空点迹
QVector<PointRecv>().swap(point_process);
QVector<PointRecv>().swap((*point_recv));
std::vector<PointRecv>().swap(point_process);
std::vector<PointRecv>().swap((*point_recv));
//临时航迹满足起始长度 转为可靠航迹
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
}
//消亡临时航迹
tmp_track_die(temp_track);
// qDebug() << "-----------------------------------------------";
// std::cout << "-----------------------------------------------";
// for(int i=0;i<temp_track->size();i++)
// {
// for(int j=0;j<(*temp_track)[i].size();j++)
// {
// qDebug() << "temp_track_azi=" << (*temp_track)[i][j].azi
// std::cout << "temp_track_azi=" << (*temp_track)[i][j].azi
// << "temp_track_dis=" << (*temp_track)[i][j].r
// << "temp_track_h=" << (*temp_track)[i][j].height
// << "temp_track_v=" << (*temp_track)[i][j].vr
@@ -149,18 +138,14 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
// }
// }
return 0;
}
void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_track,
void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
{
//关联信息
struct Asso_info
{
@@ -168,7 +153,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
int point_idx;
double d;
};
QVector <Asso_info> asso_info;
std::vector <Asso_info> asso_info;
for ( int i=0;i<point_process.size();i++)
{
@@ -219,11 +204,9 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
double y2=Z[0]*sin(Z[1]);
double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2);
// if(T_track_head == 33855816 && T_point == 33859004 )
// {
// qDebug() << "r_point=" << r_point
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "r_temp_track=" << r_temp_track
@@ -235,9 +218,6 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
// }
if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha<ALPHA_START && vr_point*v_temp_track>0)// && 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;
@@ -247,7 +227,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][L-1].asso_flag = 1;
point_process[i].Use_Flag = 1;
//qDebug() << "v_temp_track : " << v_temp_track << "vr_point : "<< vr_point;
//std::cout << "v_temp_track : " << v_temp_track << "vr_point : "<< vr_point;
}
}
}
@@ -257,7 +237,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(QVector <Temp_track> ());
(*temp_track).push_back(std::vector <Temp_track> ());
//前L个点
int L = (*temp_track)[asso_info[i].track_idx-1].size();
@@ -268,7 +248,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
}
//关联上的点
Temp_track asso_track_info_tmp;
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;
@@ -300,9 +280,8 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
}
//temp_track中删除关联上的临时航迹
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
if((*Iter).size()>=2)
@@ -324,11 +303,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
}
}
void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_track,
void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
{
@@ -338,8 +313,7 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
int track_idx;
int point_idx;
};
QVector <Asso_info> asso_info;
std::vector <Asso_info> asso_info;
//关联
for ( int i=0;i<point_process.size();i++)
@@ -351,7 +325,6 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
{
//点迹信息
double x_point, y_point, vr_point;
coor_trans Coor_trans;
@@ -371,7 +344,7 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
// if(T_track_head == 30675948)
// {
// qDebug() << "r_point=" << r_point
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
@@ -382,8 +355,8 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
// if(T_point == 30682320)
// {
// qDebug() <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
// qDebug() << "r_point=" << r_point
// std::cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
@@ -392,7 +365,6 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
// << "T_track_head=" << T_track_head;
// }
//距离差
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
@@ -408,8 +380,6 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
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) )
// && 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
@@ -422,7 +392,7 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
// if(T_track_head == 30675948)
// {
// qDebug() << "r_point=" << r_point
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
@@ -437,27 +407,24 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][0].asso_flag = 1;
point_process[i].Use_Flag = 1;
//qDebug() << "v_track_head: " << v_track_head << "vr_point: " << vr_point;
//std::cout << "v_track_head: " << v_track_head << "vr_point: " << vr_point;
}
}
}
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(QVector <Temp_track> ());
(*temp_track).push_back(std::vector <Temp_track> ());
//第一个点
(*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;
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;
@@ -489,10 +456,8 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
}
//temp_track中删除关联上的航迹头
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
if((*Iter)[0].asso_flag==1)
@@ -509,8 +474,6 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
}
////////////////////////////////////////////计算三点间的夹角////////////////////////////////////////////////
double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2)
{
@@ -520,23 +483,19 @@ double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,
return alpha;
}
void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
QVector <QVector<Temp_track>> *temp_track,
void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_track,
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
{
QVector <QVector<Temp_track>> Track_to_start;
std::vector <std::vector<Temp_track>> Track_to_start;
// 1. 将temp_track中满足条件的航迹取出, 放到Track_to_start中
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
int L=(*Iter).size();
@@ -556,7 +515,7 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
if( ( L==Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0 && snr_flag) //按长度查找TRUST_TRACK_POINT
{
//加入Track_to_start中
Track_to_start.push_back(QVector <Temp_track> ());
Track_to_start.push_back(std::vector <Temp_track> ());
for (int i = 0; i<L; i++)
{
Track_to_start[Track_to_start.size()-1].push_back((*Iter)[i]);
@@ -570,16 +529,21 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
}
}
if (Track_to_start.empty())
return;
//2.两两比较Track_to_start中的航迹信息，删除重复的航迹
int L = Work_Parameter.track_start_point_num;
for (int i=0; i<Track_to_start.size()-1;i++)
for (size_t i=0; i+1<Track_to_start.size();i++)
{
if(Track_to_start[i][0].asso_flag!=2)
{
for (int j=i+1;j<Track_to_start.size();j++)
for (size_t j=i+1;j<Track_to_start.size();j++)
{
if(Track_to_start[j][0].asso_flag!=2)
{
if (Track_to_start[i].size() < 3 || Track_to_start[j].size() < 3)
continue;
if( (Track_to_start[i][0].T == Track_to_start[j][0].T && Track_to_start[i][0].r == Track_to_start[j][0].r)
|| (Track_to_start[i][1].T == Track_to_start[j][1].T && Track_to_start[i][1].r == Track_to_start[j][1].r)
@@ -595,17 +559,14 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
Track_to_start[i][0].asso_flag=2;
}
}
}
}
}
}
QVector <QVector<Temp_track>>::iterator Iter1;
std::vector <std::vector<Temp_track>>::iterator Iter1;
for (Iter1=Track_to_start.begin(); Iter1!=Track_to_start.end();)
{
if( (*Iter1)[0].asso_flag == 2 )
@@ -619,15 +580,10 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
}
}
//3.Track_to_start中剩余的航迹起始为可靠航迹
for (int i=0; i<Track_to_start.size();i++)
for (size_t i=0; i<Track_to_start.size();i++)
{
if (Track_to_start[i].size() < 3) continue;
if(trust_track->size()<MAX_TRACK_INDEX)
{
int index = track_index_mangement.track_ind_get(trust_track);
@@ -645,7 +601,7 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
kalman Kalman;
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
Trust_Track trust_track_tmp;
Trust_Track trust_track_tmp = {};
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
@@ -684,7 +640,6 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
(*trust_track).push_back(trust_track_tmp);
//输出航迹更新信息
*Trust_track_num_Output=*Trust_track_num_Output+1;
for (int j=0;j<L;j++)
@@ -694,14 +649,18 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=Track_to_start[i][j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=Track_to_start[i][j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/PI*180;
{
double elev_ratio = (Track_to_start[i][j].r > 0.0) ? Track_to_start[i][j].height / Track_to_start[i][j].r : 0.0;
if (elev_ratio > 1.0) elev_ratio = 1.0;
if (elev_ratio < -1.0) elev_ratio = -1.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin(elev_ratio)/PI*180;
}
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=sqrt(pow(Track_to_start[i][j].X[1],2)+pow(Track_to_start[i][j].X[3],2));
Trust_Track_Output[*Trust_track_num_Output-1][j].z=Track_to_start[i][j].height;
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=Track_to_start[i][j].Amp;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = Track_to_start[i][j].snr;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = Track_to_start[i][j].RCS;
Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim, Track_to_start[i][j].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim));
@@ -712,8 +671,6 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
Direction_Angle=Direction_Angle+2*PI;
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=Direction_Angle/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=(Track_to_start[i][j].T)/1000.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].GNSS_time=Track_to_start[i][j].GNSS_time;
@@ -724,7 +681,12 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=Track_to_start[i][j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=Track_to_start[i][j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/PI*180;
{
double elev_ratio = (Track_to_start[i][j].r > 0.0) ? Track_to_start[i][j].height / Track_to_start[i][j].r : 0.0;
if (elev_ratio > 1.0) elev_ratio = 1.0;
if (elev_ratio < -1.0) elev_ratio = -1.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin(elev_ratio)/PI*180;
}
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=Track_to_start[i][j].vr;
Trust_Track_Output[*Trust_track_num_Output-1][j].point_type=0;
Trust_Track_Output[*Trust_track_num_Output-1][j].Flag_Point=1;
@@ -735,20 +697,23 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
}
}
QVector <QVector<Temp_track>>().swap(Track_to_start);
std::vector <std::vector<Temp_track>>().swap(Track_to_start);
}
void Track_Init::tmp_track_die(QVector <QVector<Temp_track>> *temp_track)
void Track_Init::tmp_track_die(std::vector <std::vector<Temp_track>> *temp_track)
{
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
int n=(*Iter).size();
if (n <= 0)
{
temp_track->erase(Iter);
Iter=temp_track->begin();
continue;
}
if( (*Iter)[n-1].buff_round >2 || n>=10)
{
@@ -762,13 +727,9 @@ void Track_Init::tmp_track_die(QVector <QVector<Temp_track>> *temp_track)
}
};
int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter)
{
for (int i=0;i<Work_Parameter.track_prohibite_area_num;i++)
{
if(r<Work_Parameter.R_max_track_prohibited[i] && r>Work_Parameter.R_min_track_prohibited[i] && azi<Work_Parameter.Azimuth_max_track_prohibited[i] && azi>Work_Parameter.Azimuth_min_track_prohibited[i])
@@ -779,13 +740,15 @@ int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara
return 0;
}
Track_Init::Track_Init()
{
}
void Track_Init::reset()
{
point_process.clear();
track_index_mangement.reset();
}
data_process_class_dll/track_init.h
+12 -17
View File
@@ -4,49 +4,44 @@
#include "track_index_mangement.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <QDebug>
#include <vector>
using namespace std;
/******************************* 航迹起始类 **************************************************/
class Track_Init
{
public:
//航迹起始逻辑法
int track_init_process_logic( QVector<PointRecv> *point_recv, //输入点迹
QVector<Trust_Track> *trust_track, //可靠航迹
QVector <QVector<Temp_track>> *temp_track,
int track_init_process_logic( std::vector<PointRecv> *point_recv, //输入点迹
std::vector<Trust_Track> *trust_track, //可靠航迹
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter
);
Track_Init();
void reset();
private:
std::vector<PointRecv> point_process; //要处理的点迹
QVector<PointRecv> point_process; //要处理的点迹
void point_temp_track_asso(QVector <QVector<Temp_track>> *temp_track,
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter); //临时航迹与点迹关联
void point_track_head_asso( QVector <QVector<Temp_track>> *temp_track,
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter); //航迹头与点迹关联
void tmp_track_to_trust_track( QVector <Trust_Track> *trust_track, //临时航迹转可靠航迹
QVector <QVector<Temp_track>> *temp_track,
void tmp_track_to_trust_track( std::vector <Trust_Track> *trust_track, //临时航迹转可靠航迹
std::vector <std::vector<Temp_track>> *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 <QVector<Temp_track>> *temp_track);//临时航迹消亡
void tmp_track_die(std::vector <std::vector<Temp_track>> *temp_track);//临时航迹消亡
double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角
data_process_class_dll/track_init_direct_tracking.cpp
+37 -48
View File
@@ -1,16 +1,16 @@
#include "track_init_direct_tracking.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
#include <iostream>
#include<iomanip>
using namespace std;
int Track_Init_Direct_Tracking::track_init_process_logic( QVector<PointRecv> *point_recv, //输入点迹
QVector<Trust_Track> *trust_track, //可靠航迹
QVector <QVector<Temp_track>> *temp_track,
int Track_Init_Direct_Tracking::track_init_process_logic( std::vector<PointRecv> *point_recv, //输入点迹（当前接口未启用，保留空实现）
std::vector<Trust_Track> *trust_track, //可靠航迹
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter,
@@ -18,14 +18,11 @@ int Track_Init_Direct_Tracking::track_init_process_logic( QVector<PointRecv>
)
{
return 0;
}
void Track_Init_Direct_Tracking::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_track,
void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter) //临时航迹与点迹关联
{
//关联信息
@@ -34,7 +31,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector <QVector<Temp_tra
int track_idx;
int point_idx;
};
QVector <Asso_info> asso_info;
std::vector <Asso_info> asso_info;
for ( int i=0;i<point_process.size();i++)
{
@@ -67,7 +64,6 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector <QVector<Temp_tra
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;
if( fabs(r_point-r_temp_track)>=5 )
@@ -86,7 +82,6 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector <QVector<Temp_tra
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 && alpha<ALPHA_START && vr_point*v_temp_track>0)// && 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;
@@ -101,12 +96,10 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector <QVector<Temp_tra
}
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(QVector <Temp_track> ());
(*temp_track).push_back(std::vector <Temp_track> ());
//前L个点
int L = (*temp_track)[asso_info[i].track_idx-1].size();
@@ -117,7 +110,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector <QVector<Temp_tra
}
//关联上的点
Temp_track asso_track_info_tmp;
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;
@@ -147,9 +140,8 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector <QVector<Temp_tra
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
}
//temp_track中删除关联上的临时航迹
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
if((*Iter).size()>=2)
@@ -171,7 +163,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector <QVector<Temp_tra
}
}
void Track_Init_Direct_Tracking::point_track_head_asso( QVector <QVector<Temp_track>> *temp_track,
void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter) //航迹头与点迹关联
{
//关联上的信息
@@ -180,8 +172,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector <QVector<Temp_tr
int track_idx;
int point_idx;
};
QVector <Asso_info> asso_info;
std::vector <Asso_info> asso_info;
//关联
for ( int i=0;i<point_process.size();i++)
@@ -193,7 +184,6 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector <QVector<Temp_tr
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
{
//点迹信息
double x_point, y_point, vr_point;
coor_trans Coor_trans;
@@ -211,7 +201,6 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector <QVector<Temp_tr
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));
@@ -231,8 +220,6 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector <QVector<Temp_tr
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
@@ -245,24 +232,21 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector <QVector<Temp_tr
point_process[i].Use_Flag = 1;
}
}
}
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(QVector <Temp_track> ());
(*temp_track).push_back(std::vector <Temp_track> ());
//第一个点
(*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;
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;
@@ -293,10 +277,8 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector <QVector<Temp_tr
}
//temp_track中删除关联上的航迹头
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
if((*Iter)[0].asso_flag==1)
@@ -313,20 +295,23 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector <QVector<Temp_tr
}
void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector<Trust_Track> *trust_track, //临时航迹转可靠航迹
QVector <QVector<Temp_track>> *temp_track,
void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Track> *trust_track, //临时航迹转可靠航迹
std::vector <std::vector<Temp_track>> *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 <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
int L=(*Iter).size();
if (L < 3)
{
++Iter;
continue;
}
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
@@ -342,7 +327,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector<Trust_Track>
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;
Trust_Track trust_track_tmp = {};
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
@@ -426,17 +411,21 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector<Trust_Track>
}
}
}
void Track_Init_Direct_Tracking::tmp_track_die(QVector <QVector<Temp_track>> *temp_track)
void Track_Init_Direct_Tracking::tmp_track_die(std::vector <std::vector<Temp_track>> *temp_track)
{
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
int n=(*Iter).size();
if (n <= 0)
{
temp_track->erase(Iter);
Iter=temp_track->begin();
continue;
}
if( (*Iter)[n-1].buff_round >1 || n>=10)
{
temp_track->erase(Iter);
@@ -449,9 +438,6 @@ void Track_Init_Direct_Tracking::tmp_track_die(QVector <QVector<Temp_track>>
}
}
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};
@@ -460,11 +446,14 @@ double Track_Init_Direct_Tracking::alpha_cal_track_init(double x0,double y0,doub
return alpha;
}
Track_Init_Direct_Tracking::Track_Init_Direct_Tracking()
{
}
void Track_Init_Direct_Tracking::reset()
{
point_process.clear();
}
data_process_class_dll/track_init_direct_tracking.h
+11 -17
View File
@@ -3,16 +3,15 @@
#include "data_process_class_dll.h"
#include "parameters.h"
#include "struct.h"
#include <QVector>
#include <vector>
class Track_Init_Direct_Tracking
{
public:
//航迹起始逻辑法
int track_init_process_logic( QVector<PointRecv> *point_recv, //输入点迹
QVector<Trust_Track> *trust_track, //可靠航迹
QVector <QVector<Temp_track>> *temp_track,
int track_init_process_logic( std::vector<PointRecv> *point_recv, //输入点迹
std::vector<Trust_Track> *trust_track, //可靠航迹
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter,
@@ -20,35 +19,30 @@ public:
);
Track_Init_Direct_Tracking();
void reset();
private:
std::vector<PointRecv> point_process; //要处理的点迹
QVector<PointRecv> point_process; //要处理的点迹
void point_temp_track_asso(QVector <QVector<Temp_track>> *temp_track,
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter); //临时航迹与点迹关联
void point_track_head_asso( QVector <QVector<Temp_track>> *temp_track,
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter); //航迹头与点迹关联
void tmp_track_to_trust_track( QVector<Trust_Track> *trust_track, //临时航迹转可靠航迹
QVector <QVector<Temp_track>> *temp_track,
void tmp_track_to_trust_track( std::vector<Trust_Track> *trust_track, //临时航迹转可靠航迹
std::vector <std::vector<Temp_track>> *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 <QVector<Temp_track>> *temp_track);//临时航迹消亡
void tmp_track_die(std::vector <std::vector<Temp_track>> *temp_track);//临时航迹消亡
double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角
};
#endif // TRACK_INIT_DIRECT_TRACKING_H