更新：1、增加临时航迹录入数据库；
2、增加点迹信息的圈ID记录； Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
@@ -31,6 +31,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
|
||||
PointRecv Data_buffer_temp;
|
||||
Data_buffer_temp.point_id = point_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.azimuth=Data_Input[i].Azimuth/180*PI;
|
||||
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
|
||||
{
|
||||
last_beam_num = Data_Input[0].Beam_index_aiz;
|
||||
|
||||
circle_id++;
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
@@ -87,6 +88,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
|
||||
PointRecv Data_buffer_temp;
|
||||
Data_buffer_temp.point_id = point_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.azimuth=Data_Input[i].Azimuth/180*PI;
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
circle_id++;
|
||||
return 3;
|
||||
}
|
||||
|
||||
|
||||
@@ -102,6 +102,8 @@ private:
|
||||
|
||||
long long frame_id = 0; //帧id
|
||||
|
||||
long long circle_id = 0; //圈id
|
||||
|
||||
struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入
|
||||
|
||||
Dot_Coh dot_coh; //点迹凝聚
|
||||
|
||||
@@ -231,20 +231,24 @@ namespace rdp_record
|
||||
// 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);
|
||||
RDP_RECORD_DEFINE(RdpRecPoint);
|
||||
|
||||
//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);
|
||||
RDP_RECORD_DEFINE(RdpRecPointCohesionTWS);
|
||||
|
||||
//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);
|
||||
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,
|
||||
gnss_time_ms, track_time_s, x, y, z, v_x, v_y, v_z, range_m, azi_deg, elev_deg,
|
||||
|
||||
@@ -21,6 +21,11 @@
|
||||
#include "rdp_record.h"
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <cstddef>
|
||||
|
||||
#include "struct.h"
|
||||
|
||||
@@ -30,6 +35,18 @@
|
||||
extern template void rdp_record::Write<Entity>(const 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
|
||||
@@ -37,6 +54,7 @@ struct RdpRecPoint
|
||||
int64_t id; //自增主键（写入时固定填 0）
|
||||
long long point_id; //点迹ID
|
||||
long long frame_id; //帧ID
|
||||
long long circle_id; //圈ID
|
||||
double range; //径向距离
|
||||
double azimuth; //方位 (弧度)
|
||||
double velocity ; //速度
|
||||
@@ -65,6 +83,7 @@ struct RdpRecPoint
|
||||
id = 0;
|
||||
point_id = p.point_id;
|
||||
frame_id = p.frame_id;
|
||||
circle_id = p.circle_id;
|
||||
range = p.range;
|
||||
azimuth = p.azimuth;
|
||||
velocity = p.velocity;
|
||||
@@ -94,6 +113,7 @@ struct RdpRecPointCohesionTWS
|
||||
int64_t id; //自增主键（写入时固定填 0）
|
||||
long long point_id; //点迹ID
|
||||
long long frame_id; //帧ID
|
||||
long long circle_id; //圈ID
|
||||
double range; //径向距离
|
||||
double azimuth; //方位 (弧度)
|
||||
double velocity ; //速度
|
||||
@@ -122,6 +142,7 @@ struct RdpRecPointCohesionTWS
|
||||
id = 0;
|
||||
point_id = p.point_id;
|
||||
frame_id = p.frame_id;
|
||||
circle_id = p.circle_id;
|
||||
range = p.range;
|
||||
azimuth = p.azimuth;
|
||||
velocity = p.velocity;
|
||||
@@ -151,6 +172,7 @@ struct RdpRecPointCohesionTAS
|
||||
int64_t id; //自增主键（写入时固定填 0）
|
||||
long long point_id; //点迹ID
|
||||
long long frame_id; //帧ID
|
||||
long long circle_id; //圈ID
|
||||
double range; //径向距离
|
||||
double azimuth; //方位 (弧度)
|
||||
double velocity ; //速度
|
||||
@@ -179,6 +201,7 @@ struct RdpRecPointCohesionTAS
|
||||
id = 0;
|
||||
point_id = p.point_id;
|
||||
frame_id = p.frame_id;
|
||||
circle_id = p.circle_id;
|
||||
range = p.range;
|
||||
azimuth = p.azimuth;
|
||||
velocity = p.velocity;
|
||||
@@ -202,6 +225,61 @@ struct 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()「数据记录示例②」处，
|
||||
// TWS/TAS 共用一张表并用 track_mode 区分；每行取输出数组的 [i][0]
|
||||
struct RdpRecTrackOut
|
||||
@@ -229,4 +307,45 @@ struct 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
|
||||
|
||||
@@ -11,6 +11,7 @@ struct PointRecv
|
||||
{
|
||||
long long point_id; //点迹ID
|
||||
long long frame_id; //帧ID
|
||||
long long circle_id; //圈ID
|
||||
double range; //径向距离
|
||||
double azimuth; //方位 (弧度)
|
||||
double velocity ; //速度
|
||||
@@ -74,7 +75,7 @@ struct Trust_Track
|
||||
double vr_point;
|
||||
double prf_point;
|
||||
int point_type; //TWS TAS
|
||||
double snr_point; //信噪比
|
||||
double snr_point; //信噪比
|
||||
float amp_asv_point; //关联上的点迹信息（归一化幅度）
|
||||
float prn_fftn_point; //点迹速度维FFT点数
|
||||
|
||||
@@ -93,32 +94,35 @@ struct Trust_Track
|
||||
double P1[6][6];
|
||||
double P2[6][6];
|
||||
double P3[6][6];
|
||||
double u[3];
|
||||
double u[3];
|
||||
|
||||
};
|
||||
|
||||
//临时航迹结构体
|
||||
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 r; //距离
|
||||
double azi; //方位
|
||||
double vr; //径向速度
|
||||
double height; //高度
|
||||
double snr; //信噪比
|
||||
double SNR; //信噪比
|
||||
double RCS; //RCS
|
||||
double Amp; //幅度
|
||||
long long T; //时间戳
|
||||
long long GNSS_time; //GNSS时间戳
|
||||
double amplitude; //幅度
|
||||
long long CPI_time; //时间戳
|
||||
long long GNSS_time; //GNSS时间戳
|
||||
|
||||
double d; //关联上的点的d
|
||||
double d; //关联上的点的d（马氏距离）
|
||||
|
||||
int buff_round; //已缓存圈数
|
||||
|
||||
int Temp_track_section_idx; //临时航迹区号 与点迹区的划分相同
|
||||
int temp_track_section_idx; //临时航迹区号 与点迹区的划分相同
|
||||
|
||||
int asso_flag; //关联标志 已关联：1 未关联：0
|
||||
|
||||
|
||||
@@ -6,6 +6,8 @@
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include<iomanip>
|
||||
#include "rdp_record.h"
|
||||
#include "rdp_record_entity.h"
|
||||
using namespace std;
|
||||
|
||||
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,
|
||||
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]);
|
||||
}
|
||||
|
||||
//临时航迹的buff_round+1
|
||||
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++)
|
||||
{
|
||||
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.azi = point_process[i].azimuth;
|
||||
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.snr = point_process[i].SNR;
|
||||
temp_track_tmp.SNR = point_process[i].SNR;
|
||||
temp_track_tmp.RCS = point_process[i].RCS;
|
||||
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));
|
||||
@@ -93,7 +83,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
|
||||
temp_track_tmp.X[1]=0;
|
||||
temp_track_tmp.X[2]=point_process[i].range*sin(point_process[i].azimuth);
|
||||
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.buff_round = 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);
|
||||
|
||||
// std::cout << "-----------------------------------------------";
|
||||
// for(int i=0;i<temp_track->size();i++)
|
||||
// {
|
||||
// for(int j=0;j<(*temp_track)[i].size();j++)
|
||||
// {
|
||||
// std::cout << "temp_track_azi=" << (*temp_track)[i][j].azi
|
||||
// << "temp_track_dis=" << (*temp_track)[i][j].r
|
||||
// << "temp_track_h=" << (*temp_track)[i][j].height
|
||||
// << "temp_track_v=" << (*temp_track)[i][j].vr
|
||||
// << "temp_track_time=" << (*temp_track)[i][j].T;
|
||||
// }
|
||||
// }
|
||||
|
||||
#ifdef RDP_RECORD_ENABLE
|
||||
//记录临时航迹
|
||||
for(auto& vec_t : *temp_track){
|
||||
for(auto& t : vec_t){
|
||||
RDP_RECORD(RdpRecTempTrack(t));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
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 r_temp_track=(*temp_track)[j][L-1].r;
|
||||
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;
|
||||
|
||||
@@ -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 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
|
||||
{
|
||||
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][j].asso_flag = 0;
|
||||
(*temp_track)[(*temp_track).size()-1][j].temp_track_id = temp_track_id;
|
||||
}
|
||||
|
||||
//关联上的点
|
||||
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.azi = point_process[asso_info[i].point_idx-1].azimuth;
|
||||
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
|
||||
asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude;
|
||||
asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_time;
|
||||
asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude;
|
||||
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.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.asso_flag=0;
|
||||
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 P[4][4];
|
||||
kalman Kalman;
|
||||
double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].T)/1000.0;
|
||||
double 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);
|
||||
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];
|
||||
double v_track_head=(*temp_track)[j][0].vr;
|
||||
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));
|
||||
@@ -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][0].asso_flag = 0;
|
||||
(*temp_track)[(*temp_track).size()-1][0].temp_track_id = temp_track_id;
|
||||
|
||||
//第二个点
|
||||
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.azi = point_process[asso_info[i].point_idx-1].azimuth;
|
||||
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
|
||||
asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude;
|
||||
asso_track_info_tmp.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.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.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.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));
|
||||
@@ -417,7 +396,7 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
|
||||
double X[4];
|
||||
double P[4][4];
|
||||
kalman Kalman;
|
||||
double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].T)/1000.0;
|
||||
double 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);
|
||||
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)
|
||||
continue;
|
||||
|
||||
if( (Track_to_start[i][0].T == Track_to_start[j][0].T && Track_to_start[i][0].r == Track_to_start[j][0].r)
|
||||
|| (Track_to_start[i][1].T == Track_to_start[j][1].T && Track_to_start[i][1].r == Track_to_start[j][1].r)
|
||||
|| (Track_to_start[i][2].T == Track_to_start[j][2].T && Track_to_start[i][2].r == Track_to_start[j][2].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].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].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)
|
||||
@@ -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 X[6];
|
||||
double P[6][6];
|
||||
double T1=(Track_to_start[i][L-2].T-Track_to_start[i][L-3].T)/1000.0;
|
||||
double T2=(Track_to_start[i][L-1].T-Track_to_start[i][L-2].T)/1000.0;
|
||||
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);
|
||||
|
||||
@@ -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.P, P, 6*6*sizeof(double));
|
||||
|
||||
trust_track_tmp.Amplitude=Track_to_start[i][L-1].Amp;
|
||||
trust_track_tmp.snr_point = Track_to_start[i][L-1].snr;
|
||||
trust_track_tmp.Amplitude=Track_to_start[i][L-1].amplitude;
|
||||
trust_track_tmp.snr_point = Track_to_start[i][L-1].SNR;
|
||||
trust_track_tmp.RCS = Track_to_start[i][L-1].RCS;
|
||||
trust_track_tmp.pitch_num = Track_to_start[i][L-1].pitch_num;
|
||||
std::memcpy(trust_track_tmp.speed_dim, Track_to_start[i][L-1].speed_dim, sizeof(trust_track_tmp.speed_dim));
|
||||
std::memcpy(trust_track_tmp.range_dim, Track_to_start[i][L-1].range_dim, sizeof(trust_track_tmp.range_dim));
|
||||
trust_track_tmp.T_track = Track_to_start[i][L-1].T;
|
||||
trust_track_tmp.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.point_flag=1; //实点
|
||||
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].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].track_snr = static_cast<float>(Track_to_start[i][j].snr);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>(Track_to_start[i][j].amplitude);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast<float>(Track_to_start[i][j].SNR);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast<float>(Track_to_start[i][j].RCS);
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].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;
|
||||
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].x=static_cast<float>(Track_to_start[i][j].X[0]);
|
||||
|
||||
@@ -27,6 +27,8 @@ public:
|
||||
|
||||
private:
|
||||
|
||||
long long temp_track_id = 0; //临时航迹id
|
||||
|
||||
std::vector<PointRecv> point_process; //要处理的点迹
|
||||
|
||||
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
|
||||
|
||||
@@ -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 r_temp_track=(*temp_track)[j][L-1].r;
|
||||
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;
|
||||
|
||||
@@ -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.azi = point_process[asso_info[i].point_idx-1].azimuth;
|
||||
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
|
||||
asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude;
|
||||
asso_track_info_tmp.T = point_process[asso_info[i].point_idx-1].CPI_time;
|
||||
asso_track_info_tmp.amplitude = point_process[asso_info[i].point_idx-1].amplitude;
|
||||
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.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.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));
|
||||
@@ -132,7 +132,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
|
||||
double X[4];
|
||||
double P[4][4];
|
||||
kalman Kalman;
|
||||
double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].T)/1000.0;
|
||||
double 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);
|
||||
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];
|
||||
double v_track_head=(*temp_track)[j][0].vr;
|
||||
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));
|
||||
@@ -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.azi = point_process[asso_info[i].point_idx-1].azimuth;
|
||||
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].height;
|
||||
asso_track_info_tmp.Amp = point_process[asso_info[i].point_idx-1].amplitude;
|
||||
asso_track_info_tmp.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.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.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;
|
||||
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));
|
||||
@@ -268,7 +268,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
|
||||
double X[4];
|
||||
double P[4][4];
|
||||
kalman Kalman;
|
||||
double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].T)/1000.0;
|
||||
double 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);
|
||||
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 X[6];
|
||||
double P[6][6];
|
||||
double T1=((*Iter)[L-2].T-(*Iter)[L-3].T)/1000.0;
|
||||
double T2=((*Iter)[L-1].T-(*Iter)[L-2].T)/1000.0;
|
||||
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);
|
||||
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));
|
||||
|
||||
trust_track_tmp.Track_Index = track_ID;
|
||||
trust_track_tmp.Amplitude=(*Iter)[L-1].Amp;
|
||||
trust_track_tmp.snr_point = (*Iter)[L-1].snr;
|
||||
trust_track_tmp.Amplitude=(*Iter)[L-1].amplitude;
|
||||
trust_track_tmp.snr_point = (*Iter)[L-1].SNR;
|
||||
trust_track_tmp.RCS = (*Iter)[L-1].RCS;
|
||||
trust_track_tmp.T_track = (*Iter)[L-1].T;
|
||||
trust_track_tmp.T_track = (*Iter)[L-1].CPI_time;
|
||||
trust_track_tmp.point_flag=1; //实点
|
||||
trust_track_tmp.Track_Mode=1; //跟踪模式 TWS 0
|
||||
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].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].Amplitude=static_cast<float>((*Iter)[j].Amp);
|
||||
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].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_rcs = static_cast<float>((*Iter)[j].RCS);
|
||||
double Direction_Angle;
|
||||
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;
|
||||
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].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]);
|
||||
|
||||
Reference in New Issue
Block a user