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

更新：1、增加临时航迹录入数据库；

Browse Source 
2、增加点迹信息的圈ID记录；

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-09-30 16:33:18 +08:00
parent a4f32e4036
commit c3c7e668d2
8 changed files with 212 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/data_process.cpp
+4 -2
View File
@@ -31,6 +31,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
PointRecv Data_buffer_temp; PointRecv Data_buffer_temp;
Data_buffer_temp.point_id = point_id++; Data_buffer_temp.point_id = point_id++;
Data_buffer_temp.frame_id = frame_id; Data_buffer_temp.frame_id = frame_id;
Data_buffer_temp.circle_id = circle_id;
Data_buffer_temp.amplitude=Data_Input[i].Amplitude; Data_buffer_temp.amplitude=Data_Input[i].Amplitude;
Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI; Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI;
Data_buffer_temp.SNR = Data_Input[i].Snr; Data_buffer_temp.SNR = Data_Input[i].Snr;
@@ -65,7 +66,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
if(Data_Input[0].Beam_index_aiz == 0) //TWS if(Data_Input[0].Beam_index_aiz == 0) //TWS
{ {
last_beam_num = Data_Input[0].Beam_index_aiz; last_beam_num = Data_Input[0].Beam_index_aiz;
circle_id++;
return 1; return 1;
} }
else else
@@ -87,6 +88,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
PointRecv Data_buffer_temp; PointRecv Data_buffer_temp;
Data_buffer_temp.point_id = point_id++; Data_buffer_temp.point_id = point_id++;
Data_buffer_temp.frame_id = frame_id; Data_buffer_temp.frame_id = frame_id;
Data_buffer_temp.circle_id = circle_id;
Data_buffer_temp.amplitude=Data_Input[i].Amplitude; Data_buffer_temp.amplitude=Data_Input[i].Amplitude;
Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI; Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI;
Data_buffer_temp.SNR = Data_Input[i].Snr; Data_buffer_temp.SNR = Data_Input[i].Snr;
@@ -120,7 +122,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
// << "data_num:" << data_num; // << "data_num:" << data_num;
} }
circle_id++;
return 3; return 3;
} }
data_process_class_dll/data_process.h
+2
View File
@@ -102,6 +102,8 @@ private:
long long frame_id = 0; //帧id long long frame_id = 0; //帧id
long long circle_id = 0; //圈id
struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入 struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入
Dot_Coh dot_coh; //点迹凝聚 Dot_Coh dot_coh; //点迹凝聚
data_process_class_dll/rdp_record.cpp
+7 -3
View File
@@ -231,20 +231,24 @@ namespace rdp_record
// RDP_RECORD_DEFINE(结构体名); // RDP_RECORD_DEFINE(结构体名);
//输入点迹 //输入点迹
YLT_REFL(RdpRecPoint, id,point_id,frame_id,range,azimuth, velocity,amplitude ,height,SNR,RCS, YLT_REFL(RdpRecPoint, id,point_id,frame_id,circle_id,range,azimuth, velocity,amplitude ,height,SNR,RCS,
encoder_value,amp_asv_point,prn_fftn_point,CPI_time,GNSS_time,use_flag,azimuth_beam_index,pitch_beam_index,PRF_index,freq_index,point_type,point_section_asso); encoder_value,amp_asv_point,prn_fftn_point,CPI_time,GNSS_time,use_flag,azimuth_beam_index,pitch_beam_index,PRF_index,freq_index,point_type,point_section_asso);
RDP_RECORD_DEFINE(RdpRecPoint); RDP_RECORD_DEFINE(RdpRecPoint);
//TWS凝聚后点迹 //TWS凝聚后点迹
YLT_REFL(RdpRecPointCohesionTWS, id,point_id,frame_id,range,azimuth, velocity,amplitude ,height,SNR,RCS, YLT_REFL(RdpRecPointCohesionTWS, id,point_id,frame_id,circle_id,range,azimuth, velocity,amplitude ,height,SNR,RCS,
encoder_value,amp_asv_point,prn_fftn_point,CPI_time,GNSS_time,use_flag,azimuth_beam_index,pitch_beam_index,PRF_index,freq_index,point_type,point_section_asso); encoder_value,amp_asv_point,prn_fftn_point,CPI_time,GNSS_time,use_flag,azimuth_beam_index,pitch_beam_index,PRF_index,freq_index,point_type,point_section_asso);
RDP_RECORD_DEFINE(RdpRecPointCohesionTWS); RDP_RECORD_DEFINE(RdpRecPointCohesionTWS);
//TAS凝聚后点迹 //TAS凝聚后点迹
YLT_REFL(RdpRecPointCohesionTAS, id,point_id,frame_id,range,azimuth, velocity,amplitude ,height,SNR,RCS, YLT_REFL(RdpRecPointCohesionTAS, id,point_id,frame_id,circle_id,range,azimuth, velocity,amplitude ,height,SNR,RCS,
encoder_value,amp_asv_point,prn_fftn_point,CPI_time,GNSS_time,use_flag,azimuth_beam_index,pitch_beam_index,PRF_index,freq_index,point_type,point_section_asso); encoder_value,amp_asv_point,prn_fftn_point,CPI_time,GNSS_time,use_flag,azimuth_beam_index,pitch_beam_index,PRF_index,freq_index,point_type,point_section_asso);
RDP_RECORD_DEFINE(RdpRecPointCohesionTAS); RDP_RECORD_DEFINE(RdpRecPointCohesionTAS);
//临时航迹
YLT_REFL(RdpRecTempTrack, id, temp_track_id,point_id,circle_id,X,P,r,azi,vr,height,SNR,RCS,amplitude, CPI_time,GNSS_time,d,buff_round,temp_track_section_idx,asso_flag,pitch_num);
RDP_RECORD_DEFINE(RdpRecTempTrack);
// 示例②：本圈输出的航迹 // 示例②：本圈输出的航迹
YLT_REFL(RdpRecTrackOut, id, track_index, track_mode, point_sum, flag_point, flag_direction, YLT_REFL(RdpRecTrackOut, id, track_index, track_mode, point_sum, flag_point, flag_direction,
gnss_time_ms, track_time_s, x, y, z, v_x, v_y, v_z, range_m, azi_deg, elev_deg, gnss_time_ms, track_time_s, x, y, z, v_x, v_y, v_z, range_m, azi_deg, elev_deg,
data_process_class_dll/rdp_record_entity.h
+119
View File
@@ -21,6 +21,11 @@
#include "rdp_record.h" #include "rdp_record.h"
#include <cstdint> #include <cstdint>
#include <vector> #include <vector>
#include <iostream>
#include <sstream>
#include <string>
#include <type_traits>
#include <cstddef>
#include "struct.h" #include "struct.h"
@@ -30,6 +35,18 @@
extern template void rdp_record::Write<Entity>(const Entity&); \ extern template void rdp_record::Write<Entity>(const Entity&); \
extern template void rdp_record::Write<Entity>(const std::vector<Entity>&) extern template void rdp_record::Write<Entity>(const std::vector<Entity>&)
// 基本类型元素转字符串
template <typename T>
std::string elementToString(const T& value);
// 数组版本
template <typename T, std::size_t N>
std::string arrayToString(const T (&arr)[N]);
// 非数组元素：直接转字符串
template <typename T>
std::string arrayToString(const T& value);
// ============================ 记录实体定义区（在此新增结构体） ============================ // ============================ 记录实体定义区（在此新增结构体） ============================
// 输入点迹 —— 输入的原始点迹数据 // 输入点迹 —— 输入的原始点迹数据
struct RdpRecPoint struct RdpRecPoint
@@ -37,6 +54,7 @@ struct RdpRecPoint
int64_t id; //自增主键（写入时固定填 0） int64_t id; //自增主键（写入时固定填 0）
long long point_id; //点迹ID long long point_id; //点迹ID
long long frame_id; //帧ID long long frame_id; //帧ID
long long circle_id; //圈ID
double range; //径向距离 double range; //径向距离
double azimuth; //方位 (弧度) double azimuth; //方位 (弧度)
double velocity ; //速度 double velocity ; //速度
@@ -65,6 +83,7 @@ struct RdpRecPoint
id = 0; id = 0;
point_id = p.point_id; point_id = p.point_id;
frame_id = p.frame_id; frame_id = p.frame_id;
circle_id = p.circle_id;
range = p.range; range = p.range;
azimuth = p.azimuth; azimuth = p.azimuth;
velocity = p.velocity; velocity = p.velocity;
@@ -94,6 +113,7 @@ struct RdpRecPointCohesionTWS
int64_t id; //自增主键（写入时固定填 0） int64_t id; //自增主键（写入时固定填 0）
long long point_id; //点迹ID long long point_id; //点迹ID
long long frame_id; //帧ID long long frame_id; //帧ID
long long circle_id; //圈ID
double range; //径向距离 double range; //径向距离
double azimuth; //方位 (弧度) double azimuth; //方位 (弧度)
double velocity ; //速度 double velocity ; //速度
@@ -122,6 +142,7 @@ struct RdpRecPointCohesionTWS
id = 0; id = 0;
point_id = p.point_id; point_id = p.point_id;
frame_id = p.frame_id; frame_id = p.frame_id;
circle_id = p.circle_id;
range = p.range; range = p.range;
azimuth = p.azimuth; azimuth = p.azimuth;
velocity = p.velocity; velocity = p.velocity;
@@ -151,6 +172,7 @@ struct RdpRecPointCohesionTAS
int64_t id; //自增主键（写入时固定填 0） int64_t id; //自增主键（写入时固定填 0）
long long point_id; //点迹ID long long point_id; //点迹ID
long long frame_id; //帧ID long long frame_id; //帧ID
long long circle_id; //圈ID
double range; //径向距离 double range; //径向距离
double azimuth; //方位 (弧度) double azimuth; //方位 (弧度)
double velocity ; //速度 double velocity ; //速度
@@ -179,6 +201,7 @@ struct RdpRecPointCohesionTAS
id = 0; id = 0;
point_id = p.point_id; point_id = p.point_id;
frame_id = p.frame_id; frame_id = p.frame_id;
circle_id = p.circle_id;
range = p.range; range = p.range;
azimuth = p.azimuth; azimuth = p.azimuth;
velocity = p.velocity; velocity = p.velocity;
@@ -202,6 +225,61 @@ struct RdpRecPointCohesionTAS
}; };
RDP_RECORD_DECLARE(RdpRecPointCohesionTAS); RDP_RECORD_DECLARE(RdpRecPointCohesionTAS);
// 零时航迹
struct RdpRecTempTrack
{
int64_t id; //自增主键（写入时固定填 0）
long long temp_track_id; //临时航迹ID
long long point_id; //关联上的点迹ID
long long circle_id; //处理圈数ID
std::string X; //状态
std::string P; //协方差
double r; //距离
double azi; //方位
double vr; //径向速度
double height; //高度
double SNR; //信噪比
double RCS; //RCS
double amplitude; //幅度
long long CPI_time; //时间戳
long long GNSS_time; //GNSS时间戳
double d; //关联上的点的d（马氏距离）
int buff_round; //已缓存圈数
int temp_track_section_idx; //临时航迹区号 与点迹区的划分相同
int asso_flag; //关联标志 已关联：1 未关联：0
int pitch_num; // 俯仰波位号
RdpRecTempTrack(){
id = 0;
}
RdpRecTempTrack(Temp_track& p){
id = 0;
temp_track_id = p.temp_track_id;
point_id = p.point_id;
circle_id = p.circle_id;
X = arrayToString(p.X);
P = arrayToString(p.P);
r = p.r;
azi = p.azi;
vr = p.vr;
height = p.height;
RCS = p.RCS;
SNR = p.SNR;
amplitude = p.amplitude;
CPI_time = p.CPI_time;
GNSS_time = p.GNSS_time;
d = p.d;
buff_round = p.buff_round;
temp_track_section_idx = p.temp_track_section_idx;
asso_flag = p.asso_flag;
pitch_num = p.pitch_num;
}
};
RDP_RECORD_DECLARE(RdpRecTempTrack);
// 示例②：本圈输出的航迹 —— 记录点位于 data_process.cpp::track_process()「数据记录示例②」处， // 示例②：本圈输出的航迹 —— 记录点位于 data_process.cpp::track_process()「数据记录示例②」处，
// TWS/TAS 共用一张表并用 track_mode 区分；每行取输出数组的 [i][0] // TWS/TAS 共用一张表并用 track_mode 区分；每行取输出数组的 [i][0]
struct RdpRecTrackOut struct RdpRecTrackOut
@@ -229,4 +307,45 @@ struct RdpRecTrackOut
}; };
RDP_RECORD_DECLARE(RdpRecTrackOut); RDP_RECORD_DECLARE(RdpRecTrackOut);
// ============================ 数组转换函数 ============================
// 基本类型元素转字符串
template <typename T>
std::string elementToString(const T& value) {
std::ostringstream oss;
oss << value;
return oss.str();
}
// 前置声明数组版本
template <typename T, std::size_t N>
std::string arrayToString(const T (&arr)[N]);
// 非数组元素：直接转字符串
template <typename T>
std::string arrayToString(const T& value) {
return elementToString(value);
}
// 数组版本
template <typename T, std::size_t N>
std::string arrayToString(const T (&arr)[N]) {
std::string result;
// 如果 T 仍然是数组，说明当前不是最内层，用分号；
// 否则当前就是最内层，用逗号。
const char sep = std::is_array<T>::value ? ';' : ',';
for (std::size_t i = 0; i < N; ++i) {
if (i > 0) {
result += sep;
}
result += arrayToString(arr[i]);
}
return result;
}
#endif // RDP_RECORD_ENTITY_H #endif // RDP_RECORD_ENTITY_H
data_process_class_dll/struct.h
+9 -5
View File
@@ -11,6 +11,7 @@ struct PointRecv
{ {
long long point_id; //点迹ID long long point_id; //点迹ID
long long frame_id; //帧ID long long frame_id; //帧ID
long long circle_id; //圈ID
double range; //径向距离 double range; //径向距离
double azimuth; //方位 (弧度) double azimuth; //方位 (弧度)
double velocity ; //速度 double velocity ; //速度
@@ -100,6 +101,9 @@ struct Trust_Track
//临时航迹结构体 //临时航迹结构体
struct Temp_track struct Temp_track
{ {
long long temp_track_id; //临时航迹ID
long long point_id; //关联上的点迹ID
long long circle_id; //处理圈数ID
double X[4]; //状态 double X[4]; //状态
double P[4][4]; //协方差 double P[4][4]; //协方差
@@ -108,17 +112,17 @@ struct Temp_track
double azi; //方位 double azi; //方位
double vr; //径向速度 double vr; //径向速度
double height; //高度 double height; //高度
double snr; //信噪比 double SNR; //信噪比
double RCS; //RCS double RCS; //RCS
double Amp; //幅度 double amplitude; //幅度
long long T; //时间戳 long long CPI_time; //时间戳
long long GNSS_time; //GNSS时间戳 long long GNSS_time; //GNSS时间戳
double d; //关联上的点的d double d; //关联上的点的d（马氏距离）
int buff_round; //已缓存圈数 int buff_round; //已缓存圈数
int Temp_track_section_idx; //临时航迹区号 与点迹区的划分相同 int temp_track_section_idx; //临时航迹区号 与点迹区的划分相同
int asso_flag; //关联标志 已关联：1 未关联：0 int asso_flag; //关联标志 已关联：1 未关联：0
data_process_class_dll/track_init.cpp
+47 -68
View File
@@ -6,6 +6,8 @@
#include <vector> #include <vector>
#include <iostream> #include <iostream>
#include<iomanip> #include<iomanip>
#include "rdp_record.h"
#include "rdp_record_entity.h"
using namespace std; using namespace std;
int Track_Init::track_init_process_logic( std::vector <PointRecv> *point_recv, //输入点迹 int Track_Init::track_init_process_logic( std::vector <PointRecv> *point_recv, //输入点迹
@@ -15,27 +17,12 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
int *Trust_track_num_Output, int *Trust_track_num_Output,
struct RadarPara Work_Parameter struct RadarPara Work_Parameter
) )
{ {
// for(int i=0;i<(*point_recv).size();i++)
// {
// if((*point_recv)[i].CPI_Time == 30675948 || (*point_recv)[i].CPI_Time == 30682320
// || (*point_recv)[i].CPI_Time == 30682132 || (*point_recv)[i].CPI_Time == 30688692
// || (*point_recv)[i].CPI_Time == 30691692 || (*point_recv)[i].CPI_Time == 30691880)
// {
// std::cout << "azi=" << (*point_recv)[i].Azimuth
// << "dis=" << (*point_recv)[i].Range
// << "h=" << (*point_recv)[i].Height
// << "v=" << (*point_recv)[i].Velocity
// << "wave=" << (*point_recv)[i].beam_index
// << "time=" << (*point_recv)[i].CPI_Time;
// }
// }
//取出待起航的点迹数据 //取出待起航的点迹数据
for (size_t i=0;i<(*point_recv).size();i++) for (size_t i=0;i<(*point_recv).size();i++){
point_process.push_back((*point_recv)[i]); point_process.push_back((*point_recv)[i]);
}
//临时航迹的buff_round+1 //临时航迹的buff_round+1
for (size_t i=0;i<temp_track->size();i++) for (size_t i=0;i<temp_track->size();i++)
@@ -78,12 +65,15 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
for (unsigned int i=0;i<point_process.size();i++) for (unsigned int i=0;i<point_process.size();i++)
{ {
Temp_track temp_track_tmp = {}; Temp_track temp_track_tmp = {};
temp_track_tmp.temp_track_id = temp_track_id++;
temp_track_tmp.point_id = point_process[i].point_id;
temp_track_tmp.circle_id = point_process[i].circle_id;
temp_track_tmp.r = point_process[i].range; temp_track_tmp.r = point_process[i].range;
temp_track_tmp.azi = point_process[i].azimuth; temp_track_tmp.azi = point_process[i].azimuth;
temp_track_tmp.height = point_process[i].height; temp_track_tmp.height = point_process[i].height;
temp_track_tmp.Amp = point_process[i].amplitude; temp_track_tmp.amplitude = point_process[i].amplitude;
temp_track_tmp.vr = point_process[i].velocity; temp_track_tmp.vr = point_process[i].velocity;
temp_track_tmp.snr = point_process[i].SNR; temp_track_tmp.SNR = point_process[i].SNR;
temp_track_tmp.RCS = point_process[i].RCS; temp_track_tmp.RCS = point_process[i].RCS;
temp_track_tmp.pitch_num = point_process[i].pitch_beam_index; temp_track_tmp.pitch_num = point_process[i].pitch_beam_index;
std::memcpy(temp_track_tmp.speed_dim, point_process[i].speed_dim, sizeof(temp_track_tmp.speed_dim)); std::memcpy(temp_track_tmp.speed_dim, point_process[i].speed_dim, sizeof(temp_track_tmp.speed_dim));
@@ -93,7 +83,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
temp_track_tmp.X[1]=0; temp_track_tmp.X[1]=0;
temp_track_tmp.X[2]=point_process[i].range*sin(point_process[i].azimuth); temp_track_tmp.X[2]=point_process[i].range*sin(point_process[i].azimuth);
temp_track_tmp.X[3]=0; temp_track_tmp.X[3]=0;
temp_track_tmp.T = point_process[i].CPI_time; temp_track_tmp.CPI_time = point_process[i].CPI_time;
temp_track_tmp.GNSS_time = point_process[i].GNSS_time; temp_track_tmp.GNSS_time = point_process[i].GNSS_time;
temp_track_tmp.buff_round = 1; temp_track_tmp.buff_round = 1;
// temp_track_tmp.Temp_track_section_idx = floor(point_process[i].beam_index/BEAM_NUM_DOT_SECTION)+1; // temp_track_tmp.Temp_track_section_idx = floor(point_process[i].beam_index/BEAM_NUM_DOT_SECTION)+1;
@@ -125,19 +115,14 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
//消亡临时航迹 //消亡临时航迹
tmp_track_die(temp_track); tmp_track_die(temp_track);
// std::cout << "-----------------------------------------------"; #ifdef RDP_RECORD_ENABLE
// for(int i=0;i<temp_track->size();i++) //记录临时航迹
// { for(auto& vec_t : *temp_track){
// for(int j=0;j<(*temp_track)[i].size();j++) for(auto& t : vec_t){
// { RDP_RECORD(RdpRecTempTrack(t));
// std::cout << "temp_track_azi=" << (*temp_track)[i][j].azi }
// << "temp_track_dis=" << (*temp_track)[i][j].r }
// << "temp_track_h=" << (*temp_track)[i][j].height #endif
// << "temp_track_v=" << (*temp_track)[i][j].vr
// << "temp_track_time=" << (*temp_track)[i][j].T;
// }
// }
return 0; return 0;
} }
@@ -184,7 +169,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
double v_temp_track=(*temp_track)[j][L-1].vr; double v_temp_track=(*temp_track)[j][L-1].vr;
double r_temp_track=(*temp_track)[j][L-1].r; double r_temp_track=(*temp_track)[j][L-1].r;
double h_temp_track=(*temp_track)[j][L-1].height; double h_temp_track=(*temp_track)[j][L-1].height;
double T_track_head = static_cast<double>((*temp_track)[j][L-1].T); double T_track_head = static_cast<double>((*temp_track)[j][L-1].CPI_time);
double delta_T = (T_point - T_track_head)/1000.0; double delta_T = (T_point - T_track_head)/1000.0;
@@ -204,20 +189,6 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
double y2=Z[0]*sin(Z[1]); double y2=Z[0]*sin(Z[1]);
double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2); double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2);
// if(T_track_head == 33855816 && T_point == 33859004 )
// {
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "r_temp_track=" << r_temp_track
// << "v_temp_track=" << v_temp_track
// << "h_temp_track=" << h_temp_track
// << "T_track_head=" << T_track_head
// << "d=" << d
// << "alpha=" << alpha;
// }
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<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha<ALPHA_START && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
{ {
struct Asso_info asso_info_tmp; struct Asso_info asso_info_tmp;
@@ -245,18 +216,22 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
{ {
(*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][j]); (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][j]);
(*temp_track)[(*temp_track).size()-1][j].asso_flag = 0; (*temp_track)[(*temp_track).size()-1][j].asso_flag = 0;
(*temp_track)[(*temp_track).size()-1][j].temp_track_id = temp_track_id;
} }
//关联上的点 //关联上的点
Temp_track asso_track_info_tmp = {}; Temp_track asso_track_info_tmp = {};
asso_track_info_tmp.temp_track_id = temp_track_id++;
asso_track_info_tmp.point_id = point_process[asso_info[i].point_idx-1].point_id;
asso_track_info_tmp.circle_id = point_process[asso_info[i].point_idx-1].circle_id;
asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range; asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range;
asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth; asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth;
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height; asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude; asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude;
asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_time; asso_track_info_tmp.CPI_time = point_process[asso_info[i].point_idx-1].CPI_time;
asso_track_info_tmp.GNSS_time = point_process[asso_info[i].point_idx-1].GNSS_time; asso_track_info_tmp.GNSS_time = point_process[asso_info[i].point_idx-1].GNSS_time;
asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity; asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity;
asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].SNR; asso_track_info_tmp.SNR = point_process[asso_info[i].point_idx-1].SNR;
asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS;
asso_track_info_tmp.asso_flag=0; asso_track_info_tmp.asso_flag=0;
asso_track_info_tmp.buff_round = 1; asso_track_info_tmp.buff_round = 1;
@@ -272,7 +247,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
double X[4]; double X[4];
double P[4][4]; double P[4][4];
kalman Kalman; kalman Kalman;
double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].T)/1000.0; double 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);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
@@ -340,7 +315,7 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
y_track_head=(*temp_track)[j][0].X[2]; y_track_head=(*temp_track)[j][0].X[2];
double v_track_head=(*temp_track)[j][0].vr; double v_track_head=(*temp_track)[j][0].vr;
double h_track_head = (*temp_track)[j][0].height; double h_track_head = (*temp_track)[j][0].height;
double T_track_head = static_cast<double>((*temp_track)[j][0].T); double T_track_head = static_cast<double>((*temp_track)[j][0].CPI_time);
//距离差 //距离差
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
@@ -392,17 +367,21 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
//第一个点 //第一个点
(*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]); (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]);
(*temp_track)[(*temp_track).size()-1][0].asso_flag = 0; (*temp_track)[(*temp_track).size()-1][0].asso_flag = 0;
(*temp_track)[(*temp_track).size()-1][0].temp_track_id = temp_track_id;
//第二个点 //第二个点
struct Temp_track asso_track_info_tmp = {}; struct Temp_track asso_track_info_tmp = {};
asso_track_info_tmp.temp_track_id = temp_track_id++;
asso_track_info_tmp.point_id = point_process[asso_info[i].point_idx-1].point_id;
asso_track_info_tmp.circle_id = point_process[asso_info[i].point_idx-1].circle_id;
asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range; asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range;
asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth; asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth;
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height; asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude; asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude;
asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity; asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity;
asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].SNR; asso_track_info_tmp.SNR = point_process[asso_info[i].point_idx-1].SNR;
asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS;
asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_time; asso_track_info_tmp.CPI_time = point_process[asso_info[i].point_idx-1].CPI_time;
asso_track_info_tmp.GNSS_time = point_process[asso_info[i].point_idx-1].GNSS_time; asso_track_info_tmp.GNSS_time = point_process[asso_info[i].point_idx-1].GNSS_time;
asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_beam_index; asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_beam_index;
std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim));
@@ -417,7 +396,7 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
double X[4]; double X[4];
double P[4][4]; double P[4][4];
kalman Kalman; kalman Kalman;
double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].T)/1000.0; double 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);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
@@ -514,9 +493,9 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
if (Track_to_start[i].size() < 3 || Track_to_start[j].size() < 3) if (Track_to_start[i].size() < 3 || Track_to_start[j].size() < 3)
continue; continue;
if( (Track_to_start[i][0].T == Track_to_start[j][0].T && Track_to_start[i][0].r == Track_to_start[j][0].r) if( (Track_to_start[i][0].CPI_time == Track_to_start[j][0].CPI_time && Track_to_start[i][0].r == Track_to_start[j][0].r)
|| (Track_to_start[i][1].T == Track_to_start[j][1].T && Track_to_start[i][1].r == Track_to_start[j][1].r) || (Track_to_start[i][1].CPI_time == Track_to_start[j][1].CPI_time && Track_to_start[i][1].r == Track_to_start[j][1].r)
|| (Track_to_start[i][2].T == Track_to_start[j][2].T && Track_to_start[i][2].r == Track_to_start[j][2].r) || (Track_to_start[i][2].CPI_time == Track_to_start[j][2].CPI_time && Track_to_start[i][2].r == Track_to_start[j][2].r)
) )
{ {
if(Track_to_start[i][L-1].d<Track_to_start[j][L-1].d) if(Track_to_start[i][L-1].d<Track_to_start[j][L-1].d)
@@ -565,8 +544,8 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
double Z2[2]={(Track_to_start[i][L-1].r)*cos(Track_to_start[i][L-1].azi),(Track_to_start[i][L-1].r)*sin(Track_to_start[i][L-1].azi) }; double Z2[2]={(Track_to_start[i][L-1].r)*cos(Track_to_start[i][L-1].azi),(Track_to_start[i][L-1].r)*sin(Track_to_start[i][L-1].azi) };
double X[6]; double X[6];
double P[6][6]; double P[6][6];
double T1=(Track_to_start[i][L-2].T-Track_to_start[i][L-3].T)/1000.0; 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].T-Track_to_start[i][L-2].T)/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;
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P); Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
@@ -574,13 +553,13 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
memcpy(trust_track_tmp.X, X, 6*sizeof(double)); memcpy(trust_track_tmp.X, X, 6*sizeof(double));
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double)); memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
trust_track_tmp.Amplitude=Track_to_start[i][L-1].Amp; trust_track_tmp.Amplitude=Track_to_start[i][L-1].amplitude;
trust_track_tmp.snr_point = Track_to_start[i][L-1].snr; trust_track_tmp.snr_point = Track_to_start[i][L-1].SNR;
trust_track_tmp.RCS = Track_to_start[i][L-1].RCS; trust_track_tmp.RCS = Track_to_start[i][L-1].RCS;
trust_track_tmp.pitch_num = Track_to_start[i][L-1].pitch_num; trust_track_tmp.pitch_num = Track_to_start[i][L-1].pitch_num;
std::memcpy(trust_track_tmp.speed_dim, Track_to_start[i][L-1].speed_dim, sizeof(trust_track_tmp.speed_dim)); std::memcpy(trust_track_tmp.speed_dim, Track_to_start[i][L-1].speed_dim, sizeof(trust_track_tmp.speed_dim));
std::memcpy(trust_track_tmp.range_dim, Track_to_start[i][L-1].range_dim, sizeof(trust_track_tmp.range_dim)); std::memcpy(trust_track_tmp.range_dim, Track_to_start[i][L-1].range_dim, sizeof(trust_track_tmp.range_dim));
trust_track_tmp.T_track = Track_to_start[i][L-1].T; trust_track_tmp.T_track = Track_to_start[i][L-1].CPI_time;
trust_track_tmp.GNSS_time = Track_to_start[i][L-1].GNSS_time; trust_track_tmp.GNSS_time = Track_to_start[i][L-1].GNSS_time;
trust_track_tmp.point_flag=1; //实点 trust_track_tmp.point_flag=1; //实点
trust_track_tmp.Track_Mode=0; //跟踪模式 TWS 0 trust_track_tmp.Track_Mode=0; //跟踪模式 TWS 0
@@ -626,8 +605,8 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
} }
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast<float>(sqrt(pow(Track_to_start[i][j].X[1],2)+pow(Track_to_start[i][j].X[3],2))); Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast<float>(sqrt(pow(Track_to_start[i][j].X[1],2)+pow(Track_to_start[i][j].X[3],2)));
Trust_Track_Output[*Trust_track_num_Output-1][j].z=static_cast<float>(Track_to_start[i][j].height); Trust_Track_Output[*Trust_track_num_Output-1][j].z=static_cast<float>(Track_to_start[i][j].height);
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>(Track_to_start[i][j].Amp); 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_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].track_rcs = static_cast<float>(Track_to_start[i][j].RCS);
Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num; Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;
@@ -640,7 +619,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
Direction_Angle=Direction_Angle+2*PI; Direction_Angle=Direction_Angle+2*PI;
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=static_cast<float>(Direction_Angle/PI*180); Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast<float>((Track_to_start[i][j].T)/1000.0); Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast<float>((Track_to_start[i][j].CPI_time)/1000.0);
Trust_Track_Output[*Trust_track_num_Output-1][j].GNSS_time=Track_to_start[i][j].GNSS_time; Trust_Track_Output[*Trust_track_num_Output-1][j].GNSS_time=Track_to_start[i][j].GNSS_time;
Trust_Track_Output[*Trust_track_num_Output-1][j].x=static_cast<float>(Track_to_start[i][j].X[0]); Trust_Track_Output[*Trust_track_num_Output-1][j].x=static_cast<float>(Track_to_start[i][j].X[0]);
data_process_class_dll/track_init.h
+2
View File
@@ -27,6 +27,8 @@ public:
private: private:
long long temp_track_id = 0; //临时航迹id
std::vector<PointRecv> point_process; //要处理的点迹 std::vector<PointRecv> point_process; //要处理的点迹
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track, void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
data_process_class_dll/track_init_direct_tracking.cpp
+18 -18
View File
@@ -62,7 +62,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
double v_temp_track=(*temp_track)[j][L-1].vr; double v_temp_track=(*temp_track)[j][L-1].vr;
double r_temp_track=(*temp_track)[j][L-1].r; double r_temp_track=(*temp_track)[j][L-1].r;
double h_temp_track=(*temp_track)[j][L-1].height; double h_temp_track=(*temp_track)[j][L-1].height;
double T_track_head = static_cast<double>((*temp_track)[j][L-1].T); double T_track_head = static_cast<double>((*temp_track)[j][L-1].CPI_time);
double delta_T = (T_point - T_track_head)/1000.0; double delta_T = (T_point - T_track_head)/1000.0;
@@ -114,10 +114,10 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range; asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range;
asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth; asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth;
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height; asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude; asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude;
asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_time; asso_track_info_tmp.CPI_time = point_process[asso_info[i].point_idx-1].CPI_time;
asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity; asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity;
asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].SNR; asso_track_info_tmp.SNR = point_process[asso_info[i].point_idx-1].SNR;
asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS;
asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_beam_index; asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_beam_index;
std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim));
@@ -132,7 +132,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
double X[4]; double X[4];
double P[4][4]; double P[4][4];
kalman Kalman; kalman Kalman;
double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].T)/1000.0; double 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);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
@@ -199,7 +199,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
y_track_head=(*temp_track)[j][0].X[2]; y_track_head=(*temp_track)[j][0].X[2];
double v_track_head=(*temp_track)[j][0].vr; double v_track_head=(*temp_track)[j][0].vr;
double h_track_head = (*temp_track)[j][0].height; double h_track_head = (*temp_track)[j][0].height;
double T_track_head = static_cast<double>((*temp_track)[j][0].T); double T_track_head = static_cast<double>((*temp_track)[j][0].CPI_time);
//距离差 //距离差
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
@@ -250,11 +250,11 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range; asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].range;
asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth; asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].azimuth;
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height; asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude; asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude;
asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity; asso_track_info_tmp.vr = point_process[asso_info[i].point_idx-1].velocity;
asso_track_info_tmp.snr = point_process[asso_info[i].point_idx-1].SNR; asso_track_info_tmp.SNR = point_process[asso_info[i].point_idx-1].SNR;
asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS; asso_track_info_tmp.RCS = point_process[asso_info[i].point_idx-1].RCS;
asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_time; asso_track_info_tmp.CPI_time = point_process[asso_info[i].point_idx-1].CPI_time;
asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_beam_index; asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_beam_index;
std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim)); std::memcpy(asso_track_info_tmp.speed_dim, point_process[asso_info[i].point_idx-1].speed_dim, sizeof(asso_track_info_tmp.speed_dim));
std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim)); std::memcpy(asso_track_info_tmp.range_dim, point_process[asso_info[i].point_idx-1].range_dim, sizeof(asso_track_info_tmp.range_dim));
@@ -268,7 +268,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
double X[4]; double X[4];
double P[4][4]; double P[4][4];
kalman Kalman; kalman Kalman;
double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].T)/1000.0; double 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);
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double)); memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
@@ -323,8 +323,8 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
double Z2[2]={((*Iter)[L-1].r)*cos((*Iter)[L-1].azi),((*Iter)[L-1].r)*sin((*Iter)[L-1].azi) }; double Z2[2]={((*Iter)[L-1].r)*cos((*Iter)[L-1].azi),((*Iter)[L-1].r)*sin((*Iter)[L-1].azi) };
double X[6]; double X[6];
double P[6][6]; double P[6][6];
double T1=((*Iter)[L-2].T-(*Iter)[L-3].T)/1000.0; double T1=((*Iter)[L-2].CPI_time-(*Iter)[L-3].CPI_time)/1000.0;
double T2=((*Iter)[L-1].T-(*Iter)[L-2].T)/1000.0; double T2=((*Iter)[L-1].CPI_time-(*Iter)[L-2].CPI_time)/1000.0;
kalman Kalman; 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);
Trust_Track trust_track_tmp = {}; Trust_Track trust_track_tmp = {};
@@ -332,10 +332,10 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double)); memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
trust_track_tmp.Track_Index = track_ID; trust_track_tmp.Track_Index = track_ID;
trust_track_tmp.Amplitude=(*Iter)[L-1].Amp; trust_track_tmp.Amplitude=(*Iter)[L-1].amplitude;
trust_track_tmp.snr_point = (*Iter)[L-1].snr; trust_track_tmp.snr_point = (*Iter)[L-1].SNR;
trust_track_tmp.RCS = (*Iter)[L-1].RCS; trust_track_tmp.RCS = (*Iter)[L-1].RCS;
trust_track_tmp.T_track = (*Iter)[L-1].T; trust_track_tmp.T_track = (*Iter)[L-1].CPI_time;
trust_track_tmp.point_flag=1; //实点 trust_track_tmp.point_flag=1; //实点
trust_track_tmp.Track_Mode=1; //跟踪模式 TWS 0 trust_track_tmp.Track_Mode=1; //跟踪模式 TWS 0
trust_track_tmp.Target_Type=UNCONF_TARGET; trust_track_tmp.Target_Type=UNCONF_TARGET;
@@ -375,8 +375,8 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=static_cast<float>(asin((*Iter)[j].height/(*Iter)[j].r)/PI*180); Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=static_cast<float>(asin((*Iter)[j].height/(*Iter)[j].r)/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast<float>(sqrt(pow((*Iter)[j].X[1],2)+pow((*Iter)[j].X[3],2))); Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast<float>(sqrt(pow((*Iter)[j].X[1],2)+pow((*Iter)[j].X[3],2)));
Trust_Track_Output[*Trust_track_num_Output-1][j].z=static_cast<float>((*Iter)[j].height); Trust_Track_Output[*Trust_track_num_Output-1][j].z=static_cast<float>((*Iter)[j].height);
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>((*Iter)[j].Amp); Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>((*Iter)[j].amplitude);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast<float>((*Iter)[j].snr); Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast<float>((*Iter)[j].SNR);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast<float>((*Iter)[j].RCS); Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast<float>((*Iter)[j].RCS);
double Direction_Angle; double Direction_Angle;
Direction_Angle=atan2((*Iter)[j].X[3],(*Iter)[j].X[1]); Direction_Angle=atan2((*Iter)[j].X[3],(*Iter)[j].X[1]);
@@ -384,7 +384,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
Direction_Angle=Direction_Angle+2*PI; Direction_Angle=Direction_Angle+2*PI;
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=static_cast<float>(Direction_Angle/PI*180); Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast<float>(((*Iter)[j].T)/1000.0); Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast<float>(((*Iter)[j].CPI_time)/1000.0);
Trust_Track_Output[*Trust_track_num_Output-1][j].x=static_cast<float>((*Iter)[j].X[0]); Trust_Track_Output[*Trust_track_num_Output-1][j].x=static_cast<float>((*Iter)[j].X[0]);
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=static_cast<float>((*Iter)[j].X[1]); Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=static_cast<float>((*Iter)[j].X[1]);
Trust_Track_Output[*Trust_track_num_Output-1][j].y=static_cast<float>((*Iter)[j].X[2]); Trust_Track_Output[*Trust_track_num_Output-1][j].y=static_cast<float>((*Iter)[j].X[2]);