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 15:56:02 +08:00
parent 76b0b460a5
commit 7e4cb2922d
34 changed files with 202 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/data_process.cpp
+13 -16
View File
@@ -9,6 +9,9 @@
#include <iostream>
using namespace std;
// 全局工作参数对象（声明见 struct.h）。所有处理模块直接访问，由 parameters_reload() 加载。
RDP_parameter rdp_parameter{};
/*******************************************************************************/
/*******************************************************************************/
/************************************用户接口函数*******************************/
@@ -172,7 +175,7 @@ int model)
if(model==2)
{
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,rdp_parameter);
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv);
std::vector<PointRecv>().swap(Data_buffer);
}
@@ -185,21 +188,18 @@ int model)
&trust_track,
Trust_Track_Output,
Trust_track_num_Output,
latest_timestamp,
rdp_parameter
latest_timestamp
);
track_die.track_die_process( &trust_track,
Track_die_Index_Output,
Track_die_num_Output,
rdp_parameter);
Track_die_num_Output);
track_init.track_init_process_logic( &point_recv,
&trust_track,
&temp_track,
Trust_Track_Output,
Trust_track_num_Output,
rdp_parameter);
Trust_track_num_Output);
}
@@ -209,7 +209,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,rdp_parameter); //点迹凝聚
dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas); //点迹凝聚
std::vector<PointRecv>().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer
@@ -217,7 +217,6 @@ int model)
&trust_track, //航迹文件
Trust_Track_Output, //更新航迹信息
Trust_track_num_Output, //更新航迹数
rdp_parameter, //工作参数
TAS_track_idx,
latest_timestamp //最新时间戳
);
@@ -226,8 +225,7 @@ int model)
track_die_tas.track_die_process_tas( &trust_track,
Track_die_Index_Output,
Track_die_num_Output,
rdp_parameter);
Track_die_num_Output);
//std::cout << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
}
@@ -241,21 +239,20 @@ 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,rdp_parameter, latest_timestamp);
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output, latest_timestamp);
}
//数据处理参数加载（从 TOML 配置文件加载全部工作参数）
//数据处理参数加载（从 TOML 配置文件加载全部工作参数到全局 rdp_parameter）
int Data_Process::parameters_reload(const char* parameter_file_path)
{
RDP_parameter loaded{};
std::string error;
if (rdp_parameter_load(parameter_file_path, loaded, &error) != 0)
// rdp_parameter_load 仅在完整校验通过后写入目标对象，失败时保持原参数不变
if (rdp_parameter_load(parameter_file_path, rdp_parameter, &error) != 0)
{
RDP_LOG << "parameters_reload failed: " << error << std::endl;
return -1;
}
rdp_parameter = loaded;
RDP_LOG << "parameters_reload ok: " << parameter_file_path << std::endl;
return 0;
}
data_process_class_dll/data_process.h
+1 -2
View File
@@ -83,8 +83,7 @@ private:
std::vector <Trust_Track> trust_track; //可靠航迹 按航迹区存入
std::vector <std::vector<Temp_track>> temp_track; //临时航迹 按临时航迹区存入
RDP_parameter rdp_parameter{}; //工作参数 由 parameters_reload() 从配置文件加载
// 工作参数为全局对象 struct.h::rdp_parameter（由 parameters_reload() 加载），不再作为类成员。
int Beam_num; //波位数
int beam_count; //波位计数
int data_num; //波位1点数
data_process_class_dll/dot_coh.cpp
+2 -4
View File
@@ -7,8 +7,7 @@
using namespace std;
int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
std::vector <PointRecv> *point_recv,
const RDP_parameter& rdp_parameter)
std::vector <PointRecv> *point_recv)
{
if(data_input->size()>1)
@@ -99,8 +98,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, //输出点迹
const RDP_parameter& rdp_parameter //工作参数
std::vector <PointRecv> *point_recv //输出点迹
)
{
data_process_class_dll/dot_coh.h
+2 -4
View File
@@ -14,14 +14,12 @@ public:
// 点迹凝聚函数
int dot_coh_process( std::vector <PointRecv> *data_input, //输入的点迹
std::vector <PointRecv> *point_recv, //输出点迹
const RDP_parameter& rdp_parameter //工作参数
std::vector <PointRecv> *point_recv //输出点迹
);
//点迹凝聚函数，缓存一帧，一边输出，一边进行滑窗凝聚
int dot_coh_process_buff( std::vector <PointRecv> *data_input, //输入的点迹
std::vector <PointRecv> *point_recv, //输出点迹
const RDP_parameter& rdp_parameter //工作参数
std::vector <PointRecv> *point_recv //输出点迹
);
//构造函数
data_process_class_dll/dot_coh_tas.cpp
+1 -2
View File
@@ -6,8 +6,7 @@
using namespace std;
int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *data_input, //输入点迹
std::vector <PointRecv> *point_recv_tas, //输出点迹
const RDP_parameter& rdp_parameter)
std::vector <PointRecv> *point_recv_tas) //输出点迹
{
if (data_input->size() == 0) {
return 1;
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, //输出点迹
const RDP_parameter& rdp_parameter);
std::vector <PointRecv> *point_recv_tas //输出点迹
);
};
#endif // DOT_COH_TAS_H
data_process_class_dll/kalman.cpp
+16 -16
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], const RDP_parameter& rdp_parameter)
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4])
{
X[0]=Z1[0];
@@ -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, const RDP_parameter& rdp_parameter)
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
{
//X(k+1|k)
@@ -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], const RDP_parameter& rdp_parameter)
void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6])
{
double x0=Z0[0];
double y0=Z0[1];
@@ -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], const RDP_parameter& rdp_parameter)
double kalman::d_cal(double Z[2],double X[6],double P[6][6])
{
VectorXd X_pred(6);
MatrixXd P_pred(6,6);
@@ -240,7 +240,7 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6], const RDP_parameter
};
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)
double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind)
{
MatrixXd F1(6,6);
@@ -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, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//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, const RDP_parameter& rdp_parameter)
double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind)
{
MatrixXd F1(6,6);
@@ -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, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//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, const RDP_parameter& rdp_parameter )
double prt,double freq_ind )
{
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
@@ -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, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//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], const RDP_parameter& rdp_parameter)
double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2])
{
MatrixXd F1(6,6);
MatrixXd Q1(6,6);
@@ -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, const RDP_parameter& rdp_parameter)
double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind)
{
//X(k+1|k)
@@ -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, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//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, const RDP_parameter& rdp_parameter)
double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind)
{
//X(k+1|k)
Matrix4d F;
@@ -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, rdp_parameter);
double v_bind=Bind_speed(prt, freq_ind);
//d
Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr);
@@ -777,7 +777,7 @@ 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, const RDP_parameter& rdp_parameter)
double kalman::Bind_speed(double prt,double freq_ind)
{
double freq=rdp_parameter.freq_0_GHz+freq_ind*0.02;
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], const RDP_parameter& rdp_parameter);//两点初始化卡尔曼滤波器
void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4]);//两点初始化卡尔曼滤波器
double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T, const RDP_parameter& rdp_parameter); //计算点 临时航迹的d
double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T); //计算点 临时航迹的d
double d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind, const RDP_parameter& rdp_parameter);
double d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind);
double d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind, 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);
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);
void kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6]);
double d_cal(double Z[2],double X[6],double P[6][6], const RDP_parameter& rdp_parameter);
double d_cal(double Z[2],double X[6],double P[6][6]);
double d_cal_with_doppler(double F[6][6], double Q[6][6] ,double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind, 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_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);
double d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind);
void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2], 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_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, const RDP_parameter& rdp_parameter );
double prt,double freq_ind );
double Bind_speed(double prt,double freq_ind, const RDP_parameter& rdp_parameter); //根据PRT 和 频率 计算不模糊速度
double Bind_speed(double prt,double freq_ind); //根据PRT 和 频率 计算不模糊速度
};
#endif // KALMAN_H
data_process_class_dll/struct.h
+6
View File
@@ -278,4 +278,10 @@ struct RDP_parameter
float tas_prohibite_area_azimuth_max[30]; //最大方位角 （度）
};
// ******************* 全局工作参数对象 ************************//
// 所有处理模块直接访问该全局对象，不再通过函数参数逐层传递。
// 定义在 data_process.cpp；由 Data_Process::parameters_reload() 在配置文件完整校验通过后整体赋值
// （失败时保持原值）。处理期间只读；配置重载应与处理互斥，宿主按单线程接口使用（见 README BUG-03）。
extern RDP_parameter rdp_parameter;
#endif // STRUCT_H
data_process_class_dll/tas_ctrl.cpp
+9 -13
View File
@@ -27,14 +27,13 @@ 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,
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,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);
tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output);
tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output);
tas_beam_output(trust_track,Tracking_beam,latest_timestamp);
// //跟踪队列移位
// struct Tracking_Target tas_target_tmp;
@@ -48,8 +47,7 @@ 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,
const RDP_parameter& rdp_parameter)
int *Trust_track_num_Output)
{
for (size_t i=0;i<trust_track->size();i++)
@@ -63,10 +61,10 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
azi=azi/PI*180;
//进入跟踪的条件： 1.手动跟踪的目标 或 满足速度、距离、高度条件满足 关联点数大于2 2.不在禁止跟踪区域
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)
if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h)==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, rdp_parameter) == 0)
&& tas_prohibite_area( r, azi, v, h) == 0)
{
for(int j=0;j<RDP_TAS_QUEUE_CAPACITY;j++)
{
@@ -117,7 +115,7 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
// TAS 距离/速度/高度门限，这些参数未纳入新的 RDP_parameter，故原实现保持停用；
// 具体逻辑不再以注释代码形式保留（tas_ctrl.h 中保留声明，后续如需启用再按新参数补齐）。
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,const RDP_parameter& rdp_parameter)
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h)
{
for (int i=0;i<rdp_parameter.tas_prohibite_area_num;i++)
@@ -134,8 +132,7 @@ int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,const R
void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
const RDP_parameter& rdp_parameter)
int *Trust_track_num_Output)
{
//查找TAS目标是否已经消批 若消批则移出TAS队列
@@ -169,7 +166,7 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
// double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
// 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,rdp_parameter) == 1) //满足结束跟踪条件
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h) == 1) //满足结束跟踪条件
// {
// int track_index=(*trust_track)[i].Track_Index;
@@ -218,7 +215,6 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
const RDP_parameter& rdp_parameter,
long long latest_timestamp)
{
data_process_class_dll/tas_ctrl.h
+5 -9
View File
@@ -18,7 +18,6 @@ public:
struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
const RDP_parameter& rdp_parameter,
long long latest_timestamp);
private:
@@ -26,29 +25,26 @@ private:
//添加TAS目标
void tas_target_add(std::vector<Trust_Track> *trust_track,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
const RDP_parameter& rdp_parameter);
int *Trust_track_num_Output);
//删除TAS目标
void tas_target_del(std::vector<Trust_Track> *trust_track,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
const RDP_parameter& rdp_parameter);
int *Trust_track_num_Output);
//TAS队列信息输出
void tas_beam_output(std::vector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
const RDP_parameter& rdp_parameter,
long long latest_timestamp);
//TAS 自动开启条件
int tas_auto_start(double v, double r, double h, const RDP_parameter& rdp_parameter);
int tas_auto_start(double v, double r, double h);
//TAS 自动退出条件
int tas_auto_end(double v, double r, double h, const RDP_parameter& rdp_parameter);
int tas_auto_end(double v, double r, double h);
// 禁止跟踪区域判断
int tas_prohibite_area(double r,double azi, double v, double h,const RDP_parameter& rdp_parameter);
int tas_prohibite_area(double r,double azi, double v, double h);
struct Tracking_Target tas_target_queue[RDP_TAS_QUEUE_CAPACITY]; //跟踪目标信息队列
data_process_class_dll/track_asso.cpp
+17 -20
View File
@@ -19,8 +19,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
std::vector <Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
long long latest_timestamp, //最新时间戳
const RDP_parameter& rdp_parameter //工作参数
long long latest_timestamp //最新时间戳
)
{
@@ -35,7 +34,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
//IMM算法
model_interaction(trust_track);
model_filter(trust_track, rdp_parameter, latest_timestamp);
model_filter(trust_track, latest_timestamp);
model_output(trust_track);
@@ -288,7 +287,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],const RDP_parameter& rdp_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])
{
//F
@@ -428,20 +427,19 @@ void Track_Asso::IMM_d_cal(double v_track,
double X3[6], double P3[6][6],
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
const RDP_parameter& rdp_parameter)
double *d1,double *d2,double *d3)
{
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3);
kalman Kalman;
*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);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
}
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_parameter& rdp_parameter, long long latest_timestamp)
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track, long long latest_timestamp)
{
@@ -492,7 +490,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
//计算统计距离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,rdp_parameter);
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3);
//判定统计距离门限，thd_d
bool thd_d = false;
@@ -623,7 +621,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3);
if (delta_T > 0)
{
@@ -632,9 +630,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
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, 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);
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);
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
@@ -721,7 +719,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
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,rdp_parameter);
track_hight_update(track_index,point_index,trust_track);
(*trust_track)[track_index-1].Extrapolate_round = 0; //连续未用实点更新时间
(*trust_track)[track_index-1].point_flag=1; //实点
@@ -789,7 +787,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3);
//航迹X P
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
@@ -947,8 +945,7 @@ 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, //航迹
const RDP_parameter& rdp_parameter //工作参数
std::vector <Trust_Track> *trust_track //航迹
)
{
data_process_class_dll/track_asso.h
+4 -8
View File
@@ -16,8 +16,7 @@ public:
std::vector <Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
long long latest_timestamp, //最新时间戳
const RDP_parameter& rdp_parameter //工作参数
long long latest_timestamp //最新时间戳
);
Track_Asso();
@@ -31,7 +30,6 @@ private:
void model_interaction(std::vector <Trust_Track> *trust_track); //模型交互
void model_filter(std::vector <Trust_Track> *trust_track, //滤波
const RDP_parameter& rdp_parameter,
long long latest_timestamp
);
@@ -43,18 +41,16 @@ private:
double X3[6], double P3[6][6],
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
const RDP_parameter& rdp_parameter);
double *d1,double *d2,double *d3);
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);
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]);
double Pt[3][3]; //模型转移概率
//高度维更新
void track_hight_update(int updata_track_index, //更新的航迹号
int asso_point_index, //点迹号
std::vector <Trust_Track> *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
std::vector <Trust_Track> *trust_track //航迹
);
};
#endif // TRACK_ASSO_H
data_process_class_dll/track_asso_direct_tracking.cpp
+17 -21
View File
@@ -11,8 +11,7 @@ using namespace std;
int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<PointRecv> *point_recv, //点迹
Trust_Track *trust_track, //航迹
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
const RDP_parameter& rdp_parameter //工作参数
int *Trust_track_num_Output //更新航迹数
)
{
@@ -25,7 +24,7 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<P
//IMM算法
model_interaction(trust_track);
model_filter(trust_track, rdp_parameter);
model_filter(trust_track);
model_output(trust_track);
@@ -185,8 +184,7 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
}
void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, //滤波
const RDP_parameter& rdp_parameter)
void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track) //滤波
{
//存储关联信息
struct asso_info
@@ -234,7 +232,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,rdp_parameter);
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3);
//计算高度门限
double d_h;
@@ -337,7 +335,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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3);
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 +343,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, 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);
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);
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;jj++)
@@ -418,7 +416,7 @@ 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,rdp_parameter);
track_hight_update(point_index,trust_track);
}
//未关联上，航迹外推
@@ -426,7 +424,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
{
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3);
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 +508,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],const RDP_parameter& rdp_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])
{
//F
@@ -626,26 +624,24 @@ void Track_Asso_Direct_Tracking::IMM_d_cal(double v_track,
double X3[6], double P3[6][6],
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
const RDP_parameter& rdp_parameter)
double *d1,double *d2,double *d3)
{
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3);
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, 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);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
}
//高度维更新
void Track_Asso_Direct_Tracking::track_hight_update(int asso_point_index, //点迹号
Trust_Track *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
Trust_Track *trust_track //航迹
)
{
data_process_class_dll/track_asso_direct_tracking.h
+5 -9
View File
@@ -12,8 +12,7 @@ class Track_Asso_Direct_Tracking
int track_asso_process_direct_tracking(std::vector<PointRecv> *point_recv, //点迹
Trust_Track *trust_track, //航迹
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
const RDP_parameter& rdp_parameter //工作参数
int *Trust_track_num_Output //更新航迹数
);
Track_Asso_Direct_Tracking();
@@ -27,8 +26,7 @@ class Track_Asso_Direct_Tracking
//IMM
void model_interaction(Trust_Track *trust_track); //模型交互
void model_filter(Trust_Track *trust_track, //滤波
const RDP_parameter& rdp_parameter
void model_filter(Trust_Track *trust_track //滤波
);
void model_output(Trust_Track *trust_track); //模型输出
@@ -39,17 +37,15 @@ class Track_Asso_Direct_Tracking
double X3[6], double P3[6][6],
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
const RDP_parameter& rdp_parameter);
double *d1,double *d2,double *d3);
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);
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]);
double Pt[3][3]; //模型转移概率
//高度维更新
void track_hight_update(int asso_point_index, //点迹号
Trust_Track *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
Trust_Track *trust_track //航迹
);
};
data_process_class_dll/track_asso_tas.cpp
+15 -19
View File
@@ -16,7 +16,6 @@ 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, //更新航迹数
const RDP_parameter& rdp_parameter, //工作参数
int tas_track_idx,
long long latest_timestamp //最新时间戳
)
@@ -32,7 +31,7 @@ long long latest_timestamp //最新时间戳
//IMM算法
model_interaction(trust_track,tas_track_idx);
model_filter(trust_track, rdp_parameter,tas_track_idx, latest_timestamp);
model_filter(trust_track,tas_track_idx, latest_timestamp);
model_output(trust_track,tas_track_idx);
@@ -256,7 +255,6 @@ void Track_Asso_Tas:: model_interaction( std::vector<Trust_Track> *trust_track
}
void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *trust_track, //滤波
const RDP_parameter& rdp_parameter,
int tas_track_idx,
long long latest_timestamp //最新时间戳
)
@@ -322,7 +320,7 @@ 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,rdp_parameter);
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3);
//判定统计距离门限，thd_d
bool thd_d = false;
@@ -418,7 +416,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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3);
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 +424,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, 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);
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);
Matrix3f S1_out,S2_out,S3_out;
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;jj++)
@@ -511,7 +509,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,rdp_parameter);
track_hight_update(tas_track_idx,point_index,trust_track);
}
}
@@ -539,7 +537,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3);
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 +646,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],const RDP_parameter& rdp_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])
{
//F
@@ -764,27 +762,25 @@ double X2[6], double P2[6][6],
double X3[6], double P3[6][6],
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
const RDP_parameter& rdp_parameter)
double *d1,double *d2,double *d3)
{
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,rdp_parameter);
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3);
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, 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);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
}
//高度维更新
void Track_Asso_Tas::track_hight_update(int tas_track_idx, //更新的航迹号
int asso_point_index, //点迹号
std::vector<Trust_Track> *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
std::vector<Trust_Track> *trust_track //航迹
)
{
for(size_t i=0; i<trust_track->size(); i++)
data_process_class_dll/track_asso_tas.h
+3 -7
View File
@@ -15,7 +15,6 @@ public:
std::vector<Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
const RDP_parameter& rdp_parameter, //工作参数
int tas_track_idx,
long long latest_timestamp //最新时间戳
);
@@ -31,7 +30,6 @@ private:
void model_interaction(std::vector<Trust_Track> *trust_track, int tas_track_idx); //模型交互
void model_filter(std::vector<Trust_Track> *trust_track, //滤波
const RDP_parameter& rdp_parameter,
int tas_track_idx,
long long latest_timestamp //最新时间戳
);
@@ -44,18 +42,16 @@ private:
double X3[6], double P3[6][6],
double Z[3], double prt,double freq_ind,
double delta_T,
double *d1,double *d2,double *d3,
const RDP_parameter& rdp_parameter);
double *d1,double *d2,double *d3);
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);
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]);
double Pt[3][3]; //模型转移概率
//高度维更新
void track_hight_update(int tas_track_idx, //更新的航迹号
int asso_point_index, //点迹号
std::vector<Trust_Track> *trust_track, //航迹
const RDP_parameter& rdp_parameter //工作参数
std::vector<Trust_Track> *trust_track //航迹
);
};
data_process_class_dll/track_die.cpp
+1 -2
View File
@@ -8,8 +8,7 @@ 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,
const RDP_parameter& rdp_parameter)
int *Track_die_num_Output)
{
std::vector <Trust_Track>::iterator Iter;
data_process_class_dll/track_die.h
+1 -2
View File
@@ -11,8 +11,7 @@ class Track_Die
public:
void track_die_process( std::vector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output,
const RDP_parameter& rdp_parameter);
int *Track_die_num_Output);
};
#endif // TRACK_DIE_H
data_process_class_dll/track_die_tas.cpp
+1 -2
View File
@@ -11,8 +11,7 @@ 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,
const RDP_parameter& rdp_parameter)
int *Track_die_num_Output)
{
std::vector <Trust_Track>::iterator Iter;
data_process_class_dll/track_die_tas.h
+1 -2
View File
@@ -11,8 +11,7 @@ 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,
const RDP_parameter& rdp_parameter);
int *Track_die_num_Output);
};
#endif // TRACK_DIE_TAS_H
data_process_class_dll/track_index_mangement.cpp
+1 -2
View File
@@ -11,8 +11,7 @@ void Track_Ind_Mangement::reset()
lastest_index = 0;
}
int Track_Ind_Mangement::track_ind_get( std::vector<Trust_Track> *trust_track,
const RDP_parameter& rdp_parameter)
int Track_Ind_Mangement::track_ind_get( std::vector<Trust_Track> *trust_track)
{
int isempty = 1;
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, const RDP_parameter& rdp_parameter);
int track_ind_get( std::vector<Trust_Track> *trust_track);
void reset();
private:
data_process_class_dll/track_init.cpp
+14 -18
View File
@@ -13,8 +13,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
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,
const RDP_parameter& rdp_parameter
int *Trust_track_num_Output
)
{
@@ -35,10 +34,10 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
if(point_process.size()>0)
{
//点迹与临时航迹关联
point_temp_track_asso(temp_track,rdp_parameter);
point_temp_track_asso(temp_track);
//点迹与航迹头关联
point_track_head_asso(temp_track,rdp_parameter);
point_track_head_asso(temp_track);
//删除关联上的点迹
std::vector <PointRecv>::iterator Iter;
@@ -118,7 +117,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,rdp_parameter);
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output);
}
@@ -129,8 +128,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,
const RDP_parameter& rdp_parameter)
void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track)
{
//关联信息
@@ -179,7 +177,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,rdp_parameter);
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
//计算夹角
@@ -251,7 +249,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,rdp_parameter);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -314,8 +312,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,
const RDP_parameter& rdp_parameter)
void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track)
{
//关联上的信息
@@ -434,7 +431,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,rdp_parameter);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -473,8 +470,7 @@ double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,
void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_track,
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
const RDP_parameter& rdp_parameter)
int *Trust_track_num_Output)
{
std::vector <std::vector<Temp_track>> Track_to_start;
@@ -519,7 +515,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
}
//航迹起批禁止区域判断
if(track_init_prohibit(range,azi,rdp_parameter)!=0)
if(track_init_prohibit(range,azi)!=0)
{
track_init_flag = false;
}
@@ -634,7 +630,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
if (Track_to_start[i].size() < 3) continue;
if(trust_track->size() < static_cast<std::size_t>(rdp_parameter.track_index_max))
{
int index = track_index_mangement.track_ind_get(trust_track, rdp_parameter);
int index = track_index_mangement.track_ind_get(trust_track);
if(index>0)
{
@@ -647,7 +643,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,rdp_parameter);
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
Trust_Track trust_track_tmp = {};
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
@@ -783,7 +779,7 @@ void Track_Init::tmp_track_die(std::vector <std::vector<Temp_track>> *temp_t
}
};
int Track_Init::track_init_prohibit(double r, double azi, const RDP_parameter& rdp_parameter)
int Track_Init::track_init_prohibit(double r, double azi)
{
for (int i=0;i<rdp_parameter.track_init_prohibite_area_num;i++)
data_process_class_dll/track_init.h
+5 -9
View File
@@ -18,8 +18,7 @@ public:
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,
const RDP_parameter& rdp_parameter
int *Trust_track_num_Output
);
Track_Init();
@@ -31,23 +30,20 @@ private:
std::vector<PointRecv> point_process; //要处理的点迹
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
const RDP_parameter& rdp_parameter); //临时航迹与点迹关联
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track); //临时航迹与点迹关联
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
const RDP_parameter& rdp_parameter); //航迹头与点迹关联
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track); //航迹头与点迹关联
void tmp_track_to_trust_track( std::vector <Trust_Track> *trust_track, //临时航迹转可靠航迹
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
const RDP_parameter& rdp_parameter);
int *Trust_track_num_Output);
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, const RDP_parameter& rdp_parameter); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0
int track_init_prohibit(double r, double azi); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0
Track_Ind_Mangement track_index_mangement;
data_process_class_dll/track_init_direct_tracking.cpp
+6 -10
View File
@@ -13,7 +13,6 @@ 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,
const RDP_parameter& rdp_parameter,
int track_ID
)
{
@@ -22,8 +21,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,
const RDP_parameter& rdp_parameter) //临时航迹与点迹关联
void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track) //临时航迹与点迹关联
{
//关联信息
struct Asso_info
@@ -70,7 +68,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,rdp_parameter);
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
//计算夹角
@@ -134,7 +132,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,rdp_parameter);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -163,8 +161,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,
const RDP_parameter& rdp_parameter) //航迹头与点迹关联
void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track) //航迹头与点迹关联
{
//关联上的信息
struct Asso_info
@@ -270,7 +267,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,rdp_parameter);
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
@@ -299,7 +296,6 @@ 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,
const RDP_parameter& rdp_parameter,
int track_ID)
{
//temp_track中的临时航迹转为可靠航迹
@@ -326,7 +322,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,rdp_parameter);
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
Trust_Track trust_track_tmp = {};
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
data_process_class_dll/track_init_direct_tracking.h
+2 -6
View File
@@ -14,7 +14,6 @@ public:
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
const RDP_parameter& rdp_parameter,
int track_ID
);
@@ -25,17 +24,14 @@ private:
std::vector<PointRecv> point_process; //要处理的点迹
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
const RDP_parameter& rdp_parameter); //临时航迹与点迹关联
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track); //临时航迹与点迹关联
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
const RDP_parameter& rdp_parameter); //航迹头与点迹关联
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track); //航迹头与点迹关联
void tmp_track_to_trust_track( std::vector<Trust_Track> *trust_track, //临时航迹转可靠航迹
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
const RDP_parameter& rdp_parameter,
int track_ID
);