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

更新：1、适配四面阵机扫跟踪模式。

Browse Source 
Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-07-13 10:20:32 +08:00
parent ae3ef84441
commit 8b1896dd74
21 changed files with 521 additions and 7774 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/data_process.cpp
+73 -63
View File
@@ -15,6 +15,9 @@ using namespace std;
//数据预处理
int Data_Process::data_preprocess(struct DataRev Data_Input[150])
{
latest_timestamp = Data_Input[0].CPI_time;
//qDebug() << "latest_timestamp: " <<Data_Input[0].CPI_time;
//数据存入缓存区域 Data_buffer
if(Data_Input[0].point_type == 0) //tws数据
{
@@ -41,10 +44,13 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
std::memcpy(Data_buffer_temp.range_dim, Data_Input[i].range_dim, sizeof(Data_buffer_temp.range_dim));
Data_buffer.push_back(Data_buffer_temp);
qDebug() << "AZI: " <<Data_Input[i].Azimuth << "Encoder_value: " <<Data_Input[i].Encoder_value;
}
if(Data_Input[0].Beam_index_aiz < last_beam_num) //TWS
{
last_beam_num = Data_Input[0].Beam_index_aiz;
return 1;
}
else
@@ -56,8 +62,8 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
else if(Data_Input[0].point_type == 1) //tas数据
{
// qDebug() << "TAS TARGET :" <<Data_Input[0].TAS_track_index;
// qDebug() << "TAS POINT :" <<Data_Input[0].Point_Sum;
// qDebug() << "TAS TARGET :" <<Data_Input[0].TAS_track_index;
qDebug() << "TAS POINT :" <<Data_Input[0].Point_Sum;
data_num=Data_Input[0].Point_Sum;
TAS_track_idx = Data_Input[0].TAS_track_index;
@@ -81,11 +87,11 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
Data_buffer_tas.push_back(Data_buffer_temp);
// qDebug() << "Azimuth:" << Data_Input[i].Azimuth
// << "Range:" << Data_Input[i].Range
// << "EL:" << Data_Input[i].Elevation
// << "CPI_Time"<< Data_Input[i].CPI_time
// << "data_num:" << data_num;
// qDebug() << "Azimuth:" << Data_Input[i].Azimuth
// << "Range:" << Data_Input[i].Range
// << "EL:" << Data_Input[i].Elevation
// << "CPI_Time"<< Data_Input[i].CPI_time
// << "data_num:" << data_num;
}
@@ -94,7 +100,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
else
{
return 0;
return 0;
}
}
@@ -103,19 +109,19 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
//数据处理
int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
int Track_die_Index_Output[],
int *Track_die_num_Output,
int model)
int *Trust_track_num_Output,
int Track_die_Index_Output[],
int *Track_die_num_Output,
int model)
{
//TWS处理
if(model==2)
{
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,Work_Parameter);
//qDebug() << "point_recv :" <<point_recv.size();
QVector<PointRecv>().swap(Data_buffer);
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,Work_Parameter);
//qDebug() << "point_recv :" <<point_recv.size();
QVector<PointRecv>().swap(Data_buffer);
}
@@ -126,27 +132,27 @@ int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][1
*Track_die_num_Output=0; //航迹消亡数目
track_asso.track_asso_process(&point_recv,
&trust_track,
Trust_Track_Output,
Trust_track_num_Output,
Work_Parameter
);
&trust_track,
Trust_Track_Output,
Trust_track_num_Output,
Work_Parameter
);
track_die.track_die_process( &trust_track,
Track_die_Index_Output,
Track_die_num_Output);
Track_die_Index_Output,
Track_die_num_Output);
track_init.track_init_process_logic( &point_recv,
&trust_track,
&temp_track,
Trust_Track_Output,
Trust_track_num_Output,
Work_Parameter);
// qDebug() << "point_recv: " <<point_recv.size();
// qDebug() << "trust_track: " <<trust_track.size();
// qDebug() << "temp_track: " <<temp_track.size();
// qDebug() << "Trust_track_num_Output: " <<*Trust_track_num_Output;
// qDebug() << "Track_die_num_Output: " <<*Track_die_num_Output;
&trust_track,
&temp_track,
Trust_Track_Output,
Trust_track_num_Output,
Work_Parameter);
// qDebug() << "point_recv: " <<point_recv.size();
// qDebug() << "trust_track: " <<trust_track.size();
// qDebug() << "temp_track: " <<temp_track.size();
// qDebug() << "Trust_track_num_Output: " <<*Trust_track_num_Output;
// qDebug() << "Track_die_num_Output: " <<*Track_die_num_Output;
}
@@ -158,36 +164,32 @@ int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][1
dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas); //点迹凝聚
// if(point_recv_tas.size()>0)
// {
// qDebug() << "point_recv_tas :" <<point_recv_tas[0].Azimuth/PI*180<<" "<<point_recv_tas[0].Range;
// }
// if(point_recv_tas.size()>0)
// {
// qDebug() << "point_recv_tas :" <<point_recv_tas[0].Azimuth/PI*180<<" "<<point_recv_tas[0].Range;
// }
qDebug() << "TAS process :" << TAS_track_idx << "p a r:"<<point_recv_tas[0].Point_Sum<<" " <<point_recv_tas[0].Azimuth/PI*180<<" "<<point_recv_tas[0].Range;
QVector<PointRecv>().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer
track_asso_tas.track_asso_process_tas(&point_recv_tas, //点迹文件
&trust_track, //航迹文件
Trust_Track_Output, //更新航迹信息
Trust_track_num_Output, //更新航迹数
Work_Parameter, //工作参数
TAS_track_idx);
&trust_track, //航迹文件
Trust_Track_Output, //更新航迹信息
Trust_track_num_Output, //更新航迹数
Work_Parameter, //工作参数
TAS_track_idx);
QVector<PointRecv>().swap(point_recv_tas); //清除 point_recv_tas
track_die_tas.track_die_process_tas( &trust_track,
Track_die_Index_Output,
Track_die_num_Output);
Track_die_Index_Output,
Track_die_num_Output);
//qDebug() << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
}
return 1;
}
@@ -195,16 +197,13 @@ int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][1
//波束控制
void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output)
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output)
{
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,Work_Parameter, latest_timestamp);
}
//数据处理参数初始化
int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parameter)
{
@@ -215,11 +214,6 @@ int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parame
return 0;
}
//数据处理参数修改
int Data_Process::track_process_parameters_modify(struct RadarPara Radar_Parameter)
{
@@ -239,7 +233,7 @@ int Data_Process::track_clear_all(void)
//手动航迹删除函数
int Data_Process:: track_delete(int delete_track_num, //手动删除的航迹数目
int delete_track_index[]) //手动删除的航迹号
int delete_track_index[]) //手动删除的航迹号
{
for (int i=0;i<delete_track_num ;i++)
{
@@ -249,6 +243,7 @@ int Data_Process:: track_delete(int delete_track_num, //手动
{
trust_track[j].manual_delete_flag=1;
qDebug() << "Delete :" << trust_track[j].Track_Index;
}
}
@@ -258,7 +253,7 @@ int Data_Process:: track_delete(int delete_track_num, //手动
return 0;
}
//手动转TAS跟踪函数
//手动转TAS跟踪函数
int Data_Process:: tracking_start(int track_index) //需要手动转入TAS跟踪的航迹号
{
@@ -272,7 +267,22 @@ int Data_Process:: tracking_start(int track_index) //需要手动
return 0;
}
//手动打跟踪波束
//手动取消TAS跟踪函数
int Data_Process:: tracking_stop(int track_index) //需要手动转入TAS跟踪的航迹号
{
for ( int i=0; i<trust_track.size();i++){
if(trust_track[i].Track_Index==track_index)
{
trust_track[i].manual_tracking_flag = 0;
trust_track[i].Track_Mode = 0;
}
}
return 0;
}
//手动打跟踪波束
int Data_Process::tracking_point(float Azimuth)//方位角
{
return 0;
data_process_class_dll/data_process.h
+6 -1
View File
@@ -57,6 +57,9 @@ public:
//手动转TAS跟踪函数
int tracking_start(int track_index); //需要手动转入TAS跟踪的航迹号
//手动取消TAS跟踪函数
int tracking_stop(int track_index); //需要取消TAS跟踪的航迹号
//手动打跟踪波束
int tracking_point(float Azimuth); //方位角
@@ -88,7 +91,9 @@ private:
int Track_Mode; //跟踪模式 1TAS 0TWS
int last_beam_num = INT_MIN; //上一个波位号，用于判断是否搜索完一圈
int last_beam_num = INT_MAX; //上一个波位号，用于判断是否搜索完一圈
int latest_timestamp = 0; //最新时间戳
struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入
data_process_class_dll/data_process_class_dll.h
+5 -4
View File
@@ -225,9 +225,6 @@ public:
return 0;
}
//手动航迹删除函数
virtual int track_delete(int delete_track_num, //手动删除的航迹数目
int delete_track_index[]) //手动删除的航迹号
@@ -235,13 +232,17 @@ public:
return 0;
}
//手动转TAS跟踪函数
virtual int tracking_start(int track_index) //需要手动转入TAS跟踪的航迹号
{
return 0;
}
//手动取消TAS跟踪函数
virtual int tracking_stop(int track_index) //需要手动转入TAS跟踪的航迹号
{
return 0;
}
//手动打跟踪波束
virtual int tracking_point(float Azimuth) //方位角
data_process_class_dll/eigen-3.3.7/debug/gdb/__init__.py
-1
View File
@@ -1 +0,0 @@
# Intentionally empty
data_process_class_dll/eigen-3.3.7/debug/gdb/printers.py
-214
View File
@@ -1,214 +0,0 @@
# -*- coding: utf-8 -*-
# This file is part of Eigen, a lightweight C++ template library
# for linear algebra.
#
# Copyright (C) 2009 Benjamin Schindler <bschindler@inf.ethz.ch>
#
# This Source Code Form is subject to the terms of the Mozilla Public
# License, v. 2.0. If a copy of the MPL was not distributed with this
# file, You can obtain one at http://mozilla.org/MPL/2.0/.
# Pretty printers for Eigen::Matrix
# This is still pretty basic as the python extension to gdb is still pretty basic.
# It cannot handle complex eigen types and it doesn't support any of the other eigen types
# Such as quaternion or some other type.
# This code supports fixed size as well as dynamic size matrices
# To use it:
#
# * Create a directory and put the file as well as an empty __init__.py in
# that directory.
# * Create a ~/.gdbinit file, that contains the following:
# python
# import sys
# sys.path.insert(0, '/path/to/eigen/printer/directory')
# from printers import register_eigen_printers
# register_eigen_printers (None)
# end
import gdb
import re
import itertools
class EigenMatrixPrinter:
"Print Eigen Matrix or Array of some kind"
def __init__(self, variety, val):
"Extract all the necessary information"
# Save the variety (presumably "Matrix" or "Array") for later usage
self.variety = variety
# The gdb extension does not support value template arguments - need to extract them by hand
type = val.type
if type.code == gdb.TYPE_CODE_REF:
type = type.target()
self.type = type.unqualified().strip_typedefs()
tag = self.type.tag
regex = re.compile('\<.*\>')
m = regex.findall(tag)[0][1:-1]
template_params = m.split(',')
template_params = [x.replace(" ", "") for x in template_params]
if template_params[1] == '-0x00000000000000001' or template_params[1] == '-0x000000001' or template_params[1] == '-1':
self.rows = val['m_storage']['m_rows']
else:
self.rows = int(template_params[1])
if template_params[2] == '-0x00000000000000001' or template_params[2] == '-0x000000001' or template_params[2] == '-1':
self.cols = val['m_storage']['m_cols']
else:
self.cols = int(template_params[2])
self.options = 0 # default value
if len(template_params) > 3:
self.options = template_params[3];
self.rowMajor = (int(self.options) & 0x1)
self.innerType = self.type.template_argument(0)
self.val = val
# Fixed size matrices have a struct as their storage, so we need to walk through this
self.data = self.val['m_storage']['m_data']
if self.data.type.code == gdb.TYPE_CODE_STRUCT:
self.data = self.data['array']
self.data = self.data.cast(self.innerType.pointer())
class _iterator:
def __init__ (self, rows, cols, dataPtr, rowMajor):
self.rows = rows
self.cols = cols
self.dataPtr = dataPtr
self.currentRow = 0
self.currentCol = 0
self.rowMajor = rowMajor
def __iter__ (self):
return self
def next(self):
return self.__next__() # Python 2.x compatibility
def __next__(self):
row = self.currentRow
col = self.currentCol
if self.rowMajor == 0:
if self.currentCol >= self.cols:
raise StopIteration
self.currentRow = self.currentRow + 1
if self.currentRow >= self.rows:
self.currentRow = 0
self.currentCol = self.currentCol + 1
else:
if self.currentRow >= self.rows:
raise StopIteration
self.currentCol = self.currentCol + 1
if self.currentCol >= self.cols:
self.currentCol = 0
self.currentRow = self.currentRow + 1
item = self.dataPtr.dereference()
self.dataPtr = self.dataPtr + 1
if (self.cols == 1): #if it's a column vector
return ('[%d]' % (row,), item)
elif (self.rows == 1): #if it's a row vector
return ('[%d]' % (col,), item)
return ('[%d,%d]' % (row, col), item)
def children(self):
return self._iterator(self.rows, self.cols, self.data, self.rowMajor)
def to_string(self):
return "Eigen::%s<%s,%d,%d,%s> (data ptr: %s)" % (self.variety, self.innerType, self.rows, self.cols, "RowMajor" if self.rowMajor else "ColMajor", self.data)
class EigenQuaternionPrinter:
"Print an Eigen Quaternion"
def __init__(self, val):
"Extract all the necessary information"
# The gdb extension does not support value template arguments - need to extract them by hand
type = val.type
if type.code == gdb.TYPE_CODE_REF:
type = type.target()
self.type = type.unqualified().strip_typedefs()
self.innerType = self.type.template_argument(0)
self.val = val
# Quaternions have a struct as their storage, so we need to walk through this
self.data = self.val['m_coeffs']['m_storage']['m_data']['array']
self.data = self.data.cast(self.innerType.pointer())
class _iterator:
def __init__ (self, dataPtr):
self.dataPtr = dataPtr
self.currentElement = 0
self.elementNames = ['x', 'y', 'z', 'w']
def __iter__ (self):
return self
def next(self):
return self.__next__() # Python 2.x compatibility
def __next__(self):
element = self.currentElement
if self.currentElement >= 4: #there are 4 elements in a quanternion
raise StopIteration
self.currentElement = self.currentElement + 1
item = self.dataPtr.dereference()
self.dataPtr = self.dataPtr + 1
return ('[%s]' % (self.elementNames[element],), item)
def children(self):
return self._iterator(self.data)
def to_string(self):
return "Eigen::Quaternion<%s> (data ptr: %s)" % (self.innerType, self.data)
def build_eigen_dictionary ():
pretty_printers_dict[re.compile('^Eigen::Quaternion<.*>$')] = lambda val: EigenQuaternionPrinter(val)
pretty_printers_dict[re.compile('^Eigen::Matrix<.*>$')] = lambda val: EigenMatrixPrinter("Matrix", val)
pretty_printers_dict[re.compile('^Eigen::Array<.*>$')] = lambda val: EigenMatrixPrinter("Array", val)
def register_eigen_printers(obj):
"Register eigen pretty-printers with objfile Obj"
if obj == None:
obj = gdb
obj.pretty_printers.append(lookup_function)
def lookup_function(val):
"Look-up and return a pretty-printer that can print va."
type = val.type
if type.code == gdb.TYPE_CODE_REF:
type = type.target()
type = type.unqualified().strip_typedefs()
typename = type.tag
if typename == None:
return None
for function in pretty_printers_dict:
if function.search(typename):
return pretty_printers_dict[function](val)
return None
pretty_printers_dict = {}
build_eigen_dictionary ()
data_process_class_dll/eigen-3.3.7/debug/msvc/eigen.natvis
-235
View File
@@ -1,235 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<AutoVisualizer xmlns="http://schemas.microsoft.com/vstudio/debugger/natvis/2010">
<!-- Fixed x Fixed Matrix -->
<Type Name="Eigen::Matrix&lt;*,*,*,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,-1,-1,*,*,*&gt;"/>
<DisplayString>[{$T2}, {$T3}] (fixed matrix)</DisplayString>
<Expand>
<ArrayItems Condition="Flags%2"> <!-- row major layout -->
<Rank>2</Rank>
<Size>$i==0 ? $T2 : $T3</Size>
<ValuePointer>m_storage.m_data.array</ValuePointer>
</ArrayItems>
<ArrayItems Condition="!(Flags%2)"> <!-- column major layout -->
<Direction>Backward</Direction>
<Rank>2</Rank>
<Size>$i==0 ? $T2 : $T3</Size>
<ValuePointer>m_storage.m_data.array</ValuePointer>
</ArrayItems>
</Expand>
</Type>
<!-- 2 x 2 Matrix -->
<Type Name="Eigen::Matrix&lt;*,2,2,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,2,2,*,*,*&gt;"/>
<DisplayString>[2, 2] (fixed matrix)</DisplayString>
<Expand>
<Synthetic Name="[row 0]" Condition="Flags%2">
<DisplayString>({m_storage.m_data.array[0]}, {m_storage.m_data.array[1]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 0]" Condition="!(Flags%2)">
<DisplayString>({m_storage.m_data.array[0]}, {m_storage.m_data.array[2]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 1]" Condition="Flags%2">
<DisplayString>({m_storage.m_data.array[2]}, {m_storage.m_data.array[3]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 1]" Condition="!(Flags%2)">
<DisplayString>({m_storage.m_data.array[1]}, {m_storage.m_data.array[3]})</DisplayString>
</Synthetic>
</Expand>
</Type>
<!-- 3 x 3 Matrix -->
<Type Name="Eigen::Matrix&lt;*,3,3,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,3,3,*,*,*&gt;"/>
<DisplayString>[3, 3] (fixed matrix)</DisplayString>
<Expand>
<Synthetic Name="[row 0]" Condition="Flags%2">
<DisplayString>({m_storage.m_data.array[0]}, {m_storage.m_data.array[1]}, {m_storage.m_data.array[2]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 0]" Condition="!(Flags%2)">
<DisplayString>({m_storage.m_data.array[0]}, {m_storage.m_data.array[3]}, {m_storage.m_data.array[6]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 1]" Condition="Flags%2">
<DisplayString>({m_storage.m_data.array[3]}, {m_storage.m_data.array[4]}, {m_storage.m_data.array[5]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 1]" Condition="!(Flags%2)">
<DisplayString>({m_storage.m_data.array[1]}, {m_storage.m_data.array[4]}, {m_storage.m_data.array[7]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 2]" Condition="Flags%2">
<DisplayString>({m_storage.m_data.array[6]}, {m_storage.m_data.array[7]}, {m_storage.m_data.array[8]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 2]" Condition="!(Flags%2)">
<DisplayString>({m_storage.m_data.array[2]}, {m_storage.m_data.array[5]}, {m_storage.m_data.array[8]})</DisplayString>
</Synthetic>
</Expand>
</Type>
<!-- 4 x 4 Matrix -->
<Type Name="Eigen::Matrix&lt;*,4,4,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,4,4,*,*,*&gt;"/>
<DisplayString>[4, 4] (fixed matrix)</DisplayString>
<Expand>
<Synthetic Name="[row 0]" Condition="Flags%2">
<DisplayString>({m_storage.m_data.array[0]}, {m_storage.m_data.array[1]}, {m_storage.m_data.array[2]}, {m_storage.m_data.array[3]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 0]" Condition="!(Flags%2)">
<DisplayString>({m_storage.m_data.array[0]}, {m_storage.m_data.array[4]}, {m_storage.m_data.array[8]}, {m_storage.m_data.array[12]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 1]" Condition="Flags%2">
<DisplayString>({m_storage.m_data.array[4]}, {m_storage.m_data.array[5]}, {m_storage.m_data.array[6]}, {m_storage.m_data.array[7]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 1]" Condition="!(Flags%2)">
<DisplayString>({m_storage.m_data.array[1]}, {m_storage.m_data.array[5]}, {m_storage.m_data.array[9]}, {m_storage.m_data.array[13]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 2]" Condition="Flags%2">
<DisplayString>({m_storage.m_data.array[8]}, {m_storage.m_data.array[9]}, {m_storage.m_data.array[10]}, {m_storage.m_data.array[11]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 2]" Condition="!(Flags%2)">
<DisplayString>({m_storage.m_data.array[2]}, {m_storage.m_data.array[6]}, {m_storage.m_data.array[10]}, {m_storage.m_data.array[14]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 3]" Condition="Flags%2">
<DisplayString>({m_storage.m_data.array[12]}, {m_storage.m_data.array[13]}, {m_storage.m_data.array[14]}, {m_storage.m_data.array[15]})</DisplayString>
</Synthetic>
<Synthetic Name="[row 3]" Condition="!(Flags%2)">
<DisplayString>({m_storage.m_data.array[3]}, {m_storage.m_data.array[7]}, {m_storage.m_data.array[11]}, {m_storage.m_data.array[15]})</DisplayString>
</Synthetic>
</Expand>
</Type>
<!-- Dynamic x Dynamic Matrix -->
<Type Name="Eigen::Matrix&lt;*,-1,-1,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,-1,-1,*,*,*&gt;"/>
<DisplayString Condition="m_storage.m_data == 0">empty</DisplayString>
<DisplayString Condition="m_storage.m_data != 0">[{m_storage.m_rows}, {m_storage.m_cols}] (dynamic matrix)</DisplayString>
<Expand>
<ArrayItems Condition="Flags%2"> <!-- row major layout -->
<Rank>2</Rank>
<Size>$i==0 ? m_storage.m_rows : m_storage.m_cols</Size>
<ValuePointer>m_storage.m_data</ValuePointer>
</ArrayItems>
<ArrayItems Condition="!(Flags%2)"> <!-- column major layout -->
<Direction>Backward</Direction>
<Rank>2</Rank>
<Size>$i==0 ? m_storage.m_rows : m_storage.m_cols</Size>
<ValuePointer>m_storage.m_data</ValuePointer>
</ArrayItems>
</Expand>
</Type>
<!-- Fixed x Dynamic Matrix -->
<Type Name="Eigen::Matrix&lt;*,*,-1,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,*,-1,*,*,*&gt;"/>
<DisplayString Condition="m_storage.m_data == 0">empty</DisplayString>
<DisplayString Condition="m_storage.m_data != 0">[{$T2}, {m_storage.m_cols}] (dynamic column matrix)</DisplayString>
<Expand>
<ArrayItems Condition="Flags%2"> <!-- row major layout -->
<Rank>2</Rank>
<Size>$i==0 ? $T2 : m_storage.m_cols</Size>
<ValuePointer>m_storage.m_data</ValuePointer>
</ArrayItems>
<ArrayItems Condition="!(Flags%2)"> <!-- column major layout -->
<Direction>Backward</Direction>
<Rank>2</Rank>
<Size>$i==0 ? $T2 : m_storage.m_cols</Size>
<ValuePointer>m_storage.m_data</ValuePointer>
</ArrayItems>
</Expand>
</Type>
<!-- Dynamic x Fixed Matrix -->
<Type Name="Eigen::Matrix&lt;*,-1,*,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,-1,*,*,*,*&gt;"/>
<DisplayString Condition="m_storage.m_data == 0">empty</DisplayString>
<DisplayString Condition="m_storage.m_data != 0">[{m_storage.m_rows}, {$T2}] (dynamic row matrix)</DisplayString>
<Expand>
<ArrayItems Condition="Flags%2"> <!-- row major layout -->
<Rank>2</Rank>
<Size>$i==0 ? m_storage.m_rows : $T2</Size>
<ValuePointer>m_storage.m_data</ValuePointer>
</ArrayItems>
<ArrayItems Condition="!(Flags%2)"> <!-- column major layout -->
<Direction>Backward</Direction>
<Rank>2</Rank>
<Size>$i==0 ? m_storage.m_rows : $T2</Size>
<ValuePointer>m_storage.m_data</ValuePointer>
</ArrayItems>
</Expand>
</Type>
<!-- Dynamic Column Vector -->
<Type Name="Eigen::Matrix&lt;*,1,-1,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,1,-1,*,*,*&gt;"/>
<DisplayString Condition="m_storage.m_data == 0">empty</DisplayString>
<DisplayString Condition="m_storage.m_data != 0">[{m_storage.m_cols}] (dynamic column vector)</DisplayString>
<Expand>
<Item Name="[size]">m_storage.m_cols</Item>
<ArrayItems>
<Size>m_storage.m_cols</Size>
<ValuePointer>m_storage.m_data</ValuePointer>
</ArrayItems>
</Expand>
</Type>
<!-- Dynamic Row Vector -->
<Type Name="Eigen::Matrix&lt;*,-1,1,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,-1,1,*,*,*&gt;"/>
<DisplayString Condition="m_storage.m_data == 0">empty</DisplayString>
<DisplayString Condition="m_storage.m_data != 0">[{m_storage.m_rows}] (dynamic row vector)</DisplayString>
<Expand>
<Item Name="[size]">m_storage.m_rows</Item>
<ArrayItems>
<Size>m_storage.m_rows</Size>
<ValuePointer>m_storage.m_data</ValuePointer>
</ArrayItems>
</Expand>
</Type>
<!-- Fixed Vector -->
<Type Name="Eigen::Matrix&lt;*,1,1,*,*,*&gt;">
<AlternativeType Name="Eigen::Array&lt;*,1,1,*,*,*&gt;"/>
<DisplayString>[1] ({m_storage.m_data.array[0]})</DisplayString>
<Expand>
<Item Name="[x]">m_storage.m_data.array[0]</Item>
</Expand>
</Type>
<Type Name="Eigen::Matrix&lt;*,2,1,*,*,*&gt;">
<AlternativeType Name="Eigen::Matrix&lt;*,1,2,*,*,*&gt;"/>
<AlternativeType Name="Eigen::Array&lt;*,2,1,*,*,*&gt;"/>
<AlternativeType Name="Eigen::Array&lt;*,1,2,*,*,*&gt;"/>
<DisplayString>[2] ({m_storage.m_data.array[0]}, {m_storage.m_data.array[1]})</DisplayString>
<Expand>
<Item Name="[x]">m_storage.m_data.array[0]</Item>
<Item Name="[y]">m_storage.m_data.array[1]</Item>
</Expand>
</Type>
<Type Name="Eigen::Matrix&lt;*,3,1,*,*,*&gt;">
<AlternativeType Name="Eigen::Matrix&lt;*,1,3,*,*,*&gt;"/>
<AlternativeType Name="Eigen::Array&lt;*,3,1,*,*,*&gt;"/>
<AlternativeType Name="Eigen::Array&lt;*,1,3,*,*,*&gt;"/>
<DisplayString>[3] ({m_storage.m_data.array[0]}, {m_storage.m_data.array[1]}, {m_storage.m_data.array[2]})</DisplayString>
<Expand>
<Item Name="[x]">m_storage.m_data.array[0]</Item>
<Item Name="[y]">m_storage.m_data.array[1]</Item>
<Item Name="[z]">m_storage.m_data.array[2]</Item>
</Expand>
</Type>
<Type Name="Eigen::Matrix&lt;*,4,1,*,*,*&gt;">
<AlternativeType Name="Eigen::Matrix&lt;*,1,4,*,*,*&gt;"/>
<AlternativeType Name="Eigen::Array&lt;*,4,1,*,*,*&gt;"/>
<AlternativeType Name="Eigen::Array&lt;*,1,4,*,*,*&gt;"/>
<DisplayString>[4] ({m_storage.m_data.array[0]}, {m_storage.m_data.array[1]}, {m_storage.m_data.array[2]}, {m_storage.m_data.array[3]})</DisplayString>
<Expand>
<Item Name="[x]">m_storage.m_data.array[0]</Item>
<Item Name="[y]">m_storage.m_data.array[1]</Item>
<Item Name="[z]">m_storage.m_data.array[2]</Item>
<Item Name="[w]">m_storage.m_data.array[3]</Item>
</Expand>
</Type>
</AutoVisualizer>
data_process_class_dll/eigen-3.3.7/debug/msvc/eigen_autoexp_part.dat
-295
View File
@@ -1,295 +0,0 @@
; ***************************************************************
; * Eigen Visualizer
; *
; * Author: Hauke Heibel <hauke.heibel@gmail.com>
; *
; * Support the enhanced debugging of the following Eigen
; * types (*: any, +:fixed dimension) :
; *
; * - Eigen::Matrix<*,4,1,*,*,*> and Eigen::Matrix<*,1,4,*,*,*>
; * - Eigen::Matrix<*,3,1,*,*,*> and Eigen::Matrix<*,1,3,*,*,*>
; * - Eigen::Matrix<*,2,1,*,*,*> and Eigen::Matrix<*,1,2,*,*,*>
; * - Eigen::Matrix<*,-1,-1,*,*,*>
; * - Eigen::Matrix<*,+,-1,*,*,*>
; * - Eigen::Matrix<*,-1,+,*,*,*>
; * - Eigen::Matrix<*,+,+,*,*,*>
; *
; * Matrices are displayed properly independantly of the memory
; * alignment (RowMajor vs. ColMajor).
; *
; * This file is distributed WITHOUT ANY WARRANTY. Please ensure
; * that your original autoexp.dat file is copied to a safe
; * place before proceeding with its modification.
; ***************************************************************
[Visualizer]
; Fixed size 4-vectors
Eigen::Matrix<*,4,1,*,*,*>|Eigen::Matrix<*,1,4,*,*,*>{
children
(
#(
[internals]: [$c,!],
x : ($c.m_storage.m_data.array)[0],
y : ($c.m_storage.m_data.array)[1],
z : ($c.m_storage.m_data.array)[2],
w : ($c.m_storage.m_data.array)[3]
)
)
preview
(
#(
"[",
4,
"](",
#array(expr: $e.m_storage.m_data.array[$i], size: 4),
")"
)
)
}
; Fixed size 3-vectors
Eigen::Matrix<*,3,1,*,*,*>|Eigen::Matrix<*,1,3,*,*,*>{
children
(
#(
[internals]: [$c,!],
x : ($c.m_storage.m_data.array)[0],
y : ($c.m_storage.m_data.array)[1],
z : ($c.m_storage.m_data.array)[2]
)
)
preview
(
#(
"[",
3,
"](",
#array(expr: $e.m_storage.m_data.array[$i], size: 3),
")"
)
)
}
; Fixed size 2-vectors
Eigen::Matrix<*,2,1,*,*,*>|Eigen::Matrix<*,1,2,*,*,*>{
children
(
#(
[internals]: [$c,!],
x : ($c.m_storage.m_data.array)[0],
y : ($c.m_storage.m_data.array)[1]
)
)
preview
(
#(
"[",
2,
"](",
#array(expr: $e.m_storage.m_data.array[$i], size: 2),
")"
)
)
}
; Fixed size 1-vectors
Eigen::Matrix<*,1,1,*,*,*>|Eigen::Matrix<*,1,1,*,*,*>{
children
(
#(
[internals]: [$c,!],
x : ($c.m_storage.m_data.array)[0]
)
)
preview
(
#(
"[",
1,
"](",
#array(expr: $e.m_storage.m_data.array[$i], size: 1),
")"
)
)
}
; Dynamic matrices (ColMajor and RowMajor support)
Eigen::Matrix<*,-1,-1,*,*,*>{
children
(
#(
[internals]: [$c,!],
rows: $c.m_storage.m_rows,
cols: $c.m_storage.m_cols,
; Check for RowMajorBit
#if ($c.Flags & 0x1) (
#array(
rank: 2,
base: 0,
expr: ($c.m_storage.m_data)[($i % $c.m_storage.m_rows)*$c.m_storage.m_cols + (($i- $i % $c.m_storage.m_rows)/$c.m_storage.m_rows)],
size: ($r==1)*$c.m_storage.m_rows+($r==0)*$c.m_storage.m_cols
)
) #else (
#array(
rank: 2,
base: 0,
expr: ($c.m_storage.m_data)[$i],
size: ($r==1)*$c.m_storage.m_rows+($r==0)*$c.m_storage.m_cols
)
)
)
)
preview
(
#(
"[",
$c.m_storage.m_rows,
",",
$c.m_storage.m_cols,
"](",
#array(
expr : [($c.m_storage.m_data)[$i],g],
size : $c.m_storage.m_rows*$c.m_storage.m_cols
),
")"
)
)
}
; Fixed rows, dynamic columns matrix (ColMajor and RowMajor support)
Eigen::Matrix<*,*,-1,*,*,*>{
children
(
#(
[internals]: [$c,!],
rows: $c.RowsAtCompileTime,
cols: $c.m_storage.m_cols,
; Check for RowMajorBit
#if ($c.Flags & 0x1) (
#array(
rank: 2,
base: 0,
expr: ($c.m_storage.m_data)[($i % $c.RowsAtCompileTime)*$c.m_storage.m_cols + (($i- $i % $c.RowsAtCompileTime)/$c.RowsAtCompileTime)],
size: ($r==1)*$c.RowsAtCompileTime+($r==0)*$c.m_storage.m_cols
)
) #else (
#array(
rank: 2,
base: 0,
expr: ($c.m_storage.m_data)[$i],
size: ($r==1)*$c.RowsAtCompileTime+($r==0)*$c.m_storage.m_cols
)
)
)
)
preview
(
#(
"[",
$c.RowsAtCompileTime,
",",
$c.m_storage.m_cols,
"](",
#array(
expr : [($c.m_storage.m_data)[$i],g],
size : $c.RowsAtCompileTime*$c.m_storage.m_cols
),
")"
)
)
}
; Dynamic rows, fixed columns matrix (ColMajor and RowMajor support)
Eigen::Matrix<*,-1,*,*,*,*>{
children
(
#(
[internals]: [$c,!],
rows: $c.m_storage.m_rows,
cols: $c.ColsAtCompileTime,
; Check for RowMajorBit
#if ($c.Flags & 0x1) (
#array(
rank: 2,
base: 0,
expr: ($c.m_storage.m_data)[($i % $c.m_storage.m_rows)*$c.ColsAtCompileTime + (($i- $i % $c.m_storage.m_rows)/$c.m_storage.m_rows)],
size: ($r==1)*$c.m_storage.m_rows+($r==0)*$c.ColsAtCompileTime
)
) #else (
#array(
rank: 2,
base: 0,
expr: ($c.m_storage.m_data)[$i],
size: ($r==1)*$c.m_storage.m_rows+($r==0)*$c.ColsAtCompileTime
)
)
)
)
preview
(
#(
"[",
$c.m_storage.m_rows,
",",
$c.ColsAtCompileTime,
"](",
#array(
expr : [($c.m_storage.m_data)[$i],g],
size : $c.m_storage.m_rows*$c.ColsAtCompileTime
),
")"
)
)
}
; Fixed size matrix (ColMajor and RowMajor support)
Eigen::Matrix<*,*,*,*,*,*>{
children
(
#(
[internals]: [$c,!],
rows: $c.RowsAtCompileTime,
cols: $c.ColsAtCompileTime,
; Check for RowMajorBit
#if ($c.Flags & 0x1) (
#array(
rank: 2,
base: 0,
expr: ($c.m_storage.m_data.array)[($i % $c.RowsAtCompileTime)*$c.ColsAtCompileTime + (($i- $i % $c.RowsAtCompileTime)/$c.RowsAtCompileTime)],
size: ($r==1)*$c.RowsAtCompileTime+($r==0)*$c.ColsAtCompileTime
)
) #else (
#array(
rank: 2,
base: 0,
expr: ($c.m_storage.m_data.array)[$i],
size: ($r==1)*$c.RowsAtCompileTime+($r==0)*$c.ColsAtCompileTime
)
)
)
)
preview
(
#(
"[",
$c.RowsAtCompileTime,
",",
$c.ColsAtCompileTime,
"](",
#array(
expr : [($c.m_storage.m_data.array)[$i],g],
size : $c.RowsAtCompileTime*$c.ColsAtCompileTime
),
")"
)
)
}
data_process_class_dll/parameters.h
+5 -3
View File
@@ -70,7 +70,8 @@
//#define DATA_RATE_MIDDLE 1
//#define DATA_RATE_FAR 1
#define DATA_RATE_TAS 0.33
//#define DATA_RATE_TAS 0.046875
#define DATA_RATE_TAS 0.0625
#define SIGMA_R 10.0 //测量误差
#define SIGMA_A 0.02
@@ -101,7 +102,7 @@
#define ALPHA_START 60 //起航夹角 120 越大越容易起批
#define TRACK_DIE_ROUND 4 // 航迹消亡时间
#define TRACK_DIE_ROUND_TAS 5
#define TRACK_DIE_ROUND_TAS 96 //
#define ASSO_THORD 3
@@ -111,7 +112,8 @@
#define TAS_QUEUE_LENGTH 1 //Tas调用的cpi数
#define MAX_TAS_NUM 1
#define T_TAS_PRED 0.33
//#define T_TAS_PRED 0.046875
#define T_TAS_PRED DATA_RATE_TAS
#define UNCONF_TARGET 0
data_process_class_dll/tas_ctrl.cpp
+30 -24
View File
@@ -3,6 +3,7 @@
#include <qmath.h>
#include"memory.h"
#include <QVector>
#include <QDebug>
using namespace std;
@@ -22,20 +23,21 @@ void TAS_Ctrl::tas_ctrl_process(QVector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
struct RadarPara Work_Parameter,
int latest_timestamp)
{
tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
tas_beam_output(trust_track,Tracking_beam);
tas_beam_output(trust_track,Tracking_beam,latest_timestamp);
//跟踪队列移位
struct Tracking_Target tas_target_tmp;
memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target));
for (int i=TAS_QUEUE_LENGTH-1;i>0;i--)
{
memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
}
memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target));
// //跟踪队列移位
// struct Tracking_Target tas_target_tmp;
// memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target));
// for (int i=TAS_QUEUE_LENGTH-1;i>0;i--)
// {
// memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
// }
// memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target));
@@ -66,7 +68,7 @@ void TAS_Ctrl::tas_target_add(QVector<Trust_Track> *trust_track,
{
for(int j=0;j<TAS_QUEUE_LENGTH;j++)
{
if(tas_target_queue[j].empty_flag==0 && j%2==0)
if(tas_target_queue[j].empty_flag==0)// && j%2==0)
{
//插入跟踪队列
tas_target_queue[j].Index=(*trust_track)[i].Track_Index;
@@ -159,13 +161,14 @@ void TAS_Ctrl::tas_target_del(QVector<Trust_Track> *trust_track,
{
//查找TAS队列里的目标是否存在
int flag=0;
for (int j=0;j<trust_track->size();j++)
for (int j=0;j<trust_track->size();j++){
{
if(tas_target_queue[i].Index==(*trust_track)[j].Track_Index && (*trust_track)[j].Track_Mode == 1)
if(tas_target_queue[i].Index==(*trust_track)[j].Track_Index && (*trust_track)[j].Track_Mode == 1 && (*trust_track)[j].manual_tracking_flag == 1)
{
flag=1;
}
}
}
//目标不存在 说明已消批 从队列里删除
if(flag == 0)
{
@@ -249,47 +252,50 @@ void TAS_Ctrl::tas_target_del(QVector<Trust_Track> *trust_track,
void TAS_Ctrl::tas_beam_output(QVector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam)
struct TrackingBeam *Tracking_beam,
int latest_timestamp)
{
if(tas_target_queue[0].empty_flag==1)
{
double H_track;
double X_now[6];
int CPI_time = 0;
for (int i=0;i<trust_track->size();i++ )
{
if((*trust_track)[i].Track_Index == tas_target_queue[0].Index)
{
H_track = (*trust_track)[i].Height;
CPI_time = (*trust_track)[i].T_track;
for(int ii=0;ii<6;ii++)
X_now[ii]=(*trust_track)[i].X[ii];
}
}
float delta_T = (latest_timestamp - CPI_time) / 1000.0f;
//预测目标位置 计算跟踪波束波位号 俯仰角
double x_track=X_now[0]+X_now[1]*T_TAS_PRED;
double y_track=X_now[3]+X_now[4]*T_TAS_PRED;
double x_track=X_now[0]+X_now[1]*delta_T;
double y_track=X_now[3]+X_now[4]*delta_T;
double amzi,range;
coor_trans Coor_trans;
Coor_trans.cart2polar(x_track,y_track,&range,&amzi);
//目标距离
Tracking_beam->Range=range;
//目标方位
Tracking_beam->Azi=amzi/PI*180;
// Tracking_beam->Azi+=6;
//目标俯仰角
double elev=asin(H_track/range)/PI*180;
if(elev<=0)
elev=0;
else if(elev>=40)
elev=40;
else
elev=elev;
// if(elev<=0)
// elev=0;
// else if(elev>=40)
// elev=40;
// else
// elev=elev;
Tracking_beam->Elev=elev;
data_process_class_dll/tas_ctrl.h
+4 -2
View File
@@ -18,7 +18,8 @@ public:
struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter);
struct RadarPara Work_Parameter,
int latest_timestamp);
private:
@@ -37,7 +38,8 @@ private:
//TAS队列信息输出
void tas_beam_output(QVector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam);
struct TrackingBeam *Tracking_beam,
int latest_timestamp);
//TAS 自动开启条件
int tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter);
data_process_class_dll/track_asso_tas.cpp
+390 -390
View File
@@ -10,12 +10,12 @@ using namespace std;
int Track_Asso_Tas::track_asso_process_tas(QVector<PointRecv> *point_recv_tas, //点迹文件
QVector<Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter, //工作参数
int tas_track_idx
)
QVector<Trust_Track> *trust_track, //航迹文件
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
int *Trust_track_num_Output, //更新航迹数
struct RadarPara Work_Parameter, //工作参数
int tas_track_idx
)
{
@@ -39,53 +39,53 @@ int Track_Asso_Tas::track_asso_process_tas(QVector<PointRecv> *poi
//输出航迹
for (int i=0;i<trust_track->size();i++)
{ // 输出更新航迹条件：
if((*trust_track)[i].Track_Index == tas_track_idx)
{
*Trust_track_num_Output= *Trust_track_num_Output+1;
if((*trust_track)[i].Track_Index == tas_track_idx)
{
*Trust_track_num_Output= *Trust_track_num_Output+1;
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
double x,y;
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
coor_trans Coor_trans;
double r, azi;
Coor_trans.cart2polar(x, y, &r, &azi);
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r;
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin((*trust_track)[i].Height/r)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V =
sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = (*trust_track)[i].Amplitude;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track)[i].T_track/1000.0;
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track)[i].point_flag;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height;
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
double x,y;
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
coor_trans Coor_trans;
double r, azi;
Coor_trans.cart2polar(x, y, &r, &azi);
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r;
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin((*trust_track)[i].Height/r)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V =
sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = (*trust_track)[i].Amplitude;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track)[i].T_track/1000.0;
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track)[i].point_flag;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height;
double Direction_Angle;
Direction_Angle=atan((*trust_track)[i].X[4]/(*trust_track)[i].X[1]);
if((*trust_track)[i].X[1]<0){Direction_Angle=Direction_Angle+PI;}
if((*trust_track)[i].X[1]>0&&(*trust_track)[i].X[4]<0){Direction_Angle=Direction_Angle+2*PI;}
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180;
double Direction_Angle;
Direction_Angle=atan((*trust_track)[i].X[4]/(*trust_track)[i].X[1]);
if((*trust_track)[i].X[1]<0){Direction_Angle=Direction_Angle+PI;}
if((*trust_track)[i].X[1]>0&&(*trust_track)[i].X[4]<0){Direction_Angle=Direction_Angle+2*PI;}
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180;
//关联点信息
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=(*trust_track)[i].range_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track)[i].azi_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track)[i].elev_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track)[i].vr_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=1;
Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = (*trust_track)[i].prf_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track)[i].X[0];
Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track)[i].X[3];
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track)[i].X[1];
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track)[i].X[4];
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*trust_track)[i].RCS;
Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;
//关联点信息
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=(*trust_track)[i].range_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track)[i].azi_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track)[i].elev_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track)[i].vr_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=1;
Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = (*trust_track)[i].prf_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track)[i].X[0];
Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track)[i].X[3];
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track)[i].X[1];
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track)[i].X[4];
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*trust_track)[i].RCS;
Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim, (*trust_track)[i].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim));
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim, (*trust_track)[i].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim));
}
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim, (*trust_track)[i].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim));
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim, (*trust_track)[i].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim));
}
}
@@ -100,131 +100,131 @@ void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, in
{
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
{
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
{
// if(point_process.size()>0)
// {
// qDebug() << "model_interaction point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
// }
// if(point_process.size()>0)
// {
// qDebug() << "model_interaction point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
// }
double u_last[3];
u_last[0]=(*trust_track)[loop_of_track].u[0];
u_last[1]=(*trust_track)[loop_of_track].u[1];
u_last[2]=(*trust_track)[loop_of_track].u[2];
double u_last[3];
u_last[0]=(*trust_track)[loop_of_track].u[0];
u_last[1]=(*trust_track)[loop_of_track].u[1];
u_last[2]=(*trust_track)[loop_of_track].u[2];
double u_t[3][3];
double c[3];
c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];
double u_t[3][3];
double c[3];
c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];
u_t[0][0]=Pt[0][0]*u_last[0]/c[0];
u_t[1][0]=Pt[1][0]*u_last[1]/c[0];
u_t[2][0]=Pt[2][0]*u_last[2]/c[0];
u_t[0][1]=Pt[0][1]*u_last[0]/c[1];
u_t[1][1]=Pt[1][1]*u_last[1]/c[1];
u_t[2][1]=Pt[2][1]*u_last[2]/c[1];
u_t[0][2]=Pt[0][2]*u_last[0]/c[2];
u_t[1][2]=Pt[1][2]*u_last[1]/c[2];
u_t[2][2]=Pt[2][2]*u_last[2]/c[2];
u_t[0][0]=Pt[0][0]*u_last[0]/c[0];
u_t[1][0]=Pt[1][0]*u_last[1]/c[0];
u_t[2][0]=Pt[2][0]*u_last[2]/c[0];
u_t[0][1]=Pt[0][1]*u_last[0]/c[1];
u_t[1][1]=Pt[1][1]*u_last[1]/c[1];
u_t[2][1]=Pt[2][1]*u_last[2]/c[1];
u_t[0][2]=Pt[0][2]*u_last[0]/c[2];
u_t[1][2]=Pt[1][2]*u_last[1]/c[2];
u_t[2][2]=Pt[2][2]*u_last[2]/c[2];
double Xo1_last[6],Xo2_last[6],Xo3_last[6],X1[6],X2[6],X3[6];
for (int i=0;i<6;i++)
{
X1[i]=(*trust_track)[loop_of_track].X1[i];
X2[i]=(*trust_track)[loop_of_track].X2[i];
X3[i]=(*trust_track)[loop_of_track].X3[i];
}
double Xo1_last[6],Xo2_last[6],Xo3_last[6],X1[6],X2[6],X3[6];
for (int i=0;i<6;i++)
{
X1[i]=(*trust_track)[loop_of_track].X1[i];
X2[i]=(*trust_track)[loop_of_track].X2[i];
X3[i]=(*trust_track)[loop_of_track].X3[i];
}
for (int i=0;i<6;i++)
{
Xo1_last[i]=u_t[0][0]*X1[i]+u_t[1][0]*X2[i]+u_t[2][0]*X3[i];
Xo2_last[i]=u_t[0][1]*X1[i]+u_t[1][1]*X2[i]+u_t[2][1]*X3[i];
Xo3_last[i]=u_t[0][2]*X1[i]+u_t[1][2]*X2[i]+u_t[2][2]*X3[i];
}
for (int i=0;i<6;i++)
{
Xo1_last[i]=u_t[0][0]*X1[i]+u_t[1][0]*X2[i]+u_t[2][0]*X3[i];
Xo2_last[i]=u_t[0][1]*X1[i]+u_t[1][1]*X2[i]+u_t[2][1]*X3[i];
Xo3_last[i]=u_t[0][2]*X1[i]+u_t[1][2]*X2[i]+u_t[2][2]*X3[i];
}
double X1_sub_Xo1[6],X2_sub_Xo1[6],X3_sub_Xo1[6];
double X1_sub_Xo2[6],X2_sub_Xo2[6],X3_sub_Xo2[6];
double X1_sub_Xo3[6],X2_sub_Xo3[6],X3_sub_Xo3[6];
double X11[6][6],X21[6][6],X31[6][6];
double X12[6][6],X22[6][6],X32[6][6];
double X13[6][6],X23[6][6],X33[6][6];
double X1_sub_Xo1[6],X2_sub_Xo1[6],X3_sub_Xo1[6];
double X1_sub_Xo2[6],X2_sub_Xo2[6],X3_sub_Xo2[6];
double X1_sub_Xo3[6],X2_sub_Xo3[6],X3_sub_Xo3[6];
double X11[6][6],X21[6][6],X31[6][6];
double X12[6][6],X22[6][6],X32[6][6];
double X13[6][6],X23[6][6],X33[6][6];
for (int i=0;i<6;i++)
{
for (int i=0;i<6;i++)
{
X1_sub_Xo1[i]=X1[i]-Xo1_last[i];
X2_sub_Xo1[i]=X2[i]-Xo1_last[i];
X3_sub_Xo1[i]=X3[i]-Xo1_last[i];
X1_sub_Xo2[i]=X1[i]-Xo2_last[i];
X2_sub_Xo2[i]=X2[i]-Xo2_last[i];
X3_sub_Xo2[i]=X3[i]-Xo2_last[i];
X1_sub_Xo3[i]=X1[i]-Xo3_last[i];
X2_sub_Xo3[i]=X2[i]-Xo3_last[i];
X3_sub_Xo3[i]=X3[i]-Xo3_last[i];
X1_sub_Xo1[i]=X1[i]-Xo1_last[i];
X2_sub_Xo1[i]=X2[i]-Xo1_last[i];
X3_sub_Xo1[i]=X3[i]-Xo1_last[i];
X1_sub_Xo2[i]=X1[i]-Xo2_last[i];
X2_sub_Xo2[i]=X2[i]-Xo2_last[i];
X3_sub_Xo2[i]=X3[i]-Xo2_last[i];
X1_sub_Xo3[i]=X1[i]-Xo3_last[i];
X2_sub_Xo3[i]=X2[i]-Xo3_last[i];
X3_sub_Xo3[i]=X3[i]-Xo3_last[i];
}
}
for (int i=0;i<6;i++)
for(int j=0;j<6;j++)
{
X11[i][j]=X1_sub_Xo1[i]*X1_sub_Xo1[j];
X21[i][j]=X2_sub_Xo1[i]*X2_sub_Xo1[j];
X31[i][j]=X3_sub_Xo1[i]*X3_sub_Xo1[j];
X12[i][j]=X1_sub_Xo2[i]*X1_sub_Xo2[j];
X22[i][j]=X2_sub_Xo2[i]*X2_sub_Xo2[j];
X32[i][j]=X3_sub_Xo2[i]*X3_sub_Xo2[j];
X13[i][j]=X1_sub_Xo3[i]*X1_sub_Xo3[j];
X23[i][j]=X2_sub_Xo3[i]*X2_sub_Xo3[j];
X33[i][j]=X3_sub_Xo3[i]*X3_sub_Xo3[j];
}
for (int i=0;i<6;i++)
for(int j=0;j<6;j++)
{
X11[i][j]=X1_sub_Xo1[i]*X1_sub_Xo1[j];
X21[i][j]=X2_sub_Xo1[i]*X2_sub_Xo1[j];
X31[i][j]=X3_sub_Xo1[i]*X3_sub_Xo1[j];
X12[i][j]=X1_sub_Xo2[i]*X1_sub_Xo2[j];
X22[i][j]=X2_sub_Xo2[i]*X2_sub_Xo2[j];
X32[i][j]=X3_sub_Xo2[i]*X3_sub_Xo2[j];
X13[i][j]=X1_sub_Xo3[i]*X1_sub_Xo3[j];
X23[i][j]=X2_sub_Xo3[i]*X2_sub_Xo3[j];
X33[i][j]=X3_sub_Xo3[i]*X3_sub_Xo3[j];
}
double Po1_last[6][6],Po2_last[6][6],Po3_last[6][6];
for (int i=0;i<6;i++)
for(int j=0;j<6;j++)
{
Po1_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X11[i][j])*u_t[0][0]+((*trust_track)[loop_of_track].P2[i][j]+X21[i][j])*u_t[1][0]+((*trust_track)[loop_of_track].P3[i][j]+X31[i][j])*u_t[2][0];
Po2_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X12[i][j])*u_t[0][1]+((*trust_track)[loop_of_track].P2[i][j]+X22[i][j])*u_t[1][1]+((*trust_track)[loop_of_track].P3[i][j]+X32[i][j])*u_t[2][1];
Po3_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X13[i][j])*u_t[0][2]+((*trust_track)[loop_of_track].P2[i][j]+X23[i][j])*u_t[1][2]+((*trust_track)[loop_of_track].P3[i][j]+X33[i][j])*u_t[2][2];
}
double Po1_last[6][6],Po2_last[6][6],Po3_last[6][6];
for (int i=0;i<6;i++)
for(int j=0;j<6;j++)
{
Po1_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X11[i][j])*u_t[0][0]+((*trust_track)[loop_of_track].P2[i][j]+X21[i][j])*u_t[1][0]+((*trust_track)[loop_of_track].P3[i][j]+X31[i][j])*u_t[2][0];
Po2_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X12[i][j])*u_t[0][1]+((*trust_track)[loop_of_track].P2[i][j]+X22[i][j])*u_t[1][1]+((*trust_track)[loop_of_track].P3[i][j]+X32[i][j])*u_t[2][1];
Po3_last[i][j]=((*trust_track)[loop_of_track].P1[i][j]+X13[i][j])*u_t[0][2]+((*trust_track)[loop_of_track].P2[i][j]+X23[i][j])*u_t[1][2]+((*trust_track)[loop_of_track].P3[i][j]+X33[i][j])*u_t[2][2];
}
for(int i=0;i<6;i++)
{
(*trust_track)[loop_of_track].X1[i]=Xo1_last[i];
(*trust_track)[loop_of_track].X2[i]=Xo2_last[i];
(*trust_track)[loop_of_track].X3[i]=Xo3_last[i];
}
for(int i=0;i<6;i++)
for (int j=0;j<6;j++)
{
(*trust_track)[loop_of_track].P1[i][j]=Po1_last[i][j];
(*trust_track)[loop_of_track].P2[i][j]=Po2_last[i][j];
(*trust_track)[loop_of_track].P3[i][j]=Po3_last[i][j];
}
}
}
for(int i=0;i<6;i++)
{
(*trust_track)[loop_of_track].X1[i]=Xo1_last[i];
(*trust_track)[loop_of_track].X2[i]=Xo2_last[i];
(*trust_track)[loop_of_track].X3[i]=Xo3_last[i];
}
for(int i=0;i<6;i++)
for (int j=0;j<6;j++)
{
(*trust_track)[loop_of_track].P1[i][j]=Po1_last[i][j];
(*trust_track)[loop_of_track].P2[i][j]=Po2_last[i][j];
(*trust_track)[loop_of_track].P3[i][j]=Po3_last[i][j];
}
}
}
}
void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust_track, //滤波
struct RadarPara Work_Parameter,
int tas_track_idx
)
struct RadarPara Work_Parameter,
int tas_track_idx
)
{
//存储关联信息
struct asso_info
@@ -247,29 +247,29 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
double h_track;
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
{
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
{
// if(point_process.size()>0)
// {
// qDebug() << "model_filter point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
// }
// if(point_process.size()>0)
// {
// qDebug() << "model_filter point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
// }
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
memcpy(X3,(*trust_track)[loop_of_track].X3,6*sizeof(double));
memcpy(P1,(*trust_track)[loop_of_track].P1,6*6*sizeof(double));
memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
memcpy(X3,(*trust_track)[loop_of_track].X3,6*sizeof(double));
memcpy(P1,(*trust_track)[loop_of_track].P1,6*6*sizeof(double));
memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
T_track = (*trust_track)[loop_of_track].T_track;
v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
r_track = sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));
h_track = (*trust_track)[loop_of_track].Height;
}
T_track = (*trust_track)[loop_of_track].T_track;
v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
r_track = sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));
h_track = (*trust_track)[loop_of_track].Height;
}
}
}
@@ -277,12 +277,12 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
for (int loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
{
//量测信息
// double Z[2]={point_process[loop_of_point].Range*cos(point_process[loop_of_point].Azimuth),
// point_process[loop_of_point].Range*sin(point_process[loop_of_point].Azimuth)};
// double Z[2]={point_process[loop_of_point].Range*cos(point_process[loop_of_point].Azimuth),
// point_process[loop_of_point].Range*sin(point_process[loop_of_point].Azimuth)};
double Z[3] = {point_process[loop_of_point].Range,
point_process[loop_of_point].Azimuth,
point_process[loop_of_point].Velocity};
// double v_point = point_process[loop_of_point].Velocity;
point_process[loop_of_point].Azimuth,
point_process[loop_of_point].Velocity};
// double v_point = point_process[loop_of_point].Velocity;
double prt = point_process[loop_of_point].PRF_index;
double freq_ind = point_process[loop_of_point].Freq_index;
double delta_T = (point_process[loop_of_point].CPI_Time - T_track)/1000.0;
@@ -298,7 +298,7 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
{
if(abs(h_track-h_point)<=150)
{
d_h = 1;
d_h = 1;
}
else
{
@@ -310,7 +310,7 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
{
if(abs(h_track-h_point)<=200)
{
d_h = 1;
d_h = 1;
}
else
{
@@ -322,7 +322,7 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
{
if(abs(h_track-h_point)<=200)
{
d_h = 1;
d_h = 1;
}
else
{
@@ -333,7 +333,7 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
{
if(abs(h_track-h_point)<=300)
{
d_h = 1;
d_h = 1;
}
else
{
@@ -342,20 +342,20 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
}
else
{
d_h = 1;
d_h = 1;
}
// qDebug() << "T d" <<point_process[loop_of_point].CPI_Time/1000<<" "<<T_track/1000<<" "<<delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// qDebug() << "h_track" <<h_track<<" h_point"<<h_point<<" d_h"<<d_h;
// qDebug() << "T d" <<point_process[loop_of_point].CPI_Time/1000<<" "<<T_track/1000<<" "<<delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
// qDebug() << "h_track" <<h_track<<" h_point"<<h_point<<" d_h"<<d_h;
//小于关联门限 保存关联信息
if((d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD)
)// && d_h == 1
)// && d_h == 1
{
// qDebug() << "TAS asoooooo111" ;
// qDebug() << "TAS asoooooo111" ;
struct asso_info associated_info_tmp;
associated_info_tmp.d1=d1;
associated_info_tmp.d2=d2;
@@ -376,7 +376,7 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
if (associated_info.size()>0)
{
// qDebug() << "TAS asoooooo222" ;
// qDebug() << "TAS asoooooo222" ;
//找最近点
int min_index=1;
@@ -398,9 +398,9 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
double Z[3]={point_process[point_index-1].Range,point_process[point_index-1].Azimuth,point_process[point_index-1].Velocity};
double prt = point_process[point_index-1].PRF_index;
double freq_ind = point_process[point_index-1].Freq_index;
// double Z[2];
// Z[0]=point_process[point_index-1].Range*cos(point_process[point_index-1].Azimuth);
// Z[1]=point_process[point_index-1].Range*sin(point_process[point_index-1].Azimuth);
// double Z[2];
// Z[0]=point_process[point_index-1].Range*cos(point_process[point_index-1].Azimuth);
// Z[1]=point_process[point_index-1].Range*sin(point_process[point_index-1].Azimuth);
double delta_T = (point_process[point_index-1].CPI_Time - T_track)/1000.0;
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
@@ -408,9 +408,9 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
double S1[2][2],S2[2][2],S3[2][2];
kalman Kalman;
// Kalman.kalman_filter(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1);
// Kalman.kalman_filter(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2);
// Kalman.kalman_filter(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3);
// Kalman.kalman_filter(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1);
// Kalman.kalman_filter(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2);
// Kalman.kalman_filter(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3);
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind );
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind );
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind );
@@ -432,65 +432,65 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
//更新航迹
for(int i=0;i<trust_track->size();i++)
if((*trust_track)[i].Track_Index == tas_track_idx)
if((*trust_track)[i].Track_Index == tas_track_idx)
{
//更新模型概率
double u_last[3];
u_last[0]=(*trust_track)[i].u[0];
u_last[1]=(*trust_track)[i].u[1];
u_last[2]=(*trust_track)[i].u[2];
double c[3];
c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];
(*trust_track)[i].u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
(*trust_track)[i].u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
(*trust_track)[i].u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
//更新 X1 X2 X3 P1 P2 P3
for(int ii=0;ii<6;ii++)
{
//更新模型概率
double u_last[3];
u_last[0]=(*trust_track)[i].u[0];
u_last[1]=(*trust_track)[i].u[1];
u_last[2]=(*trust_track)[i].u[2];
double c[3];
c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2];
c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2];
c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2];
(*trust_track)[i].u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
(*trust_track)[i].u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
(*trust_track)[i].u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
//更新 X1 X2 X3 P1 P2 P3
for(int ii=0;ii<6;ii++)
{
(*trust_track)[i].X1[ii]=X1_filter[ii];
(*trust_track)[i].X2[ii]=X2_filter[ii];
(*trust_track)[i].X3[ii]=X3_filter[ii];
}
for(int ii=0;ii<6;ii++)
for (int jj=0;jj<6;jj++)
{
(*trust_track)[i].P1[ii][jj]=P1_filter[ii][jj];
(*trust_track)[i].P2[ii][jj]=P2_filter[ii][jj];
(*trust_track)[i].P3[ii][jj]=P3_filter[ii][jj];
}
//更新航迹时间
(*trust_track)[i].T_track = point_process[point_index-1].CPI_Time;
//更新关联上的点迹信息
(*trust_track)[i].range_point=point_process[point_index-1].Range;
(*trust_track)[i].azi_point=point_process[point_index-1].Azimuth/PI*180;
(*trust_track)[i].elev_point= asin(point_process[point_index-1].Height/point_process[point_index-1].Range)/PI*180;
(*trust_track)[i].vr_point=point_process[point_index-1].Velocity;
(*trust_track)[i].point_type = 1;
(*trust_track)[i].prf_point = point_process[point_index-1].PRF_index;
(*trust_track)[i].snr_point = point_process[point_index-1].snr;
(*trust_track)[i].Extrapolate_round = 0; //连续未用实点更新时间
(*trust_track)[i].point_flag=1; //实点
(*trust_track)[i].Amplitude= point_process[point_index-1].Amplitude; //幅度
(*trust_track)[i].associate_point_number = (*trust_track)[i].associate_point_number+1; //关联点数+1
(*trust_track)[i].RCS = point_process[point_index-1].RCS;
(*trust_track)[i].pitch_num = point_process[point_index-1].pitch_num;
std::memcpy((*trust_track)[i].speed_dim, point_process[point_index-1].speed_dim, sizeof((*trust_track)[i].speed_dim));
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);
(*trust_track)[i].X1[ii]=X1_filter[ii];
(*trust_track)[i].X2[ii]=X2_filter[ii];
(*trust_track)[i].X3[ii]=X3_filter[ii];
}
for(int ii=0;ii<6;ii++)
for (int jj=0;jj<6;jj++)
{
(*trust_track)[i].P1[ii][jj]=P1_filter[ii][jj];
(*trust_track)[i].P2[ii][jj]=P2_filter[ii][jj];
(*trust_track)[i].P3[ii][jj]=P3_filter[ii][jj];
}
//更新航迹时间
(*trust_track)[i].T_track = point_process[point_index-1].CPI_Time;
//更新关联上的点迹信息
(*trust_track)[i].range_point=point_process[point_index-1].Range;
(*trust_track)[i].azi_point=point_process[point_index-1].Azimuth/PI*180;
(*trust_track)[i].elev_point= asin(point_process[point_index-1].Height/point_process[point_index-1].Range)/PI*180;
(*trust_track)[i].vr_point=point_process[point_index-1].Velocity;
(*trust_track)[i].point_type = 1;
(*trust_track)[i].prf_point = point_process[point_index-1].PRF_index;
(*trust_track)[i].snr_point = point_process[point_index-1].snr;
(*trust_track)[i].Extrapolate_round = 0; //连续未用实点更新时间
(*trust_track)[i].point_flag=1; //实点
(*trust_track)[i].Amplitude= point_process[point_index-1].Amplitude; //幅度
(*trust_track)[i].associate_point_number = (*trust_track)[i].associate_point_number+1; //关联点数+1
(*trust_track)[i].RCS = point_process[point_index-1].RCS;
(*trust_track)[i].pitch_num = point_process[point_index-1].pitch_num;
std::memcpy((*trust_track)[i].speed_dim, point_process[point_index-1].speed_dim, sizeof((*trust_track)[i].speed_dim));
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);
}
}
//未关联上，航迹外推
@@ -532,72 +532,72 @@ void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust
void Track_Asso_Tas::model_output(QVector<Trust_Track> *trust_track, int tas_track_idx )
{
for(int i=0; i<trust_track->size(); i++)
if((*trust_track)[i].Track_Index==tas_track_idx)
{
if((*trust_track)[i].Track_Index==tas_track_idx)
{
double u_now[3];
u_now[0]=(*trust_track)[i].u[0];
u_now[1]=(*trust_track)[i].u[1];
u_now[2]=(*trust_track)[i].u[2];
double u_now[3];
u_now[0]=(*trust_track)[i].u[0];
u_now[1]=(*trust_track)[i].u[1];
u_now[2]=(*trust_track)[i].u[2];
double X1_filter[6],X2_filter[6],X3_filter[6];
double P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
for (int ii=0;ii<6;ii++)
double X1_filter[6],X2_filter[6],X3_filter[6];
double P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
for (int ii=0;ii<6;ii++)
{
X1_filter[ii]=(*trust_track)[i].X1[ii];
X2_filter[ii]=(*trust_track)[i].X2[ii];
X3_filter[ii]=(*trust_track)[i].X3[ii];
}
for (int ii=0;ii<6;ii++)
for(int jj=0;jj<6;jj++)
{
X1_filter[ii]=(*trust_track)[i].X1[ii];
X2_filter[ii]=(*trust_track)[i].X2[ii];
X3_filter[ii]=(*trust_track)[i].X3[ii];
P1_filter[ii][jj]=(*trust_track)[i].P1[ii][jj];
P2_filter[ii][jj]=(*trust_track)[i].P2[ii][jj];
P3_filter[ii][jj]=(*trust_track)[i].P3[ii][jj];
}
for (int ii=0;ii<6;ii++)
for(int jj=0;jj<6;jj++)
{
P1_filter[ii][jj]=(*trust_track)[i].P1[ii][jj];
P2_filter[ii][jj]=(*trust_track)[i].P2[ii][jj];
P3_filter[ii][jj]=(*trust_track)[i].P3[ii][jj];
}
double X_filter[6], P_filter[6][6];
for (int ii=0;ii<6;ii++)
X_filter[ii]=u_now[0]*X1_filter[ii]+u_now[1]*X2_filter[ii]+u_now[2]*X3_filter[ii];
double X_filter[6], P_filter[6][6];
for (int ii=0;ii<6;ii++)
X_filter[ii]=u_now[0]*X1_filter[ii]+u_now[1]*X2_filter[ii]+u_now[2]*X3_filter[ii];
double X1_sub_X[6], X2_sub_X[6], X3_sub_X[6];
double X1X[6][6], X2X[6][6],X3X[6][6];
for (int ii=0;ii<6;ii++)
double X1_sub_X[6], X2_sub_X[6], X3_sub_X[6];
double X1X[6][6], X2X[6][6],X3X[6][6];
for (int ii=0;ii<6;ii++)
{
X1_sub_X[ii]=X1_filter[ii]-X_filter[ii];
X2_sub_X[ii]=X2_filter[ii]-X_filter[ii];
X3_sub_X[ii]=X3_filter[ii]-X_filter[ii];
}
for(int ii=0;ii<6;ii++)
for (int jj=0;jj<6;jj++)
{
X1_sub_X[ii]=X1_filter[ii]-X_filter[ii];
X2_sub_X[ii]=X2_filter[ii]-X_filter[ii];
X3_sub_X[ii]=X3_filter[ii]-X_filter[ii];
X1X[ii][jj]=X1_sub_X[ii]*X1_sub_X[jj];
X2X[ii][jj]=X2_sub_X[ii]*X2_sub_X[jj];
X3X[ii][jj]=X3_sub_X[ii]*X3_sub_X[jj];
}
for(int ii=0;ii<6;ii++)
for (int jj=0;jj<6;jj++)
{
X1X[ii][jj]=X1_sub_X[ii]*X1_sub_X[jj];
X2X[ii][jj]=X2_sub_X[ii]*X2_sub_X[jj];
X3X[ii][jj]=X3_sub_X[ii]*X3_sub_X[jj];
}
for(int ii=0;ii<6;ii++)
for (int jj=0;jj<6;jj++)
P_filter[ii][jj]=u_now[0]*(P1_filter[ii][jj]+X1X[ii][jj])+u_now[1]*(P2_filter[ii][jj]+X2X[ii][jj])+u_now[2]*(P3_filter[ii][jj]+X3X[ii][jj]);
for(int ii=0;ii<6;ii++)
for (int jj=0;jj<6;jj++)
P_filter[ii][jj]=u_now[0]*(P1_filter[ii][jj]+X1X[ii][jj])+u_now[1]*(P2_filter[ii][jj]+X2X[ii][jj])+u_now[2]*(P3_filter[ii][jj]+X3X[ii][jj]);
//本地航迹文件更新
(*trust_track)[i].X[0]=X_filter[0]; //位置 速度
(*trust_track)[i].X[1]=X_filter[1];
(*trust_track)[i].X[2]=X_filter[2];
(*trust_track)[i].X[3]=X_filter[3];
(*trust_track)[i].X[4]=X_filter[4];
(*trust_track)[i].X[5]=X_filter[5];
//本地航迹文件更新
(*trust_track)[i].X[0]=X_filter[0]; //位置 速度
(*trust_track)[i].X[1]=X_filter[1];
(*trust_track)[i].X[2]=X_filter[2];
(*trust_track)[i].X[3]=X_filter[3];
(*trust_track)[i].X[4]=X_filter[4];
(*trust_track)[i].X[5]=X_filter[5];
for(int ii=0;ii<6;ii++) //协方差矩阵
for (int jj=0;jj<6;jj++)
(*trust_track)[i].P[ii][jj]=P_filter[ii][jj];
for(int ii=0;ii<6;ii++) //协方差矩阵
for (int jj=0;jj<6;jj++)
(*trust_track)[i].P[ii][jj]=P_filter[ii][jj];
//航迹区更新
double r, azi;
coor_trans Coor_trans;
Coor_trans.cart2polar((*trust_track)[i].X[0], (*trust_track)[i].X[3], &r, &azi);
//航迹区更新
double r, azi;
coor_trans Coor_trans;
Coor_trans.cart2polar((*trust_track)[i].X[0], (*trust_track)[i].X[3], &r, &azi);
}
}
}
@@ -665,58 +665,58 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6],
q2=10.0*Work_Parameter.Model2_Q_fast;
}
else
{
q2=Work_Parameter.Model2_Q_fast;
}
{
q2=Work_Parameter.Model2_Q_fast;
}
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q122=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
double q132=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
double q222=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
double q232=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
double q332=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q122=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
double q132=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
double q222=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
double q232=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
double q332=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
memset(Q2,0,36*sizeof(double));
Q2[0][0]=q2*q112;Q2[0][1]=q2*q122; Q2[0][2]=q2*q132;
Q2[1][0]=q2*q122;Q2[1][1]=q2*q222; Q2[1][2]=q2*q232;
Q2[2][0]=q2*q132; Q2[2][1]=q2*q232; Q2[2][2]=q2*q332;
memset(Q2,0,36*sizeof(double));
Q2[0][0]=q2*q112;Q2[0][1]=q2*q122; Q2[0][2]=q2*q132;
Q2[1][0]=q2*q122;Q2[1][1]=q2*q222; Q2[1][2]=q2*q232;
Q2[2][0]=q2*q132; Q2[2][1]=q2*q232; Q2[2][2]=q2*q332;
Q2[3][3]=q2*q112; Q2[3][4]=q2*q122; Q2[3][5]=q2*q132;
Q2[4][3]=q2*q122; Q2[4][4]=q2*q222; Q2[4][5]=q2*q232;
Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332;
Q2[3][3]=q2*q112; Q2[3][4]=q2*q122; Q2[3][5]=q2*q132;
Q2[4][3]=q2*q122; Q2[4][4]=q2*q222; Q2[4][5]=q2*q232;
Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332;
//模型3
//Q
double q3;
if(v_track<=5)
{
q3=Work_Parameter.Model3_Q_slow/20;
}
else if(v_track<=100&&v_track>50)
{
q3=10.0*Work_Parameter.Model3_Q_fast;
}
else
{
q3=Work_Parameter.Model3_Q_fast;
}
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q123=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
double q133=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
double q223=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
double q233=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
double q333=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
//模型3
//Q
double q3;
if(v_track<=5)
{
q3=Work_Parameter.Model3_Q_slow/20;
}
else if(v_track<=100&&v_track>50)
{
q3=10.0*Work_Parameter.Model3_Q_fast;
}
else
{
q3=Work_Parameter.Model3_Q_fast;
}
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
double q123=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
double q133=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
double q223=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
double q233=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
double q333=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
memset(Q3,0,36*sizeof(double));
Q3[0][0]=q3*q113; Q3[0][1]=q3*q123; Q3[0][2]=q3*q133;
Q3[1][0]=q3*q123; Q3[1][1]=q3*q223; Q3[1][2]=q3*q233;
Q3[2][0]=q3*q133; Q3[2][1]=q3*q233; Q3[2][2]=q3*q333;
memset(Q3,0,36*sizeof(double));
Q3[0][0]=q3*q113; Q3[0][1]=q3*q123; Q3[0][2]=q3*q133;
Q3[1][0]=q3*q123; Q3[1][1]=q3*q223; Q3[1][2]=q3*q233;
Q3[2][0]=q3*q133; Q3[2][1]=q3*q233; Q3[2][2]=q3*q333;
Q3[3][3]=q3*q113; Q3[3][4]=q3*q123; Q3[3][5]=q3*q133;
Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233;
Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333;
Q3[3][3]=q3*q113; Q3[3][4]=q3*q123; Q3[3][5]=q3*q133;
Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233;
Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333;
}
@@ -724,21 +724,21 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6],
// 计算三个 d
void Track_Asso_Tas::IMM_d_cal(double v_track,
double X1[6], double P1[6][6],
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,
struct RadarPara Work_Parameter)
double X1[6], double P1[6][6],
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,
struct RadarPara Work_Parameter)
{
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
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_with_doppler(F,Q1,Z,X1,P1,v_point,prt,freq_ind);
// *d2=Kalman.d_cal_with_doppler(F,Q2,Z,X2,P2,v_point,prt,freq_ind);
// *d3=Kalman.d_cal_with_doppler(F,Q3,Z,X3,P3,v_point,prt,freq_ind);
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
@@ -749,62 +749,62 @@ void Track_Asso_Tas::IMM_d_cal(double v_track,
//高度维更新
void Track_Asso_Tas::track_hight_update(int tas_track_idx, //更新的航迹号
int asso_point_index, //点迹号
QVector<Trust_Track> *trust_track //航迹
)
int asso_point_index, //点迹号
QVector<Trust_Track> *trust_track //航迹
)
{
for(int i=0; i<trust_track->size(); i++)
if((*trust_track)[i].Track_Index==tas_track_idx)
{
(*trust_track)[i].Hight_smooth.push_back(point_process[asso_point_index-1].Height);
{
(*trust_track)[i].Hight_smooth.push_back(point_process[asso_point_index-1].Height);
int height_win_length;
if((*trust_track)[i].range_point <= 1000)
{
height_win_length = H_F_WIN_LEN+2;
}
else if((*trust_track)[i].range_point <= 2000 && (*trust_track)[i].range_point > 1000)
{
height_win_length = H_F_WIN_LEN+3;
}
else if((*trust_track)[i].range_point <= 4000 && (*trust_track)[i].range_point > 2000)
{
height_win_length = H_F_WIN_LEN+4;
}
else
{
height_win_length = H_F_WIN_LEN+6;
}
int height_win_length;
if((*trust_track)[i].range_point <= 1000)
{
height_win_length = H_F_WIN_LEN+2;
}
else if((*trust_track)[i].range_point <= 2000 && (*trust_track)[i].range_point > 1000)
{
height_win_length = H_F_WIN_LEN+3;
}
else if((*trust_track)[i].range_point <= 4000 && (*trust_track)[i].range_point > 2000)
{
height_win_length = H_F_WIN_LEN+4;
}
else
{
height_win_length = H_F_WIN_LEN+6;
}
double sum=0;
if((*trust_track)[i].Hight_smooth.size()<height_win_length)
{
double sum=0;
if((*trust_track)[i].Hight_smooth.size()<height_win_length)
{
for (int ii=0;ii<(*trust_track)[i].Hight_smooth.size();ii++)
{
sum=sum+(*trust_track)[i].Hight_smooth[ii];
for (int ii=0;ii<(*trust_track)[i].Hight_smooth.size();ii++)
{
sum=sum+(*trust_track)[i].Hight_smooth[ii];
}
}
(*trust_track)[i].Height=sum/(*trust_track)[i].Hight_smooth.size();
(*trust_track)[i].Height=sum/(*trust_track)[i].Hight_smooth.size();
}
else if((*trust_track)[i].Hight_smooth.size()>=height_win_length)
{
}
else if((*trust_track)[i].Hight_smooth.size()>=height_win_length)
{
int N=(*trust_track)[i].Hight_smooth.size();
for (int ii=0;ii<height_win_length;ii++)
{
sum=sum+(*trust_track)[i].Hight_smooth[N-1-ii];
}
(*trust_track)[i].Height=sum/height_win_length;
}
int N=(*trust_track)[i].Hight_smooth.size();
for (int ii=0;ii<height_win_length;ii++)
{
sum=sum+(*trust_track)[i].Hight_smooth[N-1-ii];
}
(*trust_track)[i].Height=sum/height_win_length;
}
}
}
}
data_process_class_dll/track_die_tas.cpp
+6 -4
View File
@@ -4,22 +4,24 @@
#include <qmath.h>
#include"memory.h"
#include <QVector>
#include <QDebug>
using namespace std;
void Track_Die_Tas::track_die_process_tas( QVector <Trust_Track> *trust_track,
int Track_die_Index_Output[],
int *Track_die_num_Output)
{
int Track_die_Index_Output[],
int *Track_die_num_Output)
{
QVector <Trust_Track>::iterator Iter;
for (Iter=trust_track->begin(); Iter!=trust_track->end();)
{
if( ((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND_TAS)
||(*Iter).manual_delete_flag == 1)
||(*Iter).manual_delete_flag == 1)
{
qDebug() << "remove tas target :" << (*Iter).Track_Index;
//输出消亡信息
*Track_die_num_Output=*Track_die_num_Output+1;
Track_die_Index_Output[*Track_die_num_Output-1]=(*Iter).Track_Index;