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

更新：1、修改参数配置方式，改为由配置文件输入；

Browse Source 
Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-10-27 10:36:14 +08:00
parent 0e325fb153
commit 423c200659
39 changed files with 19638 additions and 945 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/data_process.cpp
+22 -23
View File
@@ -1,7 +1,9 @@
#include "data_process.h"
#include "rdp_log.h"
#include "rdp_record.h"
#include "rdp_parameter_loader.h"
#include <cstring>
#include <string>
#include <vector>
#include <cmath>
#include <iostream>
@@ -182,7 +184,7 @@ int model)
if(model==2)
{
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,Work_Parameter);
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,rdp_parameter);
std::vector<PointRecv>().swap(Data_buffer);
}
@@ -196,19 +198,20 @@ int model)
Trust_Track_Output,
Trust_track_num_Output,
latest_timestamp,
Work_Parameter
rdp_parameter
);
track_die.track_die_process( &trust_track,
Track_die_Index_Output,
Track_die_num_Output);
Track_die_num_Output,
rdp_parameter);
track_init.track_init_process_logic( &point_recv,
&trust_track,
&temp_track,
Trust_Track_Output,
Trust_track_num_Output,
Work_Parameter);
rdp_parameter);
}
@@ -218,7 +221,7 @@ int model)
*Trust_track_num_Output=0; //航迹更新数
*Track_die_num_Output=0; //航迹消亡数目
dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas); //点迹凝聚
dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas,rdp_parameter); //点迹凝聚
std::vector<PointRecv>().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer
@@ -226,7 +229,7 @@ int model)
&trust_track, //航迹文件
Trust_Track_Output, //更新航迹信息
Trust_track_num_Output, //更新航迹数
Work_Parameter, //工作参数
rdp_parameter, //工作参数
TAS_track_idx,
latest_timestamp //最新时间戳
);
@@ -235,7 +238,8 @@ int model)
track_die_tas.track_die_process_tas( &trust_track,
Track_die_Index_Output,
Track_die_num_Output);
Track_die_num_Output,
rdp_parameter);
//std::cout << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
}
@@ -249,27 +253,22 @@ void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam,
int *Trust_track_num_Output)
{
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,Work_Parameter, latest_timestamp);
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,rdp_parameter, latest_timestamp);
}
//数据处理参数初始化
int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parameter)
//数据处理参数加载（从 TOML 配置文件加载全部工作参数）
int Data_Process::parameters_reload(const char* parameter_file_path)
{
//工作参数设置
if (Radar_Parameter.track_start_point_num < 3 || Radar_Parameter.track_start_point_num > 9)
RDP_parameter loaded{};
std::string error;
if (rdp_parameter_load(parameter_file_path, loaded, &error) != 0)
{
RDP_LOG << "parameters_reload failed: " << error << std::endl;
return -1;
memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara));
}
return 0;
}
//数据处理参数修改
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));
rdp_parameter = loaded;
RDP_LOG << "parameters_reload ok: " << parameter_file_path << std::endl;
return 0;
}
data_process_class_dll/data_process.h
+6 -7
View File
@@ -68,11 +68,10 @@ public:
//手动打跟踪波束
int tracking_point(float Azimuth); //方位角
//参数设置初始化
int track_process_parameters_initial(struct RadarPara Radar_Parameter);
//参数设置修改函数
int track_process_parameters_modify(struct RadarPara Radar_Parameter);
//参数加载函数（从 TOML 配置文件加载全部工作参数到 rdp_parameter）
//参数：parameter_file_path - 配置文件路径
//返回：成功返回 0；失败返回 -1（失败时不改变当前参数）
int parameters_reload(const char* parameter_file_path) override;
private:
@@ -84,6 +83,8 @@ private:
std::vector <Trust_Track> trust_track; //可靠航迹 按航迹区存入
std::vector <std::vector<Temp_track>> temp_track; //临时航迹 按临时航迹区存入
RDP_parameter rdp_parameter{}; //工作参数 由 parameters_reload() 从配置文件加载
int Beam_num; //波位数
int beam_count; //波位计数
int data_num; //波位1点数
@@ -104,8 +105,6 @@ private:
long long circle_id = 0; //圈id
struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入
Dot_Coh dot_coh; //点迹凝聚
Dot_Coh_TAS dot_coh_tas;
data_process_class_dll/data_process_class_dll.h
+5 -73
View File
@@ -109,70 +109,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Target_direct_tracking
int open_flag; // 1 引导跟踪 0 结束引导跟踪
};
// 雷达参数结构体
struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
{
int Beam_num; //波位数
float Height; //雷达平台高度 （m）
float Sys_delay; //系统延迟 (s)
float R_TAS_max; //TAS最大距离 （m）
float R_TAS_min; //TAS最小距离 (m)
float V_TAS_max; //TAS最大速度 (m/s)
float V_TAS_min; //TAS最小速度 (m/s)
float TAS_Height_1; //TAS跟踪高度1 (m)
float TAS_Height_2; //TAS跟踪高度2 (m)
float TAS_Height_3; //TAS跟踪高度3 (m)
float TAS_Height_4; //TAS跟踪高度4 (m)
float TAS_Height_5; //TAS跟踪高度5 (m)
int track_prohibite_area_num; //禁止航迹起始的区域数目
float R_max_track_prohibited[30]; //最大距离 (m)
float R_min_track_prohibited[30]; //最小距离 (m)
float Azimuth_max_track_prohibited[30]; //最大方位角 (度)
float Azimuth_min_track_prohibited[30]; //最小方位角 （度）
int TAS_prohibite_area_num; //禁止TAS的区域数目
float R_max_TAS_prohibited[30]; //最大距离 （m）
float R_min_TAS_prohibited[30]; //最小距离 (m)
float Azimuth_max_TAS_prohibited[30]; //最大方位角 (度)
float Azimuth_min_TAS_prohibited[30]; //最小方位角 (度)
int cfar_th; //cfar门限
int work_mode; //工作模式 0进程 1中程 2远程
int north_angle; //北偏角
float V_MAX; //最大速度
float V_MIN; //最小速度
float DATA_RATE_SHORT; //三种不同模式下的 数据率
float DATA_RATE_MIDDLE;
float DATA_RATE_FAR;
float DATA_RATE_TAS; //TAS模式数据率，由dll调用者输入；TAS波束输出间隔阈值为 DATA_RATE_TAS 秒
//数据关联参数
int track_start_point_num; //起批点数(典型值 3或4)
float track_start_threshold; //起航波门大小(典型值 3)
float track_asso_threshold; //关联波门大小(典型值 3)
float track_asso_threshold_tas; //关联波门大小tas (典型值 3)
//singer模型机动参数
float Model1_Q_fast; //0.00001
float Model1_Q_slow; //0.00001
float Model2_Q_fast; //0.1
float Model2_Q_slow; //0.001
float Model3_Q_fast; //0.8
float Model3_Q_slow; //0.4
};
class DATA_PROCESS_CLASS_DLLSHARED_EXPORT Data_process_class_dll
{
@@ -261,16 +197,12 @@ public:
return 0;
}
//参数设置初始化函数
virtual int track_process_parameters_initial(struct RadarPara Radar_Parameter)
//数据处理参数加载函数（从 TOML 配置文件加载全部工作参数）
//参数：parameter_file_path - 配置文件路径（宿主进程当前工作目录可达的路径）
//返回：成功返回 0；失败返回 -1（失败时不改变当前参数）
virtual int parameters_reload(const char*)
{
return 0;
}
//参数设置修改函数
virtual int track_process_parameters_modify(struct RadarPara Radar_Parameter)
{
return 0;
return -1;
}
};
data_process_class_dll/dot_coh.cpp
+14 -14
View File
@@ -8,7 +8,7 @@ using namespace std;
int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
std::vector <PointRecv> *point_recv,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
if(data_input->size()>1)
@@ -32,10 +32,10 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
float point_1_A=static_cast<float>((*data_input)[i].amplitude);
//凝聚条件： 距离、方位接近
if(Work_Parameter.work_mode == 0)
if(rdp_parameter.radar_work_mode == 0)
{
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)//
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
&& point_0_A <point_1_A )//
{
point_0_R=static_cast<float>((*data_input)[i].range);
@@ -44,7 +44,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
point_0_F=static_cast<float>((*data_input)[i].azimuth);
(*data_input)[loop_of_point].use_flag=1;
}
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)//
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
&& point_0_A >= point_1_A)//
{
(*data_input)[i].use_flag=1;
@@ -53,7 +53,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
else
{
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
&& point_0_A <point_1_A )//
{
point_0_R=static_cast<float>((*data_input)[i].range);
@@ -62,7 +62,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
point_0_F=static_cast<float>((*data_input)[i].azimuth);
(*data_input)[loop_of_point].use_flag=1;
}
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
&& point_0_A >= point_1_A)//
{
(*data_input)[i].use_flag=1;
@@ -79,7 +79,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
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 )
if((*Iter).use_flag==1 || (*Iter).range <rdp_parameter.point_cohesion_range_min || (*Iter).range > rdp_parameter.point_cohesion_range_max )
{
data_input->erase(Iter);
Iter=data_input->begin();
@@ -100,7 +100,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input, //输入的点迹
std::vector <PointRecv> *point_recv, //输出点迹
struct RadarPara Work_Parameter //工作参数
const RDP_parameter& rdp_parameter //工作参数
)
{
@@ -148,10 +148,10 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
float point_1_A=static_cast<float>(data_tmp[i].amplitude);
//凝聚条件： 距离、方位接近
if(Work_Parameter.work_mode == 0)
if(rdp_parameter.radar_work_mode == 0)
{
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)//
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
&& point_0_A <point_1_A )//
{
point_0_R=static_cast<float>(data_tmp[i].range);
@@ -160,7 +160,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
point_0_F=static_cast<float>(data_tmp[i].azimuth);
data_tmp[loop_of_point].use_flag=1;
}
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)//
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
&& point_0_A >= point_1_A)//
{
data_tmp[i].use_flag=1;
@@ -169,7 +169,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
else
{
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
&& point_0_A <point_1_A )//
{
point_0_R=static_cast<float>(data_tmp[i].range);
@@ -178,7 +178,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
point_0_F=static_cast<float>(data_tmp[i].azimuth);
data_tmp[loop_of_point].use_flag=1;
}
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
&& point_0_A >= point_1_A)//
{
data_tmp[i].use_flag=1;
@@ -195,7 +195,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
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 )
if((*Iter).use_flag==1 || (*Iter).range <rdp_parameter.point_cohesion_range_min || (*Iter).range > rdp_parameter.point_cohesion_range_max )
{
data_tmp.erase(Iter);
Iter=data_tmp.begin();
data_process_class_dll/dot_coh.h
+2 -2
View File
@@ -15,13 +15,13 @@ public:
// 点迹凝聚函数
int dot_coh_process( std::vector <PointRecv> *data_input, //输入的点迹
std::vector <PointRecv> *point_recv, //输出点迹
struct RadarPara Work_Parameter //工作参数
const RDP_parameter& rdp_parameter //工作参数
);
//点迹凝聚函数，缓存一帧，一边输出，一边进行滑窗凝聚
int dot_coh_process_buff( std::vector <PointRecv> *data_input, //输入的点迹
std::vector <PointRecv> *point_recv, //输出点迹
struct RadarPara Work_Parameter //工作参数
const RDP_parameter& rdp_parameter //工作参数
);
//构造函数
data_process_class_dll/dot_coh_tas.cpp
+5 -5
View File
@@ -6,8 +6,8 @@
using namespace std;
int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *data_input, //输入点迹
std::vector <PointRecv> *point_recv_tas //输出点迹
)
std::vector <PointRecv> *point_recv_tas, //输出点迹
const RDP_parameter& rdp_parameter)
{
if (data_input->size() == 0) {
return 1;
@@ -32,7 +32,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *dat
//凝聚条件： 距离、方位接近
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)
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)
&& point_0_A <point_1_A )
{
point_0_R=static_cast<float>((*data_input)[i].range);
@@ -41,7 +41,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *dat
point_0_F=static_cast<float>((*data_input)[i].azimuth);
(*data_input)[loop_of_point].use_flag=1;
}
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)
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)
&& point_0_A >= point_1_A)
{
(*data_input)[i].use_flag=1;
@@ -55,7 +55,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *dat
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 )
if((*Iter).use_flag==1 || (*Iter).range <rdp_parameter.point_cohesion_range_min || (*Iter).range > rdp_parameter.point_cohesion_range_max )
{
data_input->erase(Iter);
Iter=data_input->begin();
data_process_class_dll/dot_coh_tas.h
+2 -2
View File
@@ -9,8 +9,8 @@ public:
// 点迹凝聚函数
int dot_coh_tas_process(std::vector <PointRecv> *data_input, //输入点迹
std::vector <PointRecv> *point_recv_tas //输出点迹
);
std::vector <PointRecv> *point_recv_tas, //输出点迹
const RDP_parameter& rdp_parameter);
};
#endif // DOT_COH_TAS_H
data_process_class_dll/kalman.cpp
+69 -69
View File
@@ -15,7 +15,7 @@ static double wrapAnglePi(double a)
}
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4])
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4], const RDP_parameter& rdp_parameter)
{
X[0]=Z1[0];
@@ -26,13 +26,13 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z1[0],Z1[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
double R[2][2];
R[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R[1][0]=R[0][1];
P[0][0]=R[0][0];
@@ -57,7 +57,7 @@ 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)
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T, const RDP_parameter& rdp_parameter)
{
//X(k+1|k)
@@ -92,13 +92,13 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix2d R;
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
Matrix4d P_present;
@@ -122,7 +122,7 @@ 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])
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], const RDP_parameter& rdp_parameter)
{
double x0=Z0[0];
double y0=Z0[1];
@@ -145,23 +145,23 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z0[0],Z0[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
R0[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R0[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R0[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
R0[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R0[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R0[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*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[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R1[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R1[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R1[1][0]=R1[0][1];
Coor_trans.cart2polar(Z2[0],Z2[1],&rho,&theta);
R2[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R2[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R2[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R2[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R2[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R2[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R2[1][0]=R2[0][1];
double P11[3][3]={{R2[0][0], R2[0][0]/T2, (R2[0][0]/T2)/((T2+T1)/2)} ,
@@ -193,7 +193,7 @@ 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])
double kalman::d_cal(double Z[2],double X[6],double P[6][6], const RDP_parameter& rdp_parameter)
{
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
@@ -218,13 +218,13 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6])
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix2d R;
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
//S
@@ -240,7 +240,7 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6])
};
double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind)
double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind, const RDP_parameter& rdp_parameter)
{
MatrixXd F1(6,6);
@@ -284,9 +284,9 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
H(2,3) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,4)=-y/sqrt(x*x+y*y); H(2,5)=0;
Matrix3d R;
R(0,0)=SIGMA_R*SIGMA_R;
R(1,1)=SIGMA_A*SIGMA_A;
R(2,2)=SIGMA_V*SIGMA_V;
R(0,0)=rdp_parameter.sigma_range*rdp_parameter.sigma_range;
R(1,1)=rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(0,1)=0;
R(1,0)=0;
R(2,0)=0;
@@ -299,7 +299,7 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
S=H*P_pred*H.transpose()+R;
//bind_speed
double v_bind=Bind_speed(prt, freq_ind);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//d
Vector3d Z_mea(Z[0],Z[1],Z[2]);
@@ -313,7 +313,7 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
}
double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind)
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, const RDP_parameter& rdp_parameter)
{
MatrixXd F1(6,6);
@@ -362,15 +362,15 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix3d R;
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
R(2,2)=SIGMA_V*SIGMA_V;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(2,0)=0;
R(2,1)=0;
R(0,2)=0;
@@ -381,7 +381,7 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
S=H*P_pred*H.transpose()+R;
// bind_speed
double v_bind=Bind_speed(prt, freq_ind);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//d
Vector3d delta_z;
@@ -431,7 +431,7 @@ void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6]
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[3][3],
double prt,double freq_ind )
double prt,double freq_ind, const RDP_parameter& rdp_parameter )
{
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
@@ -477,9 +477,9 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
H(2,3) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,4)=-y/sqrt(x*x+y*y); H(2,5)=0;
Matrix3d R;
R(0,0)=SIGMA_R*SIGMA_R;
R(1,1)=SIGMA_A*SIGMA_A;
R(2,2)=SIGMA_V*SIGMA_V;
R(0,0)=rdp_parameter.sigma_range*rdp_parameter.sigma_range;
R(1,1)=rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(0,1)=0;
R(1,0)=0;
R(2,0)=0;
@@ -496,7 +496,7 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
K=P_pred*H.transpose()*S.inverse();
//bind_speed
double v_bind=Bind_speed(prt, freq_ind);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//d
Vector3d delta_z;
@@ -530,7 +530,7 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
void kalman::kalman_filter(double F[6][6], double Q[6][6],
double X_cur[6],double P_cur[6][6],
double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2])
double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2], const RDP_parameter& rdp_parameter)
{
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
@@ -570,13 +570,13 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix2d R;
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
//S
@@ -612,7 +612,7 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
};
double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind)
double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind, const RDP_parameter& rdp_parameter)
{
//X(k+1|k)
@@ -663,9 +663,9 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
H(2,2) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,3)=-y/sqrt(x*x+y*y);
Matrix3d R;
R(0,0)=SIGMA_R*SIGMA_R;
R(1,1)=SIGMA_A*SIGMA_A;
R(2,2)=SIGMA_V*SIGMA_V;
R(0,0)=rdp_parameter.sigma_range*rdp_parameter.sigma_range;
R(1,1)=rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(0,1)=0;
R(1,0)=0;
R(2,0)=0;
@@ -678,7 +678,7 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
S=H*P_pred*H.transpose()+R;
//bind_speed
double v_bind=Bind_speed(prt, freq_ind);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//d
Vector3d Z_presnet= Vector3d(Z[0],Z[1],Z[2]);
@@ -692,7 +692,7 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
}
double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind)
double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind, const RDP_parameter& rdp_parameter)
{
//X(k+1|k)
Matrix4d F;
@@ -744,15 +744,15 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
double rho,theta;
coor_trans Coor_trans;
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
Matrix3d R;
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
R(1,0)=R(0,1);
R(2,2)=SIGMA_V*SIGMA_V;
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
R(2,0)=0;
R(2,1)=0;
R(0,2)=0;
@@ -763,7 +763,7 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
S=H*P_pred*H.transpose()+R;
//bind_speed
double v_bind=Bind_speed(prt, freq_ind);
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
//d
Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr);
@@ -777,9 +777,9 @@ 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 kalman::Bind_speed(double prt,double freq_ind, const RDP_parameter& rdp_parameter)
{
double freq=FREQ0+freq_ind*0.02;
double freq=rdp_parameter.freq_0_GHz+freq_ind*0.02;
if (prt <= 0.0 || freq <= 0.0)
return 1.0; // 避免除零；正常流程 PRI 不允许为 0
data_process_class_dll/kalman.h
+11 -11
View File
@@ -13,29 +13,29 @@ public:
void kalman_pred(double F[6][6], double Q[6][6], double X[6], double P[6][6], double X_pred[6], double P_pred[6][6]);
void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4]);//两点初始化卡尔曼滤波器
void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4], const RDP_parameter& rdp_parameter);//两点初始化卡尔曼滤波器
double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T); //计算点 临时航迹的d
double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T, const RDP_parameter& rdp_parameter); //计算点 临时航迹的d
double d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind);
double d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind, const RDP_parameter& rdp_parameter);
double d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind);
double d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind, const RDP_parameter& rdp_parameter);
void kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6]);
void kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6], const RDP_parameter& rdp_parameter);
double d_cal(double Z[2],double X[6],double P[6][6]);
double d_cal(double Z[2],double X[6],double P[6][6], const RDP_parameter& rdp_parameter);
double d_cal_with_doppler(double F[6][6], double Q[6][6] ,double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind);
double d_cal_with_doppler(double F[6][6], double Q[6][6] ,double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind, const RDP_parameter& rdp_parameter);
double d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind);
double d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind, const RDP_parameter& rdp_parameter);
void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2]);
void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2], const RDP_parameter& rdp_parameter);
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[3][3],
double prt,double freq_ind );
double prt,double freq_ind, const RDP_parameter& rdp_parameter );
double Bind_speed(double prt,double freq_ind); //根据PRT 和 频率 计算不模糊速度
double Bind_speed(double prt,double freq_ind, const RDP_parameter& rdp_parameter); //根据PRT 和 频率 计算不模糊速度
};
#endif // KALMAN_H
data_process_class_dll/parameters.h
+7 -127
View File
@@ -4,9 +4,6 @@
/**********************************雷达参数*************************************************/
#define PI 3.1415926f
//频点(GHz)
#define FREQ0 9.2
/**********************************版本信息*************************************************/
#define MAJOR_VERSION 1
#define MINOR_VERSION 5
@@ -90,134 +87,17 @@
#define RDP_RECORD_DROP_LOG_INTERVAL 1024u
#endif
//#define MECHANICAL_SCANNING
//#define PHASE_SCANNING
/******************************通用数据处理参数**************************************/
//航迹质量管理参数
#define TRACK_QUALITY_MAX 100
#define TRACK_QUALITY_MIN 0
#define TRACK_QUALITY_D_WEIGHT 0.5
#define TRACK_QUALITY_SNR_WEIGHT 0.3
#define TRACK_QUALITY_LEN_WEIGHT 0.2
#define TRACK_QUALITY_ITER_COEFF 0.8
#define TRACK_QUALITY_DECREASE_COEFF_O2 -0.0008
#define TRACK_QUALITY_DECREASE_COEFF_O1 -0.0025
//航迹起批质量条件
#define TRACK_START_QUALITY_L3 0.7
#define TRACK_START_QUALITY_L4 0.6
#define TRACK_START_QUALITY_L5 0.5
#define TRACK_START_QUALITY_L6 0.4
#define TRACK_START_QUALITY_L7 0.3
//航迹消批质量条件
#define TRACK_DIE_QUALITY 0.3
#define TRACK_DIE_QUALITY_PRAMARY 0.3
#ifdef MECHANICAL_SCANNING
/******************************机扫数据处理参数**************************************/
// #define DATA_RATE_TAS 0.0625
#define SIGMA_R 10.0 //测量误差
#define SIGMA_A 0.02
#define SIGMA_E 0.2
#define SIGMA_V 2.0
#define DOT_COH_RANGE 80 //点迹凝聚
#define DOT_COH_V 2
#define DOT_COH_AZI 6
#define MAX_BEAM_NUM 100 //最大TWS波位数
/******************************编译期容量常量***********************************************/
// 说明：下列常量是「接口/内存布局」需要的编译期容量，不是雷达工作参数，
// 雷达工作参数统一由 TOML 配置文件经 parameters_reload() 载入 struct.h::RDP_parameter。
//输出航迹数组的容量（宿主接口固定维度，见 data_process_class_dll.h）
#define MAX_TRACK_NUM 500
#define MAX_TRACK_INDEX 500 //最大航迹批号
#define ASSOCIATE_THRESHOLD_MIN 1
#define ASSOCIATE_THRESHOLD_MID 3 //关联波门
#define ASSOCIATE_THRESHOLD_MAX 10
#define R_MIN 100
#define R_MAX 100000
#define TRACK_START_THRESHOLD 8 //起航波门 3 越大越容易起批
#define ALPHA_START 60 //起航夹角 120 越大越容易起批
#define MAX_EXTRAPOLATE_NUM 3 //最大外推点数
#define MAX_EXTRAPOLATE_NUM_TAS 48 //TAS最大外推点数
#define TRACK_DIE_ROUND 6 // 航迹消亡圈数
#define TRACK_DIE_ROUND_PRIMARY 10 // 重点航迹消亡圈数
#define TRACK_DIE_ROUND_TAS 96 //TAS航迹消亡圈数
#define ASSO_THORD 4
#define H_F_WIN_LEN 3
#define TAS_QUEUE_LENGTH 1 //Tas调用的cpi数
#define MAX_TAS_NUM 1
//TAS 跟踪队列容量上限（运行期配置项 tas_track_num_max 不得超过该值）
#define RDP_TAS_QUEUE_CAPACITY 30
//未确认目标类型（Track::Target_Type 的缺省语义值）
#define UNCONF_TARGET 0
#define DIRECT_TRACKING_QUEUE_LENGTH 10
#pragma message("MECHANICAL_SCANNING")
#endif
#ifdef PHASE_SCANNING
/******************************相扫数据处理参数**************************************/
// #define DATA_RATE_TAS 0.3
#define SIGMA_R 10.0 //测量误差
#define SIGMA_A 0.02
#define SIGMA_E 0.2
#define SIGMA_V 2.0
#define DOT_COH_RANGE 80 //点迹凝聚
#define DOT_COH_V 2
#define DOT_COH_AZI 6
#define MAX_BEAM_NUM 100 //最大TWS波位数
#define MAX_TRACK_NUM 500
#define MAX_TRACK_INDEX 500 //最大航迹批号
#define ASSOCIATE_THRESHOLD_MIN 1
#define ASSOCIATE_THRESHOLD_MID 3 //关联波门
#define ASSOCIATE_THRESHOLD_MAX 10
#define R_MIN 100
#define R_MAX 100000
//#define V_MAX 500 //最大速度
//#define V_MIN 1 //速度最小
#define TRACK_START_THRESHOLD 8 //起航波门 3 越大越容易起批
#define ALPHA_START 60 //起航夹角 120 越大越容易起批
#define MAX_EXTRAPOLATE_NUM 3 //最大外推点数
#define MAX_EXTRAPOLATE_NUM_TAS 5 //TAS最大外推点数
#define TRACK_DIE_ROUND 6 // 航迹消亡圈数
#define TRACK_DIE_ROUND_PRIMARY 10 // 重点航迹消亡圈数
#define TRACK_DIE_ROUND_TAS 20 //TAS航迹消亡圈数
#define ASSO_THORD 4
#define H_F_WIN_LEN 6
#define TAS_QUEUE_LENGTH 4 //Tas调用的cpi数
#define MAX_TAS_NUM 4
#define UNCONF_TARGET 0
#define DIRECT_TRACKING_QUEUE_LENGTH 10
#pragma message("PHASE_SCANNING")
#endif
#define Round(x) (((x) > 0) \
? (((int)((x) + 0.5) > (int)(x)) ? ((int)(x) + 1) : ((int)(x))) \
: (((int)((x) - 0.5) < (int)(x)) ? ((int)(x) - 1) : ((int)(x))))
data_process_class_dll/rdp_parameter_loader.cpp
+614
View File
@@ -0,0 +1,614 @@
#include "rdp_parameter_loader.h"
#include "parameters.h"
#include <cfloat>
#include <cmath>
#include <cstdint>
#include <limits>
#include <string>
#include <string_view>
#include <vector>
// toml++ 为纯头文件第三方库（third_party/toml/toml.hpp，v3.4.0）。
// 第三方头文件不参与本工程的告警治理：包含期间临时屏蔽告警（/W3 下仍要求 0 告警）。
#if defined(_MSC_VER)
#pragma warning(push, 0)
#endif
#include <toml.hpp>
#if defined(_MSC_VER)
#pragma warning(pop)
#endif
/************************************************************************************************
* 实现说明
*
* 配置文件结构与 RDP_parameter 一一对应：
* [radar_params] 雷达工作参数
* [cohesion] 点迹凝聚参数
* [track_associate] 航迹关联参数
* [track_init] 航迹起始参数
* [track_die] 航迹消亡参数
* [track_quality] 航迹质量管理参数
* [track_index] 航迹批号管理
* [beam_control] 波束调度参数
* [singer_model] singer 模型机动参数
* [track_init_prohibite_area] 航迹起始屏蔽区（数组）
* [tas_prohibite_area] 禁止 TAS 跟踪区（数组）
*
* 标量参数用「字段描述表」集中登记（段名 + 键名 + 成员指针），避免逐项重复代码；
* 屏蔽区数组因为「长度由 *_num 决定」单独处理。加载流程：
* 解析 TOML -> 校验段/键集合 -> 读取标量与数组到临时对象 -> 取值校验 -> 一次性赋值。
* 任一步失败都不会修改调用方传入的 parameter。
************************************************************************************************/
namespace
{
// 屏蔽区数组容量，必须与 struct.h::RDP_parameter 中的数组长度一致。
constexpr int kProhibiteAreaCapacity = 30;
// 带上下文的错误收集：只记录第一条错误，统一返回 false 便于链式返回。
class LoadContext
{
public:
explicit LoadContext(std::string* error_msg) : error_msg_(error_msg) {}
bool fail(const std::string& message)
{
if (!failed_)
{
failed_ = true;
if (error_msg_ != nullptr)
*error_msg_ = message;
}
return false;
}
bool failed() const { return failed_; }
private:
std::string* error_msg_;
bool failed_ = false;
};
// 标量字段描述：TOML 段名、键名、RDP_parameter 成员指针。
struct FloatField
{
const char* section;
const char* key;
float RDP_parameter::*member;
};
struct IntField
{
const char* section;
const char* key;
int RDP_parameter::*member;
};
// ------------------------------ 标量字段登记表 ------------------------------
const FloatField kFloatFields[] = {
// [radar_params] 雷达工作参数
{"radar_params", "freq_0_GHz", &RDP_parameter::freq_0_GHz},
{"radar_params", "radar_height", &RDP_parameter::radar_height},
{"radar_params", "radar_system_delay", &RDP_parameter::radar_system_delay},
{"radar_params", "data_rate_tws", &RDP_parameter::data_rate_tws},
{"radar_params", "data_rate_tas", &RDP_parameter::data_rate_tas},
{"radar_params", "data_rate_track_only", &RDP_parameter::data_rate_track_only},
// [cohesion] 点迹凝聚参数
{"cohesion", "point_cohesion_range", &RDP_parameter::point_cohesion_range},
{"cohesion", "point_cohesion_velocity", &RDP_parameter::point_cohesion_velocity},
{"cohesion", "point_cohesion_azimuth", &RDP_parameter::point_cohesion_azimuth},
{"cohesion", "point_cohesion_range_min", &RDP_parameter::point_cohesion_range_min},
{"cohesion", "point_cohesion_range_max", &RDP_parameter::point_cohesion_range_max},
// [track_associate] 航迹关联参数
{"track_associate", "sigma_range", &RDP_parameter::sigma_range},
{"track_associate", "sigma_azimuth", &RDP_parameter::sigma_azimuth},
{"track_associate", "sigma_pitch", &RDP_parameter::sigma_pitch},
{"track_associate", "sigma_velocity", &RDP_parameter::sigma_velocity},
{"track_associate", "associate_threshold", &RDP_parameter::associate_threshold},
// [track_init] 航迹起始参数
{"track_init", "track_init_threshold", &RDP_parameter::track_init_threshold},
{"track_init", "track_init_angle", &RDP_parameter::track_init_angle},
{"track_init", "track_init_quality_L3", &RDP_parameter::track_init_quality_L3},
{"track_init", "track_init_quality_L4", &RDP_parameter::track_init_quality_L4},
{"track_init", "track_init_quality_L5", &RDP_parameter::track_init_quality_L5},
{"track_init", "track_init_quality_L6", &RDP_parameter::track_init_quality_L6},
{"track_init", "track_init_quality_L7", &RDP_parameter::track_init_quality_L7},
{"track_init", "track_init_velocity_min", &RDP_parameter::track_init_velocity_min},
{"track_init", "track_init_velocity_max", &RDP_parameter::track_init_velocity_max},
// [track_die] 航迹消亡参数
{"track_die", "track_die_quality", &RDP_parameter::track_die_quality},
{"track_die", "track_die_quality_pramary", &RDP_parameter::track_die_quality_pramary},
// [track_quality] 航迹质量管理参数
{"track_quality", "track_quality_max", &RDP_parameter::track_quality_max},
{"track_quality", "track_quality_min", &RDP_parameter::track_quality_min},
{"track_quality", "track_quality_d_weight", &RDP_parameter::track_quality_d_weight},
{"track_quality", "track_quality_snr_weight", &RDP_parameter::track_quality_snr_weight},
{"track_quality", "track_quality_len_weight", &RDP_parameter::track_quality_len_weight},
{"track_quality", "track_quality_iter_coeff", &RDP_parameter::track_quality_iter_coeff},
{"track_quality", "track_quality_decrease_coeff_o2", &RDP_parameter::track_quality_decrease_coeff_o2},
{"track_quality", "track_quality_decrease_coeff_o1", &RDP_parameter::track_quality_decrease_coeff_o1},
// [singer_model] singer 模型机动参数
{"singer_model", "model_1_Q_fast", &RDP_parameter::model_1_Q_fast},
{"singer_model", "model_1_Q_slow", &RDP_parameter::model_1_Q_slow},
{"singer_model", "model_2_Q_fast", &RDP_parameter::model_2_Q_fast},
{"singer_model", "model_2_Q_slow", &RDP_parameter::model_2_Q_slow},
{"singer_model", "model_3_Q_fast", &RDP_parameter::model_3_Q_fast},
{"singer_model", "model_3_Q_slow", &RDP_parameter::model_3_Q_slow},
};
const IntField kIntFields[] = {
// [radar_params] 雷达工作参数
{"radar_params", "radar_work_mode", &RDP_parameter::radar_work_mode},
// [track_associate] 航迹关联参数
{"track_associate", "extrapolate_max", &RDP_parameter::extrapolate_max},
{"track_associate", "extrapolate_max_tas", &RDP_parameter::extrapolate_max_tas},
{"track_associate", "height_smooth_win", &RDP_parameter::height_smooth_win},
// [track_init] 航迹起始参数
{"track_init", "track_init_point_num_min", &RDP_parameter::track_init_point_num_min},
// [track_die] 航迹消亡参数
{"track_die", "track_die_round", &RDP_parameter::track_die_round},
{"track_die", "track_die_round_pramary", &RDP_parameter::track_die_round_pramary},
{"track_die", "track_die_round_tas", &RDP_parameter::track_die_round_tas},
// [track_index] 航迹批号管理
{"track_index", "track_index_max", &RDP_parameter::track_index_max},
// [beam_control] 波束调度参数
{"beam_control", "default_track_beam_type", &RDP_parameter::default_track_beam_type},
{"beam_control", "tas_track_num_max", &RDP_parameter::tas_track_num_max},
// 屏蔽区数目（对应数组长度由它决定）
{"track_init_prohibite_area", "track_init_prohibite_area_num", &RDP_parameter::track_init_prohibite_area_num},
{"tas_prohibite_area", "tas_prohibite_area_num", &RDP_parameter::tas_prohibite_area_num},
};
// 屏蔽区数组描述：段名 + 键名 + 成员指针（数组长度为 kProhibiteAreaCapacity）。
struct AreaArrayField
{
const char* section;
const char* key;
float (RDP_parameter::*member)[kProhibiteAreaCapacity];
};
const AreaArrayField kAreaArrayFields[] = {
{"track_init_prohibite_area", "track_init_prohibite_area_range_min", &RDP_parameter::track_init_prohibite_area_range_min},
{"track_init_prohibite_area", "track_init_prohibite_area_range_max", &RDP_parameter::track_init_prohibite_area_range_max},
{"track_init_prohibite_area", "track_init_prohibite_area_azimuth_min", &RDP_parameter::track_init_prohibite_area_azimuth_min},
{"track_init_prohibite_area", "track_init_prohibite_area_azimuth_max", &RDP_parameter::track_init_prohibite_area_azimuth_max},
{"tas_prohibite_area", "tas_prohibite_area_range_min", &RDP_parameter::tas_prohibite_area_range_min},
{"tas_prohibite_area", "tas_prohibite_area_range_max", &RDP_parameter::tas_prohibite_area_range_max},
{"tas_prohibite_area", "tas_prohibite_area_azimuth_min", &RDP_parameter::tas_prohibite_area_azimuth_min},
{"tas_prohibite_area", "tas_prohibite_area_azimuth_max", &RDP_parameter::tas_prohibite_area_azimuth_max},
};
const char* const kAllSections[] = {
"radar_params",
"cohesion",
"track_associate",
"track_init",
"track_die",
"track_quality",
"track_index",
"beam_control",
"singer_model",
"track_init_prohibite_area",
"tas_prohibite_area",
};
// ------------------------------ 基础读取工具 ------------------------------
bool node_to_double(const toml::node& node, double& value)
{
if (node.is_integer())
{
value = static_cast<double>(*node.value<std::int64_t>());
return true;
}
if (node.is_floating_point())
{
value = *node.value<double>();
return true;
}
return false;
}
bool read_float(const toml::table& section, const char* section_name, const char* key, float& out, LoadContext& ctx)
{
const toml::node* node = section.get(key);
if (node == nullptr)
return ctx.fail("缺少配置项 [" + std::string(section_name) + "] " + key);
double value = 0.0;
if (!node_to_double(*node, value))
return ctx.fail("配置项 [" + std::string(section_name) + "] " + key + " 必须是数字");
if (!std::isfinite(value) || std::fabs(value) > static_cast<double>(FLT_MAX))
return ctx.fail("配置项 [" + std::string(section_name) + "] " + key + " 超出 float 可表示范围");
out = static_cast<float>(value);
return true;
}
bool read_int(const toml::table& section, const char* section_name, const char* key, int& out, LoadContext& ctx)
{
const toml::node* node = section.get(key);
if (node == nullptr)
return ctx.fail("缺少配置项 [" + std::string(section_name) + "] " + key);
if (!node->is_integer())
return ctx.fail("配置项 [" + std::string(section_name) + "] " + key + " 必须是整数");
const std::int64_t value = *node->value<std::int64_t>();
if (value < std::numeric_limits<int>::min() || value > std::numeric_limits<int>::max())
return ctx.fail("配置项 [" + std::string(section_name) + "] " + key + " 超出 int 可表示范围");
out = static_cast<int>(value);
return true;
}
// 取配置段；缺失或不是表都算错误（严格模式：11 个配置段全部必填）。
const toml::table* get_section(const toml::table& root, const char* section_name, LoadContext& ctx)
{
const toml::node* node = root.get(section_name);
if (node == nullptr)
{
ctx.fail("缺少配置段 [" + std::string(section_name) + "]");
return nullptr;
}
const toml::table* table = node->as_table();
if (table == nullptr)
{
ctx.fail("配置项 [" + std::string(section_name) + "] 必须是 TOML 表");
return nullptr;
}
return table;
}
// 收集某个段允许出现的全部键（标量 + 数组）。
std::vector<std::string_view> allowed_keys_of(const std::string_view section)
{
std::vector<std::string_view> keys;
for (const FloatField& field : kFloatFields)
{
if (section == field.section)
keys.emplace_back(field.key);
}
for (const IntField& field : kIntFields)
{
if (section == field.section)
keys.emplace_back(field.key);
}
for (const AreaArrayField& field : kAreaArrayFields)
{
if (section == field.section)
keys.emplace_back(field.key);
}
return keys;
}
// 严格校验某个段中只出现允许的键（拼错的键按未知参数拒绝）。
bool check_keys_of_section(const toml::table& section, const char* section_name, LoadContext& ctx)
{
const std::vector<std::string_view> allowed = allowed_keys_of(section_name);
for (const auto& [key, value] : section)
{
(void)value;
bool known = false;
for (const std::string_view item : allowed)
{
if (key.str() == item)
{
known = true;
break;
}
}
if (!known)
return ctx.fail("配置段 [" + std::string(section_name) + "] 中存在未知配置项: " + std::string(key.str()));
}
return true;
}
// 读取一个屏蔽区段的 4 个数组。
// 约定：*_num > 0 时数组必须存在且元素个数等于 *_num；*_num == 0 时数组可省略，
// 存在时必须是空数组。数组元素写入 RDP_parameter 的定长数组（未使用位置保持 0）。
bool read_area_arrays(const toml::table& section, const char* section_name, int area_num,
RDP_parameter& parameter, LoadContext& ctx)
{
for (const AreaArrayField& field : kAreaArrayFields)
{
if (std::string_view(field.section) != section_name)
continue;
const toml::node* node = section.get(field.key);
if (node == nullptr)
{
if (area_num == 0)
continue;
return ctx.fail("缺少配置项 [" + std::string(section_name) + "] " + field.key);
}
const toml::array* array = node->as_array();
if (array == nullptr)
return ctx.fail("配置项 [" + std::string(section_name) + "] " + field.key + " 必须是数组");
if (static_cast<int>(array->size()) != area_num)
return ctx.fail("配置项 [" + std::string(section_name) + "] " + field.key + " 的元素个数(" +
std::to_string(array->size()) + ")与 " + std::string(section_name) + "_num(" +
std::to_string(area_num) + ")不一致");
float* destination = parameter.*(field.member);
int index = 0;
for (const toml::node& element : *array)
{
double value = 0.0;
if (!node_to_double(element, value))
return ctx.fail("配置项 [" + std::string(section_name) + "] " + field.key + " 的第 " +
std::to_string(index + 1) + " 个元素必须是数字");
if (!std::isfinite(value) || std::fabs(value) > static_cast<double>(FLT_MAX))
return ctx.fail("配置项 [" + std::string(section_name) + "] " + field.key + " 的第 " +
std::to_string(index + 1) + " 个元素超出 float 可表示范围");
destination[index] = static_cast<float>(value);
++index;
}
}
return true;
}
// ------------------------------ 取值校验 ------------------------------
bool validate_parameter(const RDP_parameter& p, LoadContext& ctx)
{
// 雷达工作参数
if (p.radar_work_mode < 0 || p.radar_work_mode > 2)
return ctx.fail("参数 radar_work_mode 必须在 0~2 之间（实际 " + std::to_string(p.radar_work_mode) + "）");
if (!(p.freq_0_GHz > 0.0f))
return ctx.fail("参数 freq_0_GHz 必须大于 0");
if (p.radar_system_delay < 0.0f)
return ctx.fail("参数 radar_system_delay 不能为负");
if (!(p.data_rate_tws > 0.0f))
return ctx.fail("参数 data_rate_tws 必须大于 0");
if (!(p.data_rate_tas > 0.0f))
return ctx.fail("参数 data_rate_tas 必须大于 0");
if (p.data_rate_track_only < 0.0f)
return ctx.fail("参数 data_rate_track_only 不能为负");
// 点迹凝聚参数
if (!(p.point_cohesion_range > 0.0f))
return ctx.fail("参数 point_cohesion_range 必须大于 0");
if (!(p.point_cohesion_velocity > 0.0f))
return ctx.fail("参数 point_cohesion_velocity 必须大于 0");
if (!(p.point_cohesion_azimuth > 0.0f))
return ctx.fail("参数 point_cohesion_azimuth 必须大于 0");
if (p.point_cohesion_range_min < 0.0f)
return ctx.fail("参数 point_cohesion_range_min 不能为负");
if (!(p.point_cohesion_range_max > p.point_cohesion_range_min))
return ctx.fail("参数 point_cohesion_range_max 必须大于 point_cohesion_range_min");
// 航迹关联参数
if (!(p.sigma_range > 0.0f))
return ctx.fail("参数 sigma_range 必须大于 0");
if (!(p.sigma_azimuth > 0.0f))
return ctx.fail("参数 sigma_azimuth 必须大于 0");
if (!(p.sigma_pitch > 0.0f))
return ctx.fail("参数 sigma_pitch 必须大于 0");
if (!(p.sigma_velocity > 0.0f))
return ctx.fail("参数 sigma_velocity 必须大于 0");
if (!(p.associate_threshold > 0.0f))
return ctx.fail("参数 associate_threshold 必须大于 0");
if (p.extrapolate_max < 0)
return ctx.fail("参数 extrapolate_max 不能为负");
if (p.extrapolate_max_tas < 0)
return ctx.fail("参数 extrapolate_max_tas 不能为负");
if (p.height_smooth_win < 1)
return ctx.fail("参数 height_smooth_win 必须大于等于 1");
// 航迹起始参数
if (!(p.track_init_threshold > 0.0f))
return ctx.fail("参数 track_init_threshold 必须大于 0");
if (!(p.track_init_angle > 0.0f) || p.track_init_angle > 180.0f)
return ctx.fail("参数 track_init_angle 必须在 (0, 180] 度范围内");
if (p.track_init_point_num_min < 3 || p.track_init_point_num_min > 9)
return ctx.fail("参数 track_init_point_num_min 必须在 3~9 之间（实际 " +
std::to_string(p.track_init_point_num_min) + "）");
const float init_quality[] = {
p.track_init_quality_L3, p.track_init_quality_L4, p.track_init_quality_L5,
p.track_init_quality_L6, p.track_init_quality_L7};
for (int i = 0; i < 5; ++i)
{
if (init_quality[i] < 0.0f || init_quality[i] > 1.0f)
return ctx.fail("参数 track_init_quality_L" + std::to_string(i + 3) + " 必须在 0~1 之间");
}
if (p.track_init_velocity_min < 0.0f)
return ctx.fail("参数 track_init_velocity_min 不能为负");
if (!(p.track_init_velocity_max > p.track_init_velocity_min))
return ctx.fail("参数 track_init_velocity_max 必须大于 track_init_velocity_min");
// 航迹消亡参数
if (p.track_die_round < 0)
return ctx.fail("参数 track_die_round 不能为负");
if (p.track_die_round_pramary < 0)
return ctx.fail("参数 track_die_round_pramary 不能为负");
if (p.track_die_round_tas < 0)
return ctx.fail("参数 track_die_round_tas 不能为负");
if (p.track_die_quality < 0.0f || p.track_die_quality > 1.0f)
return ctx.fail("参数 track_die_quality 必须在 0~1 之间");
if (p.track_die_quality_pramary < 0.0f || p.track_die_quality_pramary > 1.0f)
return ctx.fail("参数 track_die_quality_pramary 必须在 0~1 之间");
// 航迹质量管理参数
if (p.track_quality_min < 0.0f)
return ctx.fail("参数 track_quality_min 不能为负");
if (!(p.track_quality_max > p.track_quality_min))
return ctx.fail("参数 track_quality_max 必须大于 track_quality_min");
if (p.track_quality_d_weight < 0.0f || p.track_quality_snr_weight < 0.0f || p.track_quality_len_weight < 0.0f)
return ctx.fail("参数 track_quality_d_weight / track_quality_snr_weight / track_quality_len_weight 不能为负");
if (p.track_quality_iter_coeff < 0.0f || p.track_quality_iter_coeff > 1.0f)
return ctx.fail("参数 track_quality_iter_coeff 必须在 0~1 之间");
if (p.track_quality_decrease_coeff_o2 > 0.0f || p.track_quality_decrease_coeff_o1 > 0.0f)
return ctx.fail("参数 track_quality_decrease_coeff_o2 / track_quality_decrease_coeff_o1 必须小于等于 0");
// 航迹批号管理
if (p.track_index_max < 1 || p.track_index_max > MAX_TRACK_NUM)
return ctx.fail("参数 track_index_max 必须在 1~" + std::to_string(MAX_TRACK_NUM) + " 之间（实际 " +
std::to_string(p.track_index_max) + "）");
// 波束调度参数
if (p.default_track_beam_type < 0)
return ctx.fail("参数 default_track_beam_type 不能为负");
if (p.tas_track_num_max < 0 || p.tas_track_num_max > RDP_TAS_QUEUE_CAPACITY)
return ctx.fail("参数 tas_track_num_max 必须在 0~" + std::to_string(RDP_TAS_QUEUE_CAPACITY) +
" 之间（实际 " + std::to_string(p.tas_track_num_max) + "）");
// singer 模型机动参数
const float model_q[] = {
p.model_1_Q_fast, p.model_1_Q_slow, p.model_2_Q_fast,
p.model_2_Q_slow, p.model_3_Q_fast, p.model_3_Q_slow};
const char* const model_q_names[] = {
"model_1_Q_fast", "model_1_Q_slow", "model_2_Q_fast",
"model_2_Q_slow", "model_3_Q_fast", "model_3_Q_slow"};
for (int i = 0; i < 6; ++i)
{
if (!(model_q[i] > 0.0f))
return ctx.fail(std::string("参数 ") + model_q_names[i] + " 必须大于 0");
}
// 屏蔽区
if (p.track_init_prohibite_area_num < 0 || p.track_init_prohibite_area_num > kProhibiteAreaCapacity)
return ctx.fail("参数 track_init_prohibite_area_num 必须在 0~" +
std::to_string(kProhibiteAreaCapacity) + " 之间");
if (p.tas_prohibite_area_num < 0 || p.tas_prohibite_area_num > kProhibiteAreaCapacity)
return ctx.fail("参数 tas_prohibite_area_num 必须在 0~" + std::to_string(kProhibiteAreaCapacity) + " 之间");
for (int i = 0; i < p.track_init_prohibite_area_num; ++i)
{
const std::string prefix = "track_init_prohibite_area 第 " + std::to_string(i + 1) + " 区";
if (p.track_init_prohibite_area_range_min[i] < 0.0f ||
!(p.track_init_prohibite_area_range_max[i] > p.track_init_prohibite_area_range_min[i]))
return ctx.fail(prefix + " 的距离范围非法（要求 max > min >= 0）");
if (p.track_init_prohibite_area_azimuth_min[i] < 0.0f ||
p.track_init_prohibite_area_azimuth_max[i] > 360.0f ||
!(p.track_init_prohibite_area_azimuth_max[i] > p.track_init_prohibite_area_azimuth_min[i]))
return ctx.fail(prefix + " 的方位范围非法（要求 0 =< min < max <= 360）");
}
for (int i = 0; i < p.tas_prohibite_area_num; ++i)
{
const std::string prefix = "tas_prohibite_area 第 " + std::to_string(i + 1) + " 区";
if (p.tas_prohibite_area_range_min[i] < 0.0f ||
!(p.tas_prohibite_area_range_max[i] > p.tas_prohibite_area_range_min[i]))
return ctx.fail(prefix + " 的距离范围非法（要求 max > min >= 0）");
if (p.tas_prohibite_area_azimuth_min[i] < 0.0f ||
p.tas_prohibite_area_azimuth_max[i] > 360.0f ||
!(p.tas_prohibite_area_azimuth_max[i] > p.tas_prohibite_area_azimuth_min[i]))
return ctx.fail(prefix + " 的方位范围非法（要求 0 =< min < max <= 360）");
}
return true;
}
// ------------------------------ 加载主流程 ------------------------------
bool load_all_sections(const toml::table& root, RDP_parameter& parameter, LoadContext& ctx)
{
// 1) 顶层只允许已知配置段
for (const auto& [key, value] : root)
{
(void)value;
bool known = false;
for (const char* name : kAllSections)
{
if (key.str() == name)
{
known = true;
break;
}
}
if (!known)
return ctx.fail("配置文件中存在未知配置段: [" + std::string(key.str()) + "]");
}
// 2) 先确认 11 个配置段都存在且为表（缺段优先报错，便于定位）
for (const char* name : kAllSections)
{
if (get_section(root, name, ctx) == nullptr)
return false;
}
// 3) 再校验每段内只允许登记过的键
for (const char* name : kAllSections)
{
const toml::table* section = get_section(root, name, ctx);
if (section == nullptr)
return false;
if (!check_keys_of_section(*section, name, ctx))
return false;
}
// 4) 读取全部标量
for (const FloatField& field : kFloatFields)
{
const toml::table* section = get_section(root, field.section, ctx);
if (section == nullptr)
return false;
if (!read_float(*section, field.section, field.key, parameter.*(field.member), ctx))
return false;
}
for (const IntField& field : kIntFields)
{
const toml::table* section = get_section(root, field.section, ctx);
if (section == nullptr)
return false;
if (!read_int(*section, field.section, field.key, parameter.*(field.member), ctx))
return false;
}
// 4) 读取屏蔽区数组（长度依赖已读入的 *_num）
const toml::table* init_area = get_section(root, "track_init_prohibite_area", ctx);
if (init_area == nullptr)
return false;
if (!read_area_arrays(*init_area, "track_init_prohibite_area", parameter.track_init_prohibite_area_num, parameter, ctx))
return false;
const toml::table* tas_area = get_section(root, "tas_prohibite_area", ctx);
if (tas_area == nullptr)
return false;
if (!read_area_arrays(*tas_area, "tas_prohibite_area", parameter.tas_prohibite_area_num, parameter, ctx))
return false;
return true;
}
} // namespace
int rdp_parameter_load(const char* parameter_file_path, RDP_parameter& parameter, std::string* error_msg)
{
LoadContext ctx(error_msg);
if (parameter_file_path == nullptr || parameter_file_path[0] == '\0')
{
ctx.fail("配置文件路径为空");
return -1;
}
if (error_msg != nullptr)
error_msg->clear();
toml::table root;
try
{
root = toml::parse_file(parameter_file_path);
}
catch (const toml::parse_error& err)
{
ctx.fail(std::string("TOML 解析失败: ") + std::string(err.what()));
return -1;
}
catch (const std::exception& err)
{
ctx.fail(std::string("读取配置文件失败: ") + err.what());
return -1;
}
RDP_parameter loaded{};
if (!load_all_sections(root, loaded, ctx))
return -1;
if (!validate_parameter(loaded, ctx))
return -1;
// 全部校验通过后才一次性写入，保证失败时调用方参数不变。
parameter = loaded;
return 0;
}
data_process_class_dll/rdp_parameter_loader.h
+26
View File
@@ -0,0 +1,26 @@
#ifndef RDP_PARAMETER_LOADER_H
#define RDP_PARAMETER_LOADER_H
#pragma once
#include "struct.h"
#include <string>
/************************************************************************************************
* 雷达数据处理参数加载器（内部模块，不属于宿主接口）
*
* 功能：把 TOML 配置文件加载到 struct.h::RDP_parameter。
* 约定：
* 1) 配置文件的每个配置段与 RDP_parameter 的注释分组对应，配置键名与结构体成员名完全一致；
* 2) 屏蔽区（track_init_prohibite_area / tas_prohibite_area）的数组使用与成员同名的 TOML 数组，
* 数组长度必须等于对应的 *_num，且 *_num 不超过 30；
* 3) 所有标量键均必须存在且类型/取值合法（严格校验），任一错误返回 -1 且不修改 parameter；
* 4) 数组键在 *_num == 0 时可以省略，存在时必须为空数组；*_num > 0 时必须存在且长度等于 *_num。
*
* 参数：
* parameter_file_path - 配置文件路径（UTF-8，宿主进程当前工作目录可达）
* parameter - 输出参数；仅在成功时被写入
* error_msg - 可选输出；失败时写入可读的错误原因（UTF-8）
* 返回：成功返回 0；失败返回 -1
************************************************************************************************/
int rdp_parameter_load(const char* parameter_file_path, RDP_parameter& parameter, std::string* error_msg);
#endif // RDP_PARAMETER_LOADER_H
data_process_class_dll/struct.h
+88
View File
@@ -189,4 +189,92 @@ struct Direct_Tracking_Target
};
// 系统工作参数结构体
struct RDP_parameter
{
// ******************* 雷达工作参数 ************************//
int radar_work_mode; //工作模式
float freq_0_GHz; //频点0频率
float radar_height; //雷达平台高度 （m）
float radar_system_delay; //系统延迟 (s)
float data_rate_tws; //TWS模式数据率
float data_rate_tas; //TAS模式数据率
float data_rate_track_only; //纯跟踪模式数据率
// ******************* 点迹凝聚参数 ************************//
float point_cohesion_range; //距离凝聚范围
float point_cohesion_velocity; //速度凝聚范围
float point_cohesion_azimuth; //方位凝聚范围
float point_cohesion_range_min; //最小凝聚距离
float point_cohesion_range_max; //最大凝聚距离
// ******************* 航迹关联参数 ************************//
float sigma_range; //距离量测精度
float sigma_azimuth; //方位量测精度
float sigma_pitch; //俯仰量测精度
float sigma_velocity; //速度量测精度
float associate_threshold; //关联门限
int extrapolate_max; //最大外推点数
int extrapolate_max_tas; //TAS最大外推点数
int height_smooth_win; //高度平滑窗长
// ******************* 航迹起始参数 ************************//
float track_init_threshold; //起航门限
float track_init_angle; //起航夹角
int track_init_point_num_min; //最少起批点数
float track_init_quality_L3; //3点起批质量门限
float track_init_quality_L4; //4点起批质量门限
float track_init_quality_L5; //5点起批质量门限
float track_init_quality_L6; //6点起批质量门限
float track_init_quality_L7; //7点起批质量门限
float track_init_velocity_min; //航迹起始最小速度
float track_init_velocity_max; //航迹起始最大速度
// ******************* 航迹消亡参数 ************************//
int track_die_round; //航迹消亡圈数
int track_die_round_pramary; //重点航迹消亡圈数
int track_die_round_tas; //TAS航迹消亡圈数
float track_die_quality; //航迹消批质量条件
float track_die_quality_pramary; //重点航迹消批质量条件
// ******************* 航迹质量管理参数 ************************//
float track_quality_max; //航迹质量最大值
float track_quality_min; //航迹质量最小值
float track_quality_d_weight; //马氏距离权重
float track_quality_snr_weight; //信噪比权重
float track_quality_len_weight; //航迹长度权重
float track_quality_iter_coeff; //航迹质量迭代系数
float track_quality_decrease_coeff_o2; //航迹质量下降系数2次项
float track_quality_decrease_coeff_o1; //航迹质量下降系数1次项
// ******************* 航迹批号管理 ************************//
int track_index_max; //最大航迹批号
// ******************* 波束调度参数 ************************//
int default_track_beam_type; //默认跟踪波束类型
int tas_track_num_max; //最大同时跟踪目标数
// ******************* singer模型机动参数 ************************//
float model_1_Q_fast; //模型1
float model_1_Q_slow; //模型1
float model_2_Q_fast; //模型2
float model_2_Q_slow; //模型2
float model_3_Q_fast; //模型3
float model_3_Q_slow; //模型3
// ******************* 航迹起始屏蔽区 ************************//
int track_init_prohibite_area_num; //航迹起始屏蔽区数
float track_init_prohibite_area_range_min[30]; //最小距离 (m)
float track_init_prohibite_area_range_max[30]; //最大距离 (m)
float track_init_prohibite_area_azimuth_min[30]; //最小方位角 (度)
float track_init_prohibite_area_azimuth_max[30]; //最大方位角 （度）
// ******************* 禁止TAS跟踪区 ************************//
int tas_prohibite_area_num; //禁止TAS的区域数
float tas_prohibite_area_range_min[30]; //最小距离 (m)
float tas_prohibite_area_range_max[30]; //最大距离 (m)
float tas_prohibite_area_azimuth_min[30]; //最小方位角 (度)
float tas_prohibite_area_azimuth_max[30]; //最大方位角 （度）
};
#endif // STRUCT_H
data_process_class_dll/tas_ctrl.cpp
+32 -70
View File
@@ -9,15 +9,9 @@
using namespace std;
// parameters.h 中定义了 DATA_RATE_TAS 宏（相扫0.3/机扫0.0625），
// 而本模块的波束门控必须使用 RadarPara 结构体成员 Work_Parameter.DATA_RATE_TAS
// （由dll调用者经 track_process_parameters_initial 输入），故取消该宏定义，
// 否则 Work_Parameter.DATA_RATE_TAS 会被预处理器展开为非法常量表达式。
#undef DATA_RATE_TAS
TAS_Ctrl::TAS_Ctrl()
{
memset(tas_target_queue,0,TAS_QUEUE_LENGTH*sizeof(Tracking_Target));
memset(tas_target_queue,0,RDP_TAS_QUEUE_CAPACITY*sizeof(Tracking_Target));
tas_target_num=0;
}
@@ -33,19 +27,19 @@ 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,
const RDP_parameter& rdp_parameter,
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,Work_Parameter,latest_timestamp);
tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,rdp_parameter);
tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,rdp_parameter);
tas_beam_output(trust_track,Tracking_beam,rdp_parameter,latest_timestamp);
// //跟踪队列移位
// struct Tracking_Target tas_target_tmp;
// memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target));
// for (int i=TAS_QUEUE_LENGTH-1;i>0;i--)
// memcpy(&tas_target_tmp, &tas_target_queue[RDP_TAS_QUEUE_CAPACITY-1], sizeof(Tracking_Target));
// for (int i=RDP_TAS_QUEUE_CAPACITY-1;i>0;i--)
// {
// memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
// }
@@ -55,26 +49,26 @@ void TAS_Ctrl::tas_ctrl_process(std::vector<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)
const RDP_parameter& rdp_parameter)
{
for (size_t i=0;i<trust_track->size();i++)
{
double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
double h=(*trust_track)[i].Height+Work_Parameter.Height;
double h=(*trust_track)[i].Height+rdp_parameter.radar_height;
double azi = atan2((*trust_track)[i].X[3],(*trust_track)[i].X[0]);
if(azi<0)
azi=azi+2*PI;
azi=azi/PI*180;
//进入跟踪的条件： 1.手动跟踪的目标 或 满足速度、距离、高度条件满足 关联点数大于2 2.不在禁止跟踪区域
if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, Work_Parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
&& tas_target_num < MAX_TAS_NUM
if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, rdp_parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
&& tas_target_num < rdp_parameter.tas_track_num_max
&& (*trust_track)[i].Track_Mode == 0
&& tas_prohibite_area( r, azi, v, h, Work_Parameter) == 0)
&& tas_prohibite_area( r, azi, v, h, rdp_parameter) == 0)
{
for(int j=0;j<TAS_QUEUE_LENGTH;j++)
for(int j=0;j<RDP_TAS_QUEUE_CAPACITY;j++)
{
if(tas_target_queue[j].empty_flag==0)// && j%2==0)
{
@@ -107,7 +101,7 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
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].Track_Mode=(*trust_track)[i].Track_Mode;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=static_cast<float>(asin((*trust_track)[i].Height/r_output)/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
break;
@@ -119,32 +113,16 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
};
//int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter)
//{
// 说明：TAS 自动开始（tas_auto_start）的原实现依赖改造前雷达参数结构体中的
// TAS 距离/速度/高度门限，这些参数未纳入新的 RDP_parameter，故原实现保持停用；
// 具体逻辑不再以注释代码形式保留（tas_ctrl.h 中保留声明，后续如需启用再按新参数补齐）。
//// if(r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
//// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) )
//// )
// if( r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) ) )
// {
// return 1;
// }
// else
// {
// return 0;
// }
//// return 0;
//};
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter)
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,const RDP_parameter& rdp_parameter)
{
for (int i=0;i<Work_Parameter.TAS_prohibite_area_num;i++)
for (int i=0;i<rdp_parameter.tas_prohibite_area_num;i++)
{
if(r<Work_Parameter.R_max_TAS_prohibited[i] && r>Work_Parameter.R_min_TAS_prohibited[i] && azi<Work_Parameter.Azimuth_max_TAS_prohibited[i] && azi>Work_Parameter.Azimuth_min_TAS_prohibited[i])
if(r<rdp_parameter.tas_prohibite_area_range_max[i] && r>rdp_parameter.tas_prohibite_area_range_min[i] && azi<rdp_parameter.tas_prohibite_area_azimuth_max[i] && azi>rdp_parameter.tas_prohibite_area_azimuth_min[i])
{
return 1;
}
@@ -157,11 +135,11 @@ int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct
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)
const RDP_parameter& rdp_parameter)
{
//查找TAS目标是否已经消批 若消批则移出TAS队列
for (int i=0;i<TAS_QUEUE_LENGTH;i++)
for (int i=0;i<RDP_TAS_QUEUE_CAPACITY;i++)
{
if(tas_target_queue[i].empty_flag == 1)
{
@@ -189,14 +167,14 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
// {
// double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
// double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
// double h=(*trust_track)[i].Height+Work_Parameter.Height;
// double h=(*trust_track)[i].Height+rdp_parameter.radar_height;
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h,Work_Parameter) == 1) //满足结束跟踪条件
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h,rdp_parameter) == 1) //满足结束跟踪条件
// {
// int track_index=(*trust_track)[i].Track_Index;
// for (int ii=0;ii<TAS_QUEUE_LENGTH;ii++)
// for (int ii=0;ii<RDP_TAS_QUEUE_CAPACITY;ii++)
// {
// if(tas_target_queue[ii].Index == track_index)
// {
@@ -224,7 +202,7 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
// 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].Track_Mode=(*trust_track)[i].Track_Mode;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=static_cast<float>(asin((*trust_track)[i].Height/r_output)/PI*180);
// Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
// Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
// Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
// Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
// }
@@ -235,28 +213,12 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
};
//int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter)
//{
// if((r<=1000 && h<Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h<Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h<Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h<Work_Parameter.TAS_Height_4) || (r>4000 && h<Work_Parameter.TAS_Height_5)
// || v<Work_Parameter.V_TAS_min || v>Work_Parameter.V_TAS_max
// )
// {
// return 1;
// }
// else
// {
// return 0;
// }
// return 0;
//};
// 说明：TAS 自动结束（tas_auto_end）的原实现同样依赖未纳入 RDP_parameter 的
// TAS 距离/速度/高度门限，故保持停用（tas_ctrl.h 中保留声明，实现待后续补齐）。
void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
struct RadarPara Work_Parameter,
const RDP_parameter& rdp_parameter,
long long latest_timestamp)
{
@@ -270,7 +232,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
double X_now[6] = {0};
//查找TAS队列中CPI时间最早的目标且满足TAS数据率的目标
for(int tas_idx=0;tas_idx<TAS_QUEUE_LENGTH;tas_idx++)
for(int tas_idx=0;tas_idx<RDP_TAS_QUEUE_CAPACITY;tas_idx++)
{
if(tas_target_queue[tas_idx].empty_flag==0)
{
@@ -285,7 +247,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
delta_T_last_track = (latest_timestamp - tas_target_queue[tas_idx].last_track_time) / 1000.0f;
//查找CPI时间最早，且满足TAS数据率的目标
if(CPI_time < earliest_CPI_time && (delta_T_last_track > Work_Parameter.DATA_RATE_TAS))
if(CPI_time < earliest_CPI_time && (delta_T_last_track > rdp_parameter.data_rate_tas))
{
earliest_CPI_time = CPI_time;
H_track = (*trust_track)[track_idx].Height;
@@ -339,7 +301,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
Tracking_beam->Elev=(float)elev;
//跟踪波束类型
Tracking_beam->type=1;
Tracking_beam->type=rdp_parameter.default_track_beam_type;
//跟踪目标批号
Tracking_beam->TAS_track_index = (*trust_track)[earliest_track_idx].Track_Index;
data_process_class_dll/tas_ctrl.h
+8 -8
View File
@@ -18,7 +18,7 @@ public:
struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter,
const RDP_parameter& rdp_parameter,
long long latest_timestamp);
private:
@@ -27,30 +27,30 @@ private:
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);
const RDP_parameter& rdp_parameter);
//删除TAS目标
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);
const RDP_parameter& rdp_parameter);
//TAS队列信息输出
void tas_beam_output(std::vector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
struct RadarPara Work_Parameter,
const RDP_parameter& rdp_parameter,
long long latest_timestamp);
//TAS 自动开启条件
int tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter);
int tas_auto_start(double v, double r, double h, const RDP_parameter& rdp_parameter);
//TAS 自动退出条件
int tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter);
int tas_auto_end(double v, double r, double h, const RDP_parameter& rdp_parameter);
// 禁止跟踪区域判断
int tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter);
int tas_prohibite_area(double r,double azi, double v, double h,const RDP_parameter& rdp_parameter);
struct Tracking_Target tas_target_queue[TAS_QUEUE_LENGTH]; //跟踪目标信息队列
struct Tracking_Target tas_target_queue[RDP_TAS_QUEUE_CAPACITY]; //跟踪目标信息队列
int tas_target_num; //跟踪目标数量
data_process_class_dll/track_asso.cpp
+49 -66
View File
@@ -20,7 +20,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
long long latest_timestamp, //最新时间戳
struct RadarPara Work_Parameter //雷达参数
const RDP_parameter& rdp_parameter //工作参数
)
{
@@ -35,7 +35,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
//IMM算法
model_interaction(trust_track);
model_filter(trust_track, Work_Parameter, latest_timestamp);
model_filter(trust_track, rdp_parameter, latest_timestamp);
model_output(trust_track);
@@ -80,8 +80,8 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
double x,y;
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*rdp_parameter.radar_system_delay;
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*rdp_parameter.radar_system_delay;
coor_trans Coor_trans;
double r, azi;
Coor_trans.cart2polar(x, y, &r, &azi);
@@ -103,8 +103,8 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=static_cast<float>((*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=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast<float>((*trust_track)[i].Quality * TRACK_QUALITY_MAX);
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast<float>((*trust_track)[i].Quality * rdp_parameter.track_quality_max);
double Direction_Angle;
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
@@ -130,7 +130,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
//直接输出点迹高度-20260605
//Trust_Track_Output[*Trust_track_num_Output-1][0].z = (*trust_track)[i].range_point * sin((*trust_track)[i].elev_point/180.0*PI);
//输出平滑后的高度
Trust_Track_Output[*Trust_track_num_Output-1][0].z = static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
Trust_Track_Output[*Trust_track_num_Output-1][0].z = static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*trust_track)[i].elev_point);
@@ -288,7 +288,7 @@ void Track_Asso:: model_interaction(std::vector <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)
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],const RDP_parameter& rdp_parameter)
{
//F
@@ -316,7 +316,7 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
//模型1
//Q
double q1=Work_Parameter.Model1_Q_slow;
double q1=rdp_parameter.model_1_Q_slow;
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
@@ -337,24 +337,24 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
double q2;
if(v_track<=5)
{
q2=Work_Parameter.Model2_Q_slow/20;
q2=rdp_parameter.model_2_Q_slow/20;
}
else if(v_track<=15&&v_track>5)
{
q2=Work_Parameter.Model2_Q_slow;
q2=rdp_parameter.model_2_Q_slow;
}
else if(v_track<=30&&v_track>15)
{
q2=Work_Parameter.Model2_Q_slow;
q2=rdp_parameter.model_2_Q_slow;
}
else if(v_track<=100&&v_track>30)
{
q2=Work_Parameter.Model2_Q_fast;
q2=rdp_parameter.model_2_Q_fast;
}
else
{
q2=Work_Parameter.Model2_Q_fast;
q2=rdp_parameter.model_2_Q_fast;
}
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
@@ -378,29 +378,29 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
double q3;
if(v_track<=5)
{
q3=Work_Parameter.Model3_Q_slow/20;
q3=rdp_parameter.model_3_Q_slow/20;
}
else if(v_track<=15&&v_track>5)
{
q3=Work_Parameter.Model3_Q_slow;
q3=rdp_parameter.model_3_Q_slow;
}
else if(v_track<=20&&v_track>15)
{
q3=Work_Parameter.Model3_Q_slow;
q3=rdp_parameter.model_3_Q_slow;
}
else if(v_track<=50&&v_track>20)
{
q3=Work_Parameter.Model3_Q_fast;
q3=rdp_parameter.model_3_Q_fast;
}
else if(v_track<=100&&v_track>50)
{
q3=2*Work_Parameter.Model3_Q_fast;
q3=2*rdp_parameter.model_3_Q_fast;
}
else
{
q3=10*Work_Parameter.Model3_Q_fast;
q3=10*rdp_parameter.model_3_Q_fast;
}
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
@@ -429,19 +429,19 @@ void Track_Asso::IMM_d_cal(double v_track,
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
kalman Kalman;
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind, rdp_parameter);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind, rdp_parameter);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind, rdp_parameter);
}
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct RadarPara Work_Parameter, long long latest_timestamp)
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_parameter& rdp_parameter, long long latest_timestamp)
{
@@ -492,17 +492,17 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
//计算统计距离d
double d1,d2,d3;
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,rdp_parameter);
//判定统计距离门限，thd_d
bool thd_d = false;
if( (r_track <= 500 && \
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 1000.0)) || \
(d1*d1 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold / 1000.0)) || \
((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)) || \
(d1*d1 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold / 100.0)) || \
(r_track >= 1000 && \
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD))\
(d1*d1 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold))\
){
thd_d = true;
}
@@ -623,7 +623,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
double v_track = sqrt(pow((*trust_track)[track_index-1].X[1],2)+pow((*trust_track)[track_index-1].X[4],2));
double delta_T = (point_process[point_index-1].CPI_time - (*trust_track)[track_index-1].T_track)/1000.0;
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
if (delta_T > 0)
{
@@ -632,9 +632,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
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 );
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind, rdp_parameter);
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind, rdp_parameter);
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind, rdp_parameter);
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
@@ -690,9 +690,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
double d_tmp = exp(-associated_info[min_index-1].d_min);
double snr_tmp = 1/(1+exp(-(point_process[point_index-1].SNR - 16) / 1.2));
double len_tmp = 1/(1+exp(-((*trust_track)[track_index-1].Valid_point_sum - 4) / 1.2)) * 0.5 + (1 - (*trust_track)[track_index-1].Valid_point_sum / (*trust_track)[track_index-1].Point_sum) * 0.5;
double tq_tmp = TRACK_QUALITY_D_WEIGHT * d_tmp + TRACK_QUALITY_SNR_WEIGHT * snr_tmp + TRACK_QUALITY_LEN_WEIGHT * len_tmp;
double tq_tmp = rdp_parameter.track_quality_d_weight * d_tmp + rdp_parameter.track_quality_snr_weight * snr_tmp + rdp_parameter.track_quality_len_weight * len_tmp;
(*trust_track)[track_index-1].Quality = (*trust_track)[track_index-1].Quality * TRACK_QUALITY_ITER_COEFF + tq_tmp * (1 - TRACK_QUALITY_ITER_COEFF);
(*trust_track)[track_index-1].Quality = (*trust_track)[track_index-1].Quality * rdp_parameter.track_quality_iter_coeff + tq_tmp * (1 - rdp_parameter.track_quality_iter_coeff);
//更新航迹时间
(*trust_track)[track_index-1].T_track = point_process[point_index-1].CPI_time;
@@ -721,7 +721,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
std::memcpy((*trust_track)[track_index-1].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[track_index-1].range_dim));
//更新高度
track_hight_update(track_index,point_index,trust_track);
track_hight_update(track_index,point_index,trust_track,rdp_parameter);
(*trust_track)[track_index-1].Extrapolate_round = 0; //连续未用实点更新时间
(*trust_track)[track_index-1].point_flag=1; //实点
@@ -780,34 +780,16 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
if((*trust_track)[i].point_flag == 0 && (*trust_track)[i].manual_tracking_flag == 0) //未关联上点迹的航迹进行外推
{
double delta_T = 0;
// double delta_T = (latest_timestamp - (*trust_track)[i].T_track)/1000.0;
// if (delta_T <= 0)
// continue;
if(Work_Parameter.work_mode == 0)
{
delta_T = Work_Parameter.DATA_RATE_SHORT;
}
else if(Work_Parameter.work_mode == 1)
{
delta_T = Work_Parameter.DATA_RATE_MIDDLE;
}
else if(Work_Parameter.work_mode == 2)
{
delta_T = Work_Parameter.DATA_RATE_FAR;
}
else
{
delta_T = Work_Parameter.DATA_RATE_SHORT;
}
delta_T = rdp_parameter.data_rate_tws;
//外推点数未到上限，则继续外推，否则不外推
if((*trust_track)[i].Extrapolate_round <= MAX_EXTRAPOLATE_NUM)
if((*trust_track)[i].Extrapolate_round <= rdp_parameter.extrapolate_max)
{
double v_track = sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
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);
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,rdp_parameter);
//航迹X P
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
@@ -845,10 +827,10 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
//航迹质量递减
double t = (*trust_track)[i].Unupdated_time / 1000.0;
(*trust_track)[i].Quality -= t * t * TRACK_QUALITY_DECREASE_COEFF_O2 + t * TRACK_QUALITY_DECREASE_COEFF_O1;
(*trust_track)[i].Quality -= t * t * rdp_parameter.track_quality_decrease_coeff_o2 + t * rdp_parameter.track_quality_decrease_coeff_o1;
(*trust_track)[i].Unupdated_time += static_cast<long long>(delta_T*1000.0);
t = (*trust_track)[i].Unupdated_time / 1000.0;
(*trust_track)[i].Quality += t * t * TRACK_QUALITY_DECREASE_COEFF_O2 + t * TRACK_QUALITY_DECREASE_COEFF_O1;
(*trust_track)[i].Quality += t * t * rdp_parameter.track_quality_decrease_coeff_o2 + t * rdp_parameter.track_quality_decrease_coeff_o1;
(*trust_track)[i].Quality = (*trust_track)[i].Quality < 0.0 ? 0.0 : (*trust_track)[i].Quality;
(*trust_track)[i].Point_sum = (*trust_track)[i].Point_sum + 1;
@@ -965,7 +947,8 @@ void Track_Asso:: model_output(std::vector <Trust_Track> *trust_track)
//高度维更新
void Track_Asso:: track_hight_update(int updata_track_index, //更新的航迹号
int asso_point_index, //点迹号
std::vector <Trust_Track> *trust_track //航迹
std::vector <Trust_Track> *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
)
{
@@ -978,19 +961,19 @@ void Track_Asso:: track_hight_update(int updata_
if((*trust_track)[updata_track_index-1].range_point <= 1000)
{
height_win_length = H_F_WIN_LEN+2;
height_win_length = rdp_parameter.height_smooth_win+2;
}
else if((*trust_track)[updata_track_index-1].range_point <= 2000 && (*trust_track)[updata_track_index-1].range_point > 1000)
{
height_win_length = H_F_WIN_LEN+3;
height_win_length = rdp_parameter.height_smooth_win+3;
}
else if((*trust_track)[updata_track_index-1].range_point <= 4000 && (*trust_track)[updata_track_index-1].range_point > 2000)
{
height_win_length = H_F_WIN_LEN+4;
height_win_length = rdp_parameter.height_smooth_win+4;
}
else
{
height_win_length = H_F_WIN_LEN+6;
height_win_length = rdp_parameter.height_smooth_win+6;
}
double sum=0;
data_process_class_dll/track_asso.h
+6 -5
View File
@@ -17,7 +17,7 @@ public:
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
long long latest_timestamp, //最新时间戳
struct RadarPara Work_Parameter //工作参数
const RDP_parameter& rdp_parameter //工作参数
);
Track_Asso();
@@ -31,7 +31,7 @@ private:
void model_interaction(std::vector <Trust_Track> *trust_track); //模型交互
void model_filter(std::vector <Trust_Track> *trust_track, //滤波
struct RadarPara Work_Parameter,
const RDP_parameter& rdp_parameter,
long long latest_timestamp
);
@@ -44,16 +44,17 @@ private:
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
struct RadarPara Work_Parameter);
const RDP_parameter& rdp_parameter);
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter);
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter);
double Pt[3][3]; //模型转移概率
//高度维更新
void track_hight_update(int updata_track_index, //更新的航迹号
int asso_point_index, //点迹号
std::vector <Trust_Track> *trust_track //航迹
std::vector <Trust_Track> *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
);
};
#endif // TRACK_ASSO_H
data_process_class_dll/track_asso_direct_tracking.cpp
+34 -33
View File
@@ -12,7 +12,7 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<P
Trust_Track *trust_track, //航迹
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter //工作参数
const RDP_parameter& rdp_parameter //工作参数
)
{
@@ -25,7 +25,7 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<P
//IMM算法
model_interaction(trust_track);
model_filter(trust_track, Work_Parameter);
model_filter(trust_track, rdp_parameter);
model_output(trust_track);
@@ -38,8 +38,8 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<P
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
double x,y;
x=(*trust_track).X[0]+(*trust_track).X[1]*Work_Parameter.Sys_delay;
y=(*trust_track).X[3]+(*trust_track).X[4]*Work_Parameter.Sys_delay;
x=(*trust_track).X[0]+(*trust_track).X[1]*rdp_parameter.radar_system_delay;
y=(*trust_track).X[3]+(*trust_track).X[4]*rdp_parameter.radar_system_delay;
coor_trans Coor_trans;
double r, azi;
Coor_trans.cart2polar(x, y, &r, &azi);
@@ -58,7 +58,7 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<P
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track).Track_Mode;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track).T_track/1000.0;
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track).point_flag;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track).Height+Work_Parameter.Height;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track).Height+rdp_parameter.radar_height;
double Direction_Angle;
Direction_Angle=atan2((*trust_track).X[4], (*trust_track).X[1]);
@@ -186,7 +186,7 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
}
void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, //滤波
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
//存储关联信息
struct asso_info
@@ -234,7 +234,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
//计算统计距离d
double d1,d2,d3;
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,rdp_parameter);
//计算高度门限
double d_h;
@@ -290,7 +290,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
}
//小于关联门限 保存关联信息
if((d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD)
if((d1*d1<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold)
&& d_h == 1 )
{
struct asso_info associated_info_tmp;
@@ -337,7 +337,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
// Z[1]=point_process[point_index-1].Range*sin(point_process[point_index-1].Azimuth);
double delta_T = (point_process[point_index-1].CPI_time - T_track)/1000.0;
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_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[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
@@ -345,9 +345,9 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
// 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);
// Kalman.kalman_filter(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3);
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 );
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind, rdp_parameter);
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind, rdp_parameter);
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind, rdp_parameter);
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;jj++)
@@ -418,15 +418,15 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
(*trust_track).RCS = point_process[point_index-1].RCS;
//更新高度
track_hight_update(point_index,trust_track);
track_hight_update(point_index,trust_track,rdp_parameter);
}
//未关联上，航迹外推
else
{
double delta_T = Work_Parameter.DATA_RATE_TAS/1000.0;
double delta_T = rdp_parameter.data_rate_tas/1000.0;
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,Work_Parameter);
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,rdp_parameter);
double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
kalman Kalman;
@@ -510,7 +510,7 @@ void Track_Asso_Direct_Tracking::model_output(Trust_Track *trust_track)
}
// 产生 F Q 矩阵
void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter)
void Track_Asso_Direct_Tracking::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],const RDP_parameter& rdp_parameter)
{
//F
@@ -538,7 +538,7 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
//模型1
//Q
double q1=Work_Parameter.Model1_Q_slow;
double q1=rdp_parameter.model_1_Q_slow;
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
@@ -559,15 +559,15 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
double q2;
if(v_track<=5)
{
q2=Work_Parameter.Model2_Q_slow/20;
q2=rdp_parameter.model_2_Q_slow/20;
}
else if(v_track<=100&&v_track>50)
{
q2=10.0*Work_Parameter.Model2_Q_fast;
q2=10.0*rdp_parameter.model_2_Q_fast;
}
else
{
q2=Work_Parameter.Model2_Q_fast;
q2=rdp_parameter.model_2_Q_fast;
}
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
@@ -591,15 +591,15 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
double q3;
if(v_track<=5)
{
q3=Work_Parameter.Model3_Q_slow/20;
q3=rdp_parameter.model_3_Q_slow/20;
}
else if(v_track<=100&&v_track>50)
{
q3=10.0*Work_Parameter.Model3_Q_fast;
q3=10.0*rdp_parameter.model_3_Q_fast;
}
else
{
q3=Work_Parameter.Model3_Q_fast;
q3=rdp_parameter.model_3_Q_fast;
}
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q123=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
@@ -627,24 +627,25 @@ void Track_Asso_Direct_Tracking::IMM_d_cal(double v_track,
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
kalman Kalman;
// *d1=Kalman.d_cal_with_doppler(F,Q1,Z,X1,P1,v_point,prt,freq_ind);
// *d2=Kalman.d_cal_with_doppler(F,Q2,Z,X2,P2,v_point,prt,freq_ind);
// *d3=Kalman.d_cal_with_doppler(F,Q3,Z,X3,P3,v_point,prt,freq_ind);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind, rdp_parameter);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind, rdp_parameter);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind, rdp_parameter);
}
//高度维更新
void Track_Asso_Direct_Tracking::track_hight_update(int asso_point_index, //点迹号
Trust_Track *trust_track //航迹
Trust_Track *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
)
{
@@ -653,19 +654,19 @@ void Track_Asso_Direct_Tracking::track_hight_update(int asso_point
int height_win_length;
if((*trust_track).range_point <= 1000)
{
height_win_length = H_F_WIN_LEN+2;
height_win_length = rdp_parameter.height_smooth_win+2;
}
else if((*trust_track).range_point <= 2000 && (*trust_track).range_point > 1000)
{
height_win_length = H_F_WIN_LEN+3;
height_win_length = rdp_parameter.height_smooth_win+3;
}
else if((*trust_track).range_point <= 4000 && (*trust_track).range_point > 2000)
{
height_win_length = H_F_WIN_LEN+4;
height_win_length = rdp_parameter.height_smooth_win+4;
}
else
{
height_win_length = H_F_WIN_LEN+6;
height_win_length = rdp_parameter.height_smooth_win+6;
}
double sum=0;
data_process_class_dll/track_asso_direct_tracking.h
+6 -5
View File
@@ -13,7 +13,7 @@ class Track_Asso_Direct_Tracking
Trust_Track *trust_track, //航迹
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter //工作参数
const RDP_parameter& rdp_parameter //工作参数
);
Track_Asso_Direct_Tracking();
@@ -28,7 +28,7 @@ class Track_Asso_Direct_Tracking
void model_interaction(Trust_Track *trust_track); //模型交互
void model_filter(Trust_Track *trust_track, //滤波
struct RadarPara Work_Parameter
const RDP_parameter& rdp_parameter
);
void model_output(Trust_Track *trust_track); //模型输出
@@ -40,15 +40,16 @@ class Track_Asso_Direct_Tracking
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
struct RadarPara Work_Parameter);
const RDP_parameter& rdp_parameter);
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter);
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter);
double Pt[3][3]; //模型转移概率
//高度维更新
void track_hight_update(int asso_point_index, //点迹号
Trust_Track *trust_track //航迹
Trust_Track *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
);
};
data_process_class_dll/track_asso_tas.cpp
+34 -33
View File
@@ -16,7 +16,7 @@ int Track_Asso_Tas::track_asso_process_tas(std::vector<PointRecv>
std::vector<Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter, //工作参数
const RDP_parameter& rdp_parameter, //工作参数
int tas_track_idx,
long long latest_timestamp //最新时间戳
)
@@ -32,7 +32,7 @@ long long latest_timestamp //最新时间戳
//IMM算法
model_interaction(trust_track,tas_track_idx);
model_filter(trust_track, Work_Parameter,tas_track_idx, latest_timestamp);
model_filter(trust_track, rdp_parameter,tas_track_idx, latest_timestamp);
model_output(trust_track,tas_track_idx);
@@ -54,8 +54,8 @@ long long latest_timestamp //最新时间戳
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
double x,y;
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*rdp_parameter.radar_system_delay;
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*rdp_parameter.radar_system_delay;
coor_trans Coor_trans;
double r, azi;
Coor_trans.cart2polar(x, y, &r, &azi);
@@ -75,7 +75,7 @@ long long latest_timestamp //最新时间戳
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=static_cast<float>((*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=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
double Direction_Angle;
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
@@ -256,7 +256,7 @@ void Track_Asso_Tas:: model_interaction( std::vector<Trust_Track> *trust_track
}
void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *trust_track, //滤波
struct RadarPara Work_Parameter,
const RDP_parameter& rdp_parameter,
int tas_track_idx,
long long latest_timestamp //最新时间戳
)
@@ -322,12 +322,12 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
//计算统计距离d
double d1,d2,d3;
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,rdp_parameter);
//判定统计距离门限，thd_d
bool thd_d = false;
if(d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD){
if(d1*d1<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold){
thd_d = true;
}
@@ -418,7 +418,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
// Z[1]=point_process[point_index-1].Range*sin(point_process[point_index-1].Azimuth);
double delta_T = (point_process[point_index-1].CPI_time - T_track)/1000.0;
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_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[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
@@ -426,9 +426,9 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
// 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);
// Kalman.kalman_filter(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3);
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 );
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind, rdp_parameter);
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind, rdp_parameter);
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind, rdp_parameter);
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;jj++)
@@ -511,7 +511,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
std::memcpy((*trust_track)[i].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[i].range_dim));
//更新高度
track_hight_update(tas_track_idx,point_index,trust_track);
track_hight_update(tas_track_idx,point_index,trust_track,rdp_parameter);
}
}
@@ -533,13 +533,13 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
for(size_t i=0;i<trust_track->size();i++){
if((*trust_track)[i].Track_Index == tas_track_idx)
{
if((*trust_track)[i].Extrapolate_round >= MAX_EXTRAPOLATE_NUM_TAS)
if((*trust_track)[i].Extrapolate_round >= rdp_parameter.extrapolate_max_tas)
{
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);
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,rdp_parameter);
double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
kalman Kalman;
@@ -648,7 +648,7 @@ void Track_Asso_Tas::model_output(std::vector<Trust_Track> *trust_track, int
}
// 产生 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)
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],const RDP_parameter& rdp_parameter)
{
//F
@@ -676,7 +676,7 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6],
//模型1
//Q
double q1=Work_Parameter.Model1_Q_slow;
double q1=rdp_parameter.model_1_Q_slow;
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
@@ -697,15 +697,15 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6],
double q2;
if(v_track<=5)
{
q2=Work_Parameter.Model2_Q_slow/20;
q2=rdp_parameter.model_2_Q_slow/20;
}
else if(v_track<=100&&v_track>50)
{
q2=10.0*Work_Parameter.Model2_Q_fast;
q2=10.0*rdp_parameter.model_2_Q_fast;
}
else
{
q2=Work_Parameter.Model2_Q_fast;
q2=rdp_parameter.model_2_Q_fast;
}
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
@@ -729,15 +729,15 @@ Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332;
double q3;
if(v_track<=5)
{
q3=Work_Parameter.Model3_Q_slow/20;
q3=rdp_parameter.model_3_Q_slow/20;
}
else if(v_track<=100&&v_track>50)
{
q3=10.0*Work_Parameter.Model3_Q_fast;
q3=10.0*rdp_parameter.model_3_Q_fast;
}
else
{
q3=Work_Parameter.Model3_Q_fast;
q3=rdp_parameter.model_3_Q_fast;
}
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q123=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
@@ -765,25 +765,26 @@ double X3[6], double P3[6][6],
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
kalman Kalman;
// *d1=Kalman.d_cal_with_doppler(F,Q1,Z,X1,P1,v_point,prt,freq_ind);
// *d2=Kalman.d_cal_with_doppler(F,Q2,Z,X2,P2,v_point,prt,freq_ind);
// *d3=Kalman.d_cal_with_doppler(F,Q3,Z,X3,P3,v_point,prt,freq_ind);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind, rdp_parameter);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind, rdp_parameter);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind, rdp_parameter);
}
//高度维更新
void Track_Asso_Tas::track_hight_update(int tas_track_idx, //更新的航迹号
int asso_point_index, //点迹号
std::vector<Trust_Track> *trust_track //航迹
std::vector<Trust_Track> *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
)
{
for(size_t i=0; i<trust_track->size(); i++)
@@ -794,19 +795,19 @@ void Track_Asso_Tas::track_hight_update(int tas_
int height_win_length;
if((*trust_track)[i].range_point <= 1000)
{
height_win_length = H_F_WIN_LEN+6;
height_win_length = rdp_parameter.height_smooth_win+6;
}
else if((*trust_track)[i].range_point <= 2000 && (*trust_track)[i].range_point > 1000)
{
height_win_length = H_F_WIN_LEN+8;
height_win_length = rdp_parameter.height_smooth_win+8;
}
else if((*trust_track)[i].range_point <= 4000 && (*trust_track)[i].range_point > 2000)
{
height_win_length = H_F_WIN_LEN+10;
height_win_length = rdp_parameter.height_smooth_win+10;
}
else
{
height_win_length = H_F_WIN_LEN+12;
height_win_length = rdp_parameter.height_smooth_win+12;
}
double sum=0;
data_process_class_dll/track_asso_tas.h
+7 -6
View File
@@ -15,7 +15,7 @@ public:
std::vector<Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter, //工作参数
const RDP_parameter& rdp_parameter, //工作参数
int tas_track_idx,
long long latest_timestamp //最新时间戳
);
@@ -31,7 +31,7 @@ private:
void model_interaction(std::vector<Trust_Track> *trust_track, int tas_track_idx); //模型交互
void model_filter(std::vector<Trust_Track> *trust_track, //滤波
struct RadarPara Work_Parameter,
const RDP_parameter& rdp_parameter,
int tas_track_idx,
long long latest_timestamp //最新时间戳
);
@@ -45,16 +45,17 @@ private:
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
struct RadarPara Work_Parameter);
const RDP_parameter& rdp_parameter);
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter);
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter);
double Pt[3][3]; //模型转移概率
//高度维更新
void track_hight_update(int tas_track_index, //更新的航迹号
void track_hight_update(int tas_track_idx, //更新的航迹号
int asso_point_index, //点迹号
std::vector<Trust_Track> *trust_track //航迹
std::vector<Trust_Track> *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
);
};
data_process_class_dll/track_die.cpp
+4 -3
View File
@@ -8,7 +8,8 @@ using namespace std;
void Track_Die::track_die_process( std::vector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output)
int *Track_die_num_Output,
const RDP_parameter& rdp_parameter)
{
std::vector <Trust_Track>::iterator Iter;
@@ -23,13 +24,13 @@ void Track_Die::track_die_process( std::vector <Trust_Track> *trust_track,
}
//非重点航迹自动消亡
if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 0 && (*Iter).Quality < TRACK_DIE_QUALITY)
if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 0 && (*Iter).Quality < rdp_parameter.track_die_quality)
{
die_flag = true;
}
//重点航迹自动消亡
if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 1 && (*Iter).Extrapolate_round > TRACK_DIE_ROUND_PRIMARY)
if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 1 && (*Iter).Extrapolate_round > rdp_parameter.track_die_round_pramary)
{
die_flag = true;
}
data_process_class_dll/track_die.h
+2 -1
View File
@@ -11,7 +11,8 @@ class Track_Die
public:
void track_die_process( std::vector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output);
int *Track_die_num_Output,
const RDP_parameter& rdp_parameter);
};
#endif // TRACK_DIE_H
data_process_class_dll/track_die_tas.cpp
+3 -2
View File
@@ -11,7 +11,8 @@ using namespace std;
void Track_Die_Tas::track_die_process_tas( std::vector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output)
int *Track_die_num_Output,
const RDP_parameter& rdp_parameter)
{
std::vector <Trust_Track>::iterator Iter;
@@ -24,7 +25,7 @@ void Track_Die_Tas::track_die_process_tas( std::vector <Trust_Track> *trust_trac
die_flag = true;
}
if((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round > TRACK_DIE_ROUND_TAS)
if((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round > rdp_parameter.track_die_round_tas)
{
die_flag = true;
}
data_process_class_dll/track_die_tas.h
+2 -1
View File
@@ -11,7 +11,8 @@ class Track_Die_Tas
public:
void track_die_process_tas( std::vector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output);
int *Track_die_num_Output,
const RDP_parameter& rdp_parameter);
};
#endif // TRACK_DIE_TAS_H
data_process_class_dll/track_index_mangement.cpp
+10 -8
View File
@@ -11,7 +11,8 @@ void Track_Ind_Mangement::reset()
lastest_index = 0;
}
int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track)
int Track_Ind_Mangement::track_ind_get( std::vector<Trust_Track> *trust_track,
const RDP_parameter& rdp_parameter)
{
int isempty = 1;
@@ -29,18 +30,19 @@ int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track
else
{
//建立航迹号列表
int List[MAX_TRACK_INDEX]={0};
const int track_index_max = rdp_parameter.track_index_max;
std::vector<int> List(static_cast<std::size_t>(track_index_max), 0);
for (size_t i=0;i<trust_track->size();i++)
{
int ti = (*trust_track)[i].Track_Index;
if (ti >= 1 && ti <= MAX_TRACK_INDEX)
if (ti >= 1 && ti <= track_index_max)
List[ti-1]=1;
}
//分配航迹号
if(lastest_index<MAX_TRACK_INDEX)
if(lastest_index<track_index_max)
{
for (int i=lastest_index+1;i<=MAX_TRACK_INDEX;i++)
for (int i=lastest_index+1;i<=track_index_max;i++)
{
if(List[i-1]==0)
{
@@ -49,7 +51,7 @@ int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track
}
}
for (int i=1;i<=MAX_TRACK_INDEX;i++)
for (int i=1;i<=track_index_max;i++)
{
if(List[i-1]==0)
{
@@ -59,9 +61,9 @@ int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track
}
}
else if(lastest_index == MAX_TRACK_INDEX)
else if(lastest_index == track_index_max)
{
for (int i=1;i<=MAX_TRACK_INDEX;i++)
for (int i=1;i<=track_index_max;i++)
{
if(List[i-1]==0)
{
data_process_class_dll/track_index_mangement.h
+1 -1
View File
@@ -10,7 +10,7 @@ class Track_Ind_Mangement
{
public:
int track_ind_get( std::vector<Trust_Track> *trust_track);
int track_ind_get( std::vector<Trust_Track> *trust_track, const RDP_parameter& rdp_parameter);
void reset();
private:
data_process_class_dll/track_init.cpp
+38 -38
View File
@@ -14,7 +14,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
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
const RDP_parameter& rdp_parameter
)
{
@@ -35,10 +35,10 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
if(point_process.size()>0)
{
//点迹与临时航迹关联
point_temp_track_asso(temp_track,Work_Parameter);
point_temp_track_asso(temp_track,rdp_parameter);
//点迹与航迹头关联
point_track_head_asso(temp_track,Work_Parameter);
point_track_head_asso(temp_track,rdp_parameter);
//删除关联上的点迹
std::vector <PointRecv>::iterator Iter;
@@ -118,7 +118,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
#endif
//临时航迹满足起始长度 转为可靠航迹
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,rdp_parameter);
}
@@ -130,7 +130,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
}
void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
//关联信息
@@ -179,7 +179,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
{
//计算d
kalman Kalman;
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind,rdp_parameter);
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
//计算夹角
@@ -191,7 +191,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
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
if(d*d<=rdp_parameter.track_init_threshold*rdp_parameter.track_init_threshold && alpha<rdp_parameter.track_init_angle && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*rdp_parameter.sigma_pitch&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
{
struct Asso_info asso_info_tmp;
asso_info_tmp.point_idx = static_cast<int>(i)+1;
@@ -251,7 +251,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
kalman Kalman;
double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].CPI_time)/1000.0;
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -278,8 +278,8 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
snr_tmp = 1/(1+exp(-(temp_track_ref.back().SNR - 16) / 1.2));
}
quality_tmp = d_tmp * TRACK_QUALITY_D_WEIGHT + snr_tmp * TRACK_QUALITY_SNR_WEIGHT;
quality_tmp /= (TRACK_QUALITY_D_WEIGHT + TRACK_QUALITY_SNR_WEIGHT);
quality_tmp = d_tmp * rdp_parameter.track_quality_d_weight + snr_tmp * rdp_parameter.track_quality_snr_weight;
quality_tmp /= (rdp_parameter.track_quality_d_weight + rdp_parameter.track_quality_snr_weight);
if(L == 3){
for(int i=0;i<L;i++)
@@ -287,7 +287,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
temp_track_ref[i].quality = quality_tmp;
}
}else{
temp_track_ref[L-1].quality = temp_track_ref[L-2].quality * TRACK_QUALITY_ITER_COEFF + quality_tmp * (1 - TRACK_QUALITY_ITER_COEFF);
temp_track_ref[L-1].quality = temp_track_ref[L-2].quality * rdp_parameter.track_quality_iter_coeff + quality_tmp * (1 - rdp_parameter.track_quality_iter_coeff);
}
}
@@ -315,7 +315,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
}
void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
//关联上的信息
@@ -357,26 +357,26 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
double vmax;
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
if(rdp_parameter.radar_work_mode == 0) //近程模式 最大速度减小一点
{
vmax = Work_Parameter.V_MAX;
vmax = rdp_parameter.track_init_velocity_max;
}
else //中远程模式 最大速度正常用
{
vmax = Work_Parameter.V_MAX;
vmax = rdp_parameter.track_init_velocity_max;
}
double delta_T = (T_point - T_track_head)/1000.0;
// if(
// ( (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
// || (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
// ( (rdp_parameter.radar_work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=rdp_parameter.track_init_velocity_min*delta_T/2.0) ))
// || (rdp_parameter.radar_work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=rdp_parameter.track_init_velocity_min*delta_T/2.0) ))
// )
// && vr_point*v_track_head>0 )
//点航关联条件
if( dis <= vmax* delta_T && dis >= Work_Parameter.V_MIN * delta_T && \
if( dis <= vmax* delta_T && dis >= rdp_parameter.track_init_velocity_min * delta_T && \
vr_point*v_track_head>0 && \
dis >= abs(vr_point) * delta_T && \
dis >= abs(v_track_head) * delta_T
@@ -434,7 +434,7 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
kalman Kalman;
double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].CPI_time)/1000.0;
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -474,7 +474,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
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)
const RDP_parameter& rdp_parameter)
{
std::vector <std::vector<Temp_track>> Track_to_start;
@@ -498,8 +498,8 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
// double snr_tmp = 1/(1+exp(-(average_snr - 16) / 1.2));
// double &d = (*Iter)[L-1].d;
// double d_tmp = exp(-d);
// double quality_tmp = d_tmp * TRACK_QUALITY_D_WEIGHT + snr_tmp * TRACK_QUALITY_SNR_WEIGHT;
// quality_tmp /= (TRACK_QUALITY_D_WEIGHT + TRACK_QUALITY_SNR_WEIGHT);
// double quality_tmp = d_tmp * rdp_parameter.track_quality_d_weight + snr_tmp * rdp_parameter.track_quality_snr_weight;
// quality_tmp /= (rdp_parameter.track_quality_d_weight + rdp_parameter.track_quality_snr_weight);
// for(int i=0;i<L;i++)
// {
// (*Iter)[i].quality = quality_tmp;
@@ -514,12 +514,12 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
}
//航迹长度小于起始点数，不起批
if(L < Work_Parameter.track_start_point_num){
if(L < rdp_parameter.track_init_point_num_min){
track_init_flag = false;
}
//航迹起批禁止区域判断
if(track_init_prohibit(range,azi,Work_Parameter)!=0)
if(track_init_prohibit(range,azi,rdp_parameter)!=0)
{
track_init_flag = false;
}
@@ -527,32 +527,32 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
//航迹质量起批条件判断
switch(L){
case 3:
if(quality < TRACK_START_QUALITY_L3){
if(quality < rdp_parameter.track_init_quality_L3){
track_init_flag = false;
}
break;
case 4:
if(quality < TRACK_START_QUALITY_L4){
if(quality < rdp_parameter.track_init_quality_L4){
track_init_flag = false;
}
break;
case 5:
if(quality < TRACK_START_QUALITY_L5){
if(quality < rdp_parameter.track_init_quality_L5){
track_init_flag = false;
}
break;
case 6:
if(quality < TRACK_START_QUALITY_L6){
if(quality < rdp_parameter.track_init_quality_L6){
track_init_flag = false;
}
break;
case 7:
if(quality < TRACK_START_QUALITY_L7){
if(quality < rdp_parameter.track_init_quality_L7){
track_init_flag = false;
}
break;
default:
if(quality < TRACK_START_QUALITY_L7){
if(quality < rdp_parameter.track_init_quality_L7){
track_init_flag = false;
}
break;
@@ -581,7 +581,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
return;
//2.两两比较Track_to_start中的航迹信息，删除重复的航迹
int L = Work_Parameter.track_start_point_num;
int L = rdp_parameter.track_init_point_num_min;
for (size_t i=0; i+1<Track_to_start.size();i++)
{
if(Track_to_start[i][0].asso_flag!=2)
@@ -632,9 +632,9 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
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)
if(trust_track->size() < static_cast<std::size_t>(rdp_parameter.track_index_max))
{
int index = track_index_mangement.track_ind_get(trust_track);
int index = track_index_mangement.track_ind_get(trust_track, rdp_parameter);
if(index>0)
{
@@ -647,7 +647,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
double T1=(Track_to_start[i][L-2].CPI_time-Track_to_start[i][L-3].CPI_time)/1000.0;
double T2=(Track_to_start[i][L-1].CPI_time-Track_to_start[i][L-2].CPI_time)/1000.0;
kalman Kalman;
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P,rdp_parameter);
Trust_Track trust_track_tmp = {};
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
@@ -715,7 +715,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>(Track_to_start[i][j].amplitude);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast<float>(Track_to_start[i][j].SNR);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast<float>(Track_to_start[i][j].RCS);
Trust_Track_Output[*Trust_track_num_Output-1][j].Quality = static_cast<float>(Track_to_start[i][j].quality * TRACK_QUALITY_MAX);
Trust_Track_Output[*Trust_track_num_Output-1][j].Quality = static_cast<float>(Track_to_start[i][j].quality * rdp_parameter.track_quality_max);
Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;
@@ -783,12 +783,12 @@ void Track_Init::tmp_track_die(std::vector <std::vector<Temp_track>> *temp_t
}
};
int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter)
int Track_Init::track_init_prohibit(double r, double azi, const RDP_parameter& rdp_parameter)
{
for (int i=0;i<Work_Parameter.track_prohibite_area_num;i++)
for (int i=0;i<rdp_parameter.track_init_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])
if(r<rdp_parameter.track_init_prohibite_area_range_max[i] && r>rdp_parameter.track_init_prohibite_area_range_min[i] && azi<rdp_parameter.track_init_prohibite_area_azimuth_max[i] && azi>rdp_parameter.track_init_prohibite_area_azimuth_min[i])
{
return 1;
}
data_process_class_dll/track_init.h
+5 -5
View File
@@ -19,7 +19,7 @@ public:
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
const RDP_parameter& rdp_parameter
);
Track_Init();
@@ -32,22 +32,22 @@ private:
std::vector<PointRecv> point_process; //要处理的点迹
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter); //临时航迹与点迹关联
const RDP_parameter& rdp_parameter); //临时航迹与点迹关联
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter); //航迹头与点迹关联
const RDP_parameter& rdp_parameter); //航迹头与点迹关联
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);
const RDP_parameter& rdp_parameter);
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); //计算夹角
int track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0
int track_init_prohibit(double r, double azi, const RDP_parameter& rdp_parameter); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0
Track_Ind_Mangement track_index_mangement;
data_process_class_dll/track_init_direct_tracking.cpp
+17 -17
View File
@@ -13,7 +13,7 @@ int Track_Init_Direct_Tracking::track_init_process_logic( std::vector<PointRecv>
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,
const RDP_parameter& rdp_parameter,
int track_ID
)
{
@@ -23,7 +23,7 @@ int Track_Init_Direct_Tracking::track_init_process_logic( std::vector<PointRecv>
}
void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter) //临时航迹与点迹关联
const RDP_parameter& rdp_parameter) //临时航迹与点迹关联
{
//关联信息
struct Asso_info
@@ -70,7 +70,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
{
//计算d
kalman Kalman;
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind,rdp_parameter);
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
//计算夹角
@@ -82,7 +82,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
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
if(d*d<=rdp_parameter.track_init_threshold*rdp_parameter.track_init_threshold && alpha<rdp_parameter.track_init_angle && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*rdp_parameter.sigma_pitch&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
{
struct Asso_info asso_info_tmp;
asso_info_tmp.point_idx = static_cast<int>(i)+1;
@@ -134,7 +134,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
kalman Kalman;
double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].CPI_time)/1000.0;
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -164,7 +164,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
}
void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter) //航迹头与点迹关联
const RDP_parameter& rdp_parameter) //航迹头与点迹关联
{
//关联上的信息
struct Asso_info
@@ -205,24 +205,24 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
double vmax;
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
if(rdp_parameter.radar_work_mode == 0) //近程模式 最大速度减小一点
{
//vmax = V_MAX/5;
vmax = Work_Parameter.V_MAX;
//vmax = rdp_parameter.track_init_velocity_max/5;
vmax = rdp_parameter.track_init_velocity_max;
}
else //中远程模式 最大速度正常用
{
// vmax = V_MAX;
vmax = Work_Parameter.V_MAX;
// vmax = rdp_parameter.track_init_velocity_max;
vmax = rdp_parameter.track_init_velocity_max;
}
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
if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=rdp_parameter.track_init_velocity_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*rdp_parameter.sigma_pitch
{
struct Asso_info asso_info_tmp;
asso_info_tmp.point_idx = static_cast<int>(i)+1;
@@ -270,7 +270,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
kalman Kalman;
double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].CPI_time)/1000.0;
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -299,7 +299,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
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,
const RDP_parameter& rdp_parameter,
int track_ID)
{
//temp_track中的临时航迹转为可靠航迹
@@ -314,7 +314,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
}
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
if( L==rdp_parameter.track_init_point_num_min) //按长度查找TRUST_TRACK_POINT
{
//加入本地航迹文件
@@ -326,7 +326,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
double T1=((*Iter)[L-2].CPI_time-(*Iter)[L-3].CPI_time)/1000.0;
double T2=((*Iter)[L-1].CPI_time-(*Iter)[L-2].CPI_time)/1000.0;
kalman Kalman;
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P,rdp_parameter);
Trust_Track trust_track_tmp = {};
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
data_process_class_dll/track_init_direct_tracking.h
+4 -4
View File
@@ -14,7 +14,7 @@ public:
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,
const RDP_parameter& rdp_parameter,
int track_ID
);
@@ -26,16 +26,16 @@ private:
std::vector<PointRecv> point_process; //要处理的点迹
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter); //临时航迹与点迹关联
const RDP_parameter& rdp_parameter); //临时航迹与点迹关联
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter); //航迹头与点迹关联
const RDP_parameter& rdp_parameter); //航迹头与点迹关联
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,
const RDP_parameter& rdp_parameter,
int track_ID
);