更新：1、修复kalman.cpp中部分参数命名冲突的问题；
2、TWS点迹关联时增加点航距离门限，限制部分突然关联到很远的点迹的问题。 Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
+3
-1
@@ -1,2 +1,4 @@
|
||||
##忽略此文件夹##
|
||||
build-data_process_class_dll-Desktop_Qt_5_7_0_MSVC2013_32bit-Release/
|
||||
build-data_process_class_dll-Desktop_Qt_5_7_0_MSVC2013_32bit-Release/
|
||||
debug/
|
||||
release/
|
||||
@@ -0,0 +1,426 @@
|
||||
#############################################################################
|
||||
# Makefile for building: data_process_class_dll
|
||||
# Generated by qmake (3.0) (Qt 5.7.0)
|
||||
# Project: data_process_class_dll\data_process_class_dll.pro
|
||||
# Template: lib
|
||||
# Command: C:\Qt\Qt5.7.0\5.7\msvc2013\bin\qmake.exe -spec win32-msvc2013 -o Makefile data_process_class_dll\data_process_class_dll.pro
|
||||
#############################################################################
|
||||
|
||||
MAKEFILE = Makefile
|
||||
|
||||
first: release
|
||||
install: release-install
|
||||
uninstall: release-uninstall
|
||||
QMAKE = C:\Qt\Qt5.7.0\5.7\msvc2013\bin\qmake.exe
|
||||
DEL_FILE = del
|
||||
CHK_DIR_EXISTS= if not exist
|
||||
MKDIR = mkdir
|
||||
COPY = copy /y
|
||||
COPY_FILE = copy /y
|
||||
COPY_DIR = xcopy /s /q /y /i
|
||||
INSTALL_FILE = copy /y
|
||||
INSTALL_PROGRAM = copy /y
|
||||
INSTALL_DIR = xcopy /s /q /y /i
|
||||
DEL_FILE = del
|
||||
SYMLINK = $(QMAKE) -install ln -f -s
|
||||
DEL_DIR = rmdir
|
||||
MOVE = move
|
||||
SUBTARGETS = \
|
||||
release \
|
||||
debug
|
||||
|
||||
|
||||
release: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Release
|
||||
release-make_first: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Release
|
||||
release-all: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Release all
|
||||
release-clean: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Release clean
|
||||
release-distclean: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Release distclean
|
||||
release-install: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Release install
|
||||
release-uninstall: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Release uninstall
|
||||
debug: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Debug
|
||||
debug-make_first: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Debug
|
||||
debug-all: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Debug all
|
||||
debug-clean: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Debug clean
|
||||
debug-distclean: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Debug distclean
|
||||
debug-install: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Debug install
|
||||
debug-uninstall: FORCE
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Debug uninstall
|
||||
|
||||
Makefile: data_process_class_dll\data_process_class_dll.pro C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\win32-msvc2013\qmake.conf C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\spec_pre.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\common\angle.conf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\common\msvc-base.conf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\common\msvc-desktop.conf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\qconfig.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dcore.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dcore_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dextras.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dextras_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dinput.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dinput_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dlogic.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dlogic_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquick.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquick_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickextras.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickextras_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickinput.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickinput_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickrender.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickrender_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3drender.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3drender_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axbase.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axbase_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axcontainer.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axcontainer_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axserver.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axserver_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_bluetooth.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_bluetooth_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_bootstrap_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_charts.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_charts_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_clucene_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_concurrent.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_concurrent_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_core.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_core_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_datavisualization.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_datavisualization_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_dbus.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_dbus_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_designer.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_designer_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_designercomponents_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_gamepad.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_gamepad_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_gui.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_gui_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_help.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_help_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_location.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_location_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_multimedia.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_multimedia_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_multimediawidgets.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_multimediawidgets_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_network.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_network_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_nfc.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_nfc_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_opengl.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_opengl_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_openglextensions.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_openglextensions_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_packetprotocol_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_platformsupport_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_positioning.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_positioning_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_printsupport.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_printsupport_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_purchasing.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_purchasing_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qml.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qml_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qmldebug_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qmldevtools_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qmltest.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qmltest_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qtmultimediaquicktools_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quick.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quick_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickcontrols2.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickcontrols2_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickparticles_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quicktemplates2_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickwidgets.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickwidgets_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_script.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_script_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_scripttools.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_scripttools_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_scxml.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_scxml_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_sensors.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_sensors_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_serialbus.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_serialbus_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_serialport.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_serialport_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_sql.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_sql_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_svg.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_svg_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_testlib.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_testlib_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_uiplugin.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_uitools.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_uitools_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webchannel.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webchannel_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webengine.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webengine_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginecore.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginecore_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginecoreheaders_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginewidgets.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginewidgets_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_websockets.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_websockets_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webview.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webview_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_widgets.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_widgets_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_winextras.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_winextras_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_xml.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_xml_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_xmlpatterns.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_xmlpatterns_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_zlib_private.pri \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\qt_functions.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\qt_config.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\win32\qt_config.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\win32-msvc2013\qmake.conf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\spec_post.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\exclusive_builds.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\default_pre.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\win32\default_pre.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\resolve_config.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\exclusive_builds_post.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\default_post.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\win32\rtti.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\precompile_header.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\warn_on.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\qt.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\resources.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\moc.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\file_copies.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\win32\windows.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\testcase_targets.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\exceptions.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\yacc.prf \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\lex.prf \
|
||||
data_process_class_dll\data_process_class_dll.pro \
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\lib\Qt5Core.prl
|
||||
$(QMAKE) -spec win32-msvc2013 -o Makefile data_process_class_dll\data_process_class_dll.pro
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\spec_pre.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\common\angle.conf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\common\msvc-base.conf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\common\msvc-desktop.conf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\qconfig.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dcore.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dcore_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dextras.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dextras_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dinput.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dinput_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dlogic.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dlogic_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquick.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquick_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickextras.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickextras_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickinput.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickinput_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickrender.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3dquickrender_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3drender.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_3drender_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axbase.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axbase_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axcontainer.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axcontainer_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axserver.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_axserver_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_bluetooth.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_bluetooth_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_bootstrap_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_charts.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_charts_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_clucene_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_concurrent.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_concurrent_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_core.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_core_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_datavisualization.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_datavisualization_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_dbus.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_dbus_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_designer.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_designer_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_designercomponents_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_gamepad.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_gamepad_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_gui.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_gui_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_help.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_help_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_location.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_location_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_multimedia.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_multimedia_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_multimediawidgets.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_multimediawidgets_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_network.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_network_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_nfc.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_nfc_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_opengl.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_opengl_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_openglextensions.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_openglextensions_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_packetprotocol_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_platformsupport_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_positioning.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_positioning_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_printsupport.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_printsupport_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_purchasing.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_purchasing_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qml.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qml_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qmldebug_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qmldevtools_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qmltest.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qmltest_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_qtmultimediaquicktools_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quick.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quick_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickcontrols2.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickcontrols2_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickparticles_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quicktemplates2_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickwidgets.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_quickwidgets_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_script.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_script_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_scripttools.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_scripttools_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_scxml.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_scxml_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_sensors.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_sensors_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_serialbus.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_serialbus_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_serialport.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_serialport_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_sql.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_sql_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_svg.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_svg_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_testlib.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_testlib_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_uiplugin.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_uitools.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_uitools_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webchannel.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webchannel_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webengine.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webengine_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginecore.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginecore_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginecoreheaders_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginewidgets.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webenginewidgets_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_websockets.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_websockets_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webview.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_webview_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_widgets.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_widgets_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_winextras.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_winextras_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_xml.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_xml_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_xmlpatterns.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_xmlpatterns_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\modules\qt_lib_zlib_private.pri:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\qt_functions.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\qt_config.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\win32\qt_config.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\win32-msvc2013\qmake.conf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\spec_post.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\exclusive_builds.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\default_pre.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\win32\default_pre.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\resolve_config.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\exclusive_builds_post.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\default_post.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\win32\rtti.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\precompile_header.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\warn_on.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\qt.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\resources.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\moc.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\file_copies.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\win32\windows.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\testcase_targets.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\exceptions.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\yacc.prf:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\mkspecs\features\lex.prf:
|
||||
data_process_class_dll\data_process_class_dll.pro:
|
||||
C:\Qt\Qt5.7.0\5.7\msvc2013\lib\Qt5Core.prl:
|
||||
qmake: FORCE
|
||||
@$(QMAKE) -spec win32-msvc2013 -o Makefile data_process_class_dll\data_process_class_dll.pro
|
||||
|
||||
qmake_all: FORCE
|
||||
|
||||
make_first: release-make_first debug-make_first FORCE
|
||||
all: release-all debug-all FORCE
|
||||
clean: release-clean debug-clean FORCE
|
||||
-$(DEL_FILE) data_process_class_dll.exp
|
||||
distclean: release-distclean debug-distclean FORCE
|
||||
-$(DEL_FILE) Makefile
|
||||
-$(DEL_FILE) data_process_class_dll.lib
|
||||
|
||||
release-mocclean:
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Release mocclean
|
||||
debug-mocclean:
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Debug mocclean
|
||||
mocclean: release-mocclean debug-mocclean
|
||||
|
||||
release-mocables:
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Release mocables
|
||||
debug-mocables:
|
||||
@set MAKEFLAGS=$(MAKEFLAGS)
|
||||
$(MAKE) -f $(MAKEFILE).Debug mocables
|
||||
mocables: release-mocables debug-mocables
|
||||
|
||||
check: first
|
||||
|
||||
benchmark: first
|
||||
FORCE:
|
||||
|
||||
$(MAKEFILE).Release: Makefile
|
||||
$(MAKEFILE).Debug: Makefile
|
||||
+2252
File diff suppressed because one or more lines are too long
+2252
File diff suppressed because one or more lines are too long
@@ -9,3 +9,25 @@
|
||||
|
||||
|
||||
|
||||
改进的情况：
|
||||
|
||||
1. 目标凝聚中考虑360°两侧点的凝聚；（已经改20260213）
|
||||
|
||||
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
||||
|
||||
2. 起航的门限增大8（已经改20260213）
|
||||
|
||||
3. 改了点迹凝聚的逻辑，每一包数据输入的时候滑窗凝聚（ 改了一下数据预处理函数的返回值程序才能执行，20260214）
|
||||
|
||||
4. 起航的时候要删除有重叠航迹（20260215 修改了，验证中）
|
||||
|
||||
|
||||
|
||||
20260228
|
||||
v1.5.1 雷达通信协议修改后的新程序、加了TAS逻辑，待测试
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -5,13 +5,13 @@
|
||||
using namespace std;
|
||||
|
||||
//极坐标转直角坐标
|
||||
void coor_trans::polar2cart(float *x, float *y, float Range, float Azimuth)
|
||||
void coor_trans::polar2cart(double *x, double *y, double Range, double Azimuth)
|
||||
{
|
||||
*x=Range*cos(Azimuth);
|
||||
*y=Range*sin(Azimuth);
|
||||
}
|
||||
|
||||
void coor_trans::cart2polar(float x, float y, float *Range, float *Azimuth)
|
||||
void coor_trans::cart2polar(double x, double y, double *Range, double *Azimuth)
|
||||
{
|
||||
*Range=sqrt(x*x+y*y);
|
||||
*Azimuth=atan2(y,x);
|
||||
|
||||
@@ -6,9 +6,9 @@ class coor_trans
|
||||
{
|
||||
|
||||
public:
|
||||
void polar2cart(float *x, float *y, float Range, float Azimuth);
|
||||
void polar2cart( double*x, double *y, double Range, double Azimuth);
|
||||
|
||||
void cart2polar(float x, float y, float *Range, float *Azimuth);
|
||||
void cart2polar(double x, double y, double *Range, double *Azimuth);
|
||||
|
||||
|
||||
};
|
||||
|
||||
@@ -1,7 +1,9 @@
|
||||
#include "data_process.h"
|
||||
#include "memory.h"
|
||||
#include <QVector>
|
||||
#include <qmath.h>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
/*******************************************************************************/
|
||||
/*******************************************************************************/
|
||||
@@ -13,20 +15,16 @@ using namespace std;
|
||||
//数据预处理
|
||||
int Data_Process::data_preprocess(struct DataRev Data_Input[150])
|
||||
{
|
||||
beam_count=beam_count+1; //3波束计数
|
||||
|
||||
//取波束1数据 存入缓存 暂不处理
|
||||
if(beam_count%3==1)
|
||||
//数据存入缓存区域 Data_buffer
|
||||
if(Data_Input[0].point_type == 0) //tws数据
|
||||
{
|
||||
beam_index[0]=Data_Input[0].Beam_index;
|
||||
data_num_1=Data_Input[0].Point_Sum;
|
||||
data_num=Data_Input[0].Point_Sum;
|
||||
TAS_track_idx = Data_Input[0].TAS_track_index;
|
||||
for (int i=0; i<data_num_1;i++)
|
||||
for (int i=0; i<data_num;i++)
|
||||
{
|
||||
PointRecv Data_buffer_temp;
|
||||
Data_buffer_temp.Amplitude=Data_Input[i].Amplitude;
|
||||
Data_buffer_temp.Azimuth=Data_Input[i].Azimuth/180*PI;
|
||||
Data_buffer_temp.beam_index = Data_Input[i].Beam_index;
|
||||
Data_buffer_temp.snr = Data_Input[i].Snr;
|
||||
Data_buffer_temp.RCS = Data_Input[i].RCS;
|
||||
Data_buffer_temp.CPI_Time = Data_Input[i].CPI_time;
|
||||
@@ -37,92 +35,70 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
|
||||
Data_buffer_temp.Use_Flag = 0;
|
||||
Data_buffer_temp.Velocity = Data_Input[i].Velocity;
|
||||
|
||||
Data_buffer.push_back(Data_buffer_temp);
|
||||
}
|
||||
Data_buffer_temp.pitch_num = Data_Input[i].pitch_num;
|
||||
|
||||
|
||||
memset(&Data_Input[0],0,150*sizeof(DataRev));
|
||||
return 0;
|
||||
}
|
||||
|
||||
//取波束2数据 存入缓存 暂不处理
|
||||
if(beam_count%3==2)
|
||||
{
|
||||
beam_index[1]=Data_Input[0].Beam_index;
|
||||
data_num_2=Data_Input[0].Point_Sum;
|
||||
TAS_track_idx = Data_Input[0].TAS_track_index;
|
||||
for (int i=0;i<data_num_2;i++)
|
||||
{
|
||||
PointRecv Data_buffer_temp;
|
||||
Data_buffer_temp.Amplitude=Data_Input[i].Amplitude;
|
||||
Data_buffer_temp.Azimuth=Data_Input[i].Azimuth/180*PI;
|
||||
Data_buffer_temp.beam_index = Data_Input[i].Beam_index;
|
||||
Data_buffer_temp.snr = Data_Input[i].Snr;
|
||||
Data_buffer_temp.RCS = Data_Input[i].RCS;
|
||||
Data_buffer_temp.CPI_Time = Data_Input[i].CPI_time;
|
||||
Data_buffer_temp.Freq_index = Data_Input[i].Freq_index;
|
||||
Data_buffer_temp.PRF_index = Data_Input[i].PRI;
|
||||
Data_buffer_temp.Range = Data_Input[i].Range;
|
||||
Data_buffer_temp.Height= Data_Input[i].Range*sin(Data_Input[i].Elevation/180*PI);
|
||||
Data_buffer_temp.Use_Flag = 0;
|
||||
Data_buffer_temp.Velocity = Data_Input[i].Velocity;
|
||||
std::memcpy(Data_buffer_temp.speed_dim, Data_Input[i].speed_dim, sizeof(Data_buffer_temp.speed_dim));
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
memset(&Data_Input[0],0,150*sizeof(DataRev));
|
||||
return 0;
|
||||
}
|
||||
|
||||
//取波束3数据 存入缓存 开始处理
|
||||
if(beam_count%3==0)
|
||||
{
|
||||
beam_index[2]=Data_Input[0].Beam_index;
|
||||
data_num_3=Data_Input[0].Point_Sum;
|
||||
TAS_track_idx = Data_Input[0].TAS_track_index;
|
||||
for (int i=0;i<data_num_3;i++)
|
||||
if(Data_Input[0].Beam_index_aiz == 16) //TWS
|
||||
{
|
||||
PointRecv Data_buffer_temp;
|
||||
Data_buffer_temp.Amplitude=Data_Input[i].Amplitude;
|
||||
Data_buffer_temp.Azimuth=Data_Input[i].Azimuth/180*PI;
|
||||
Data_buffer_temp.beam_index = Data_Input[i].Beam_index;
|
||||
Data_buffer_temp.snr = Data_Input[i].Snr;
|
||||
Data_buffer_temp.RCS = Data_Input[i].RCS;
|
||||
Data_buffer_temp.CPI_Time = Data_Input[i].CPI_time;
|
||||
Data_buffer_temp.Freq_index = Data_Input[i].Freq_index;
|
||||
Data_buffer_temp.PRF_index = Data_Input[i].PRI;
|
||||
Data_buffer_temp.Range = Data_Input[i].Range;
|
||||
Data_buffer_temp.Height= Data_Input[i].Range*sin(Data_Input[i].Elevation/180*PI);
|
||||
Data_buffer_temp.Use_Flag = 0;
|
||||
Data_buffer_temp.Velocity = Data_Input[i].Velocity;
|
||||
|
||||
Data_buffer.push_back(Data_buffer_temp);
|
||||
}
|
||||
|
||||
memset(&Data_Input[0],0,150*sizeof(DataRev));
|
||||
|
||||
if(beam_index[0]==600 || beam_index[1]==600 || beam_index[2]==600 )
|
||||
{
|
||||
return 2; //TAS
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 1; //TWS
|
||||
return 2;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
data_num=Data_Input[0].Point_Sum;
|
||||
TAS_track_idx = Data_Input[0].TAS_track_index;
|
||||
for (int i=0; i<data_num;i++)
|
||||
{
|
||||
PointRecv Data_buffer_temp;
|
||||
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;
|
||||
Data_buffer_temp.RCS = Data_Input[i].RCS;
|
||||
Data_buffer_temp.CPI_Time = Data_Input[i].CPI_time;
|
||||
Data_buffer_temp.Freq_index = Data_Input[i].Freq_index;
|
||||
Data_buffer_temp.PRF_index = Data_Input[i].PRI;
|
||||
Data_buffer_temp.Range = Data_Input[i].Range;
|
||||
Data_buffer_temp.Height= Data_Input[i].Range*sin(Data_Input[i].Elevation/180*PI);
|
||||
Data_buffer_temp.Use_Flag = 0;
|
||||
Data_buffer_temp.Velocity = Data_Input[i].Velocity;
|
||||
Data_buffer_temp.pitch_num = Data_Input[i].pitch_num;
|
||||
std::memcpy(Data_buffer_temp.speed_dim, Data_Input[i].speed_dim, sizeof(Data_buffer_temp.speed_dim));
|
||||
std::memcpy(Data_buffer_temp.range_dim, Data_Input[i].range_dim, sizeof(Data_buffer_temp.range_dim));
|
||||
|
||||
return 0;
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
return 3;
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//数据处理
|
||||
int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
@@ -130,247 +106,82 @@ int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][1
|
||||
int *Track_die_num_Output,
|
||||
int model)
|
||||
{
|
||||
|
||||
//TWS处理
|
||||
if(model==2)
|
||||
{
|
||||
|
||||
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,Work_Parameter);
|
||||
QVector<PointRecv>().swap(Data_buffer);
|
||||
}
|
||||
|
||||
|
||||
|
||||
if(model==1)
|
||||
{
|
||||
*Trust_track_num_Output=0; //航迹更新数
|
||||
*Track_die_num_Output=0; //航迹消亡数目
|
||||
|
||||
dot_coh.dot_coh_process(&Data_buffer,beam_index,&point_recv,Work_Parameter); //点迹凝聚
|
||||
track_asso.track_asso_process(&point_recv,
|
||||
&trust_track,
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);
|
||||
|
||||
QVector<PointRecv>().swap(Data_buffer);//凝聚完毕 清除Data_buffer
|
||||
track_die.track_die_process( &trust_track,
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
|
||||
if (beam_index[0] == DOT_SECTION_1+2 )
|
||||
{
|
||||
//波束1
|
||||
// 点迹区1、9 关联航迹区9 ， 消亡航迹区9， 起始点迹区8
|
||||
track_asso.track_asso_process(&point_recv[1-1],
|
||||
&point_recv[9-1],
|
||||
&trust_track[9-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);
|
||||
|
||||
track_die.track_die_process( &trust_track[9-1],
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
|
||||
track_init.track_init_process_logic( &point_recv[8-1],
|
||||
&trust_track,
|
||||
&temp_track[8-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
|
||||
//波束2
|
||||
// 点迹区3、4 关联航迹区3 ， 消亡航迹区3， 起始点迹区2
|
||||
track_asso.track_asso_process(&point_recv[3-1],
|
||||
&point_recv[4-1],
|
||||
&trust_track[3-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);//航迹关联
|
||||
|
||||
track_die.track_die_process( &trust_track[3-1],
|
||||
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);
|
||||
|
||||
|
||||
|
||||
track_init.track_init_process_logic( &point_recv[2-1],
|
||||
&trust_track,
|
||||
&temp_track[2-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
|
||||
//波束3
|
||||
// 点迹区6、7 关联航迹区6 ， 消亡航迹区6， 起始点迹区5
|
||||
track_asso.track_asso_process(&point_recv[6-1],
|
||||
&point_recv[7-1],
|
||||
&trust_track[6-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);//航迹关联
|
||||
|
||||
track_die.track_die_process( &trust_track[6-1],
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
|
||||
track_init.track_init_process_logic( &point_recv[5-1],
|
||||
&trust_track,
|
||||
&temp_track[5-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
|
||||
|
||||
Track_section_sort();
|
||||
}
|
||||
if (beam_index[0] == DOT_SECTION_2+2 )
|
||||
{
|
||||
//波束1
|
||||
// 点迹区1、2 关联航迹区1 ， 消亡航迹区1， 起始点迹区9
|
||||
track_asso.track_asso_process(&point_recv[1-1],
|
||||
&point_recv[2-1],
|
||||
&trust_track[1-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);
|
||||
|
||||
track_die.track_die_process( &trust_track[1-1],
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
|
||||
track_init.track_init_process_logic( &point_recv[9-1],
|
||||
&trust_track,
|
||||
&temp_track[9-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
|
||||
//波束2
|
||||
// 点迹区4、5 关联航迹区4 ， 消亡航迹区4， 起始点迹区3
|
||||
|
||||
|
||||
track_asso.track_asso_process(&point_recv[4-1],
|
||||
&point_recv[5-1],
|
||||
&trust_track[4-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);
|
||||
|
||||
track_die.track_die_process( &trust_track[4-1],
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
|
||||
|
||||
track_init.track_init_process_logic( &point_recv[3-1],
|
||||
&trust_track,
|
||||
&temp_track[3-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
//波束3
|
||||
// 点迹区7、8 关联航迹区7 ， 消亡航迹区7， 起始点迹区6
|
||||
track_asso.track_asso_process(&point_recv[7-1],
|
||||
&point_recv[8-1],
|
||||
&trust_track[7-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);
|
||||
|
||||
track_die.track_die_process( &trust_track[7-1],
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
|
||||
track_init.track_init_process_logic( &point_recv[6-1],
|
||||
&trust_track,
|
||||
&temp_track[6-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
Track_section_sort();
|
||||
}
|
||||
if (beam_index[0] == 1 )
|
||||
{
|
||||
//波束1
|
||||
// 点迹区2、3 关联航迹区2 ， 消亡航迹区2， 起始点迹区1
|
||||
track_asso.track_asso_process(&point_recv[2-1],
|
||||
&point_recv[3-1],
|
||||
&trust_track[2-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);
|
||||
|
||||
track_die.track_die_process( &trust_track[2-1],
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
|
||||
|
||||
track_init.track_init_process_logic( &point_recv[1-1],
|
||||
&trust_track,
|
||||
&temp_track[1-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
|
||||
|
||||
|
||||
//波束2
|
||||
// 点迹区5、6 关联航迹区5， 消亡航迹区5， 起始点迹区4
|
||||
track_asso.track_asso_process(&point_recv[5-1],
|
||||
&point_recv[6-1],
|
||||
&trust_track[5-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);
|
||||
|
||||
track_die.track_die_process( &trust_track[5-1],
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
|
||||
track_init.track_init_process_logic( &point_recv[4-1],
|
||||
&trust_track,
|
||||
&temp_track[4-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
|
||||
//波束3
|
||||
// 点迹区8、9 关联航迹区8 ， 消亡航迹区8， 起始点迹区7
|
||||
track_asso.track_asso_process(&point_recv[8-1],
|
||||
&point_recv[9-1],
|
||||
&trust_track[8-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter
|
||||
);
|
||||
|
||||
track_die.track_die_process( &trust_track[8-1],
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
|
||||
track_init.track_init_process_logic( &point_recv[7-1],
|
||||
&trust_track,
|
||||
&temp_track[7-1],
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
Track_section_sort();
|
||||
}
|
||||
}
|
||||
|
||||
//TAS处理
|
||||
else if (model==2)
|
||||
// TAS处理
|
||||
if(model==3)
|
||||
{
|
||||
*Trust_track_num_Output=0; //航迹更新数
|
||||
*Track_die_num_Output=0; //航迹消亡数目
|
||||
dot_coh_tas.dot_coh_tas_process(&Data_buffer,&point_recv_tas); //凝聚
|
||||
QVector<PointRecv>().swap(Data_buffer);//凝聚完毕 清除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);
|
||||
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;
|
||||
// }
|
||||
|
||||
|
||||
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);
|
||||
|
||||
|
||||
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);
|
||||
|
||||
|
||||
QVector<PointRecv>().swap(point_recv_tas);//关联完毕 清空point_recv_tas
|
||||
|
||||
Track_section_sort();
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
@@ -380,15 +191,8 @@ void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output)
|
||||
{
|
||||
if(beam_count%3==0)
|
||||
{
|
||||
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
|
||||
}
|
||||
else
|
||||
{
|
||||
Tracking_beam->open_flag=0;
|
||||
}
|
||||
|
||||
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
|
||||
}
|
||||
|
||||
|
||||
@@ -397,31 +201,18 @@ void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam,
|
||||
//数据处理参数初始化
|
||||
int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parameter)
|
||||
{
|
||||
//变量初始化
|
||||
Beam_num=Radar_Parameter.Beam_num; //雷达波位数
|
||||
beam_count=0; //波位计数
|
||||
memset(beam_index,0,3*sizeof(int)); //三个波位号
|
||||
|
||||
|
||||
//工作参数设置
|
||||
memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara));
|
||||
|
||||
|
||||
//初始化容器
|
||||
for (int i=0;i<DOT_SECTION_NUM;i++)
|
||||
point_recv.push_back(QVector <PointRecv> ());
|
||||
|
||||
for (int i=0;i<TRACK_SECTION_NUM;i++)
|
||||
trust_track.push_back(QVector <Trust_Track> ());
|
||||
|
||||
for (int i=0;i<DOT_SECTION_NUM;i++)
|
||||
temp_track.push_back(QVector<QVector<Temp_track>> () );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//数据处理参数修改
|
||||
int Data_Process::track_process_parameters_modify(struct RadarPara Radar_Parameter)
|
||||
{
|
||||
@@ -430,6 +221,14 @@ int Data_Process::track_process_parameters_modify(struct RadarPara Radar_Paramet
|
||||
}
|
||||
|
||||
|
||||
//航迹清空函数
|
||||
int Data_Process::track_clear_all(void)
|
||||
{
|
||||
//清空可靠航迹
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
//手动航迹删除函数
|
||||
int Data_Process:: track_delete(int delete_track_num, //手动删除的航迹数目
|
||||
@@ -438,14 +237,14 @@ int Data_Process:: track_delete(int delete_track_num, //手动删除的航
|
||||
for (int i=0;i<delete_track_num ;i++)
|
||||
{
|
||||
for (int j=0;j<trust_track.size();j++)
|
||||
for(int k=0;k<trust_track[j].size();k++)
|
||||
{
|
||||
if(trust_track[j].Track_Index == delete_track_index[i])
|
||||
{
|
||||
if(trust_track[j][k].Track_Index == delete_track_index[i])
|
||||
{
|
||||
|
||||
trust_track[j][k].manual_delete_flag=1;
|
||||
}
|
||||
trust_track[j].manual_delete_flag=1;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -457,14 +256,12 @@ int Data_Process:: tracking_start(int track_index) //需要手动转入TAS跟
|
||||
{
|
||||
|
||||
for ( int i=0; i<trust_track.size();i++)
|
||||
for (int j=0;j<trust_track[i].size();j++)
|
||||
if(trust_track[i].Track_Index==track_index)
|
||||
{
|
||||
if(trust_track[i][j].Track_Index==track_index)
|
||||
{
|
||||
trust_track[i][j].manual_tracking_flag = 1;
|
||||
}
|
||||
trust_track[i].manual_tracking_flag = 1;
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -476,21 +273,5 @@ int Data_Process::tracking_point(float Azimuth)//方位角
|
||||
|
||||
|
||||
|
||||
void Data_Process::Track_section_sort()
|
||||
{
|
||||
for(int i=0;i<trust_track.size();i++)
|
||||
for(int j=0;j<trust_track[i].size(); )
|
||||
{
|
||||
if (trust_track[i][j].Track_section_idx!=i+1)
|
||||
{
|
||||
trust_track[trust_track[i][j].Track_section_idx-1].push_back(trust_track[i][j]);
|
||||
trust_track[i].erase(trust_track[i].begin()+j);
|
||||
}
|
||||
else
|
||||
{
|
||||
j++;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -10,9 +10,11 @@
|
||||
#include "track_asso.h"
|
||||
#include "track_asso_tas.h"
|
||||
#include "track_die.h"
|
||||
#include "track_die_tas.h"
|
||||
#include "tas_ctrl.h"
|
||||
#include <QVector>
|
||||
#include <Eigen/Dense>
|
||||
#include <QDebug>
|
||||
using namespace Eigen;
|
||||
using namespace std;
|
||||
|
||||
@@ -44,6 +46,9 @@ public:
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output);
|
||||
|
||||
//航迹清空函数
|
||||
int track_clear_all(void);
|
||||
|
||||
|
||||
//手动航迹删除函数
|
||||
int track_delete(int delete_track_num, //手动删除的航迹数目
|
||||
@@ -64,18 +69,19 @@ public:
|
||||
|
||||
private:
|
||||
|
||||
QVector <PointRecv> Data_buffer; //接收点迹 输入凝聚
|
||||
QVector <QVector<PointRecv>> point_recv; //凝聚后输出的点迹 按点迹区存入 输入航迹关联、起始
|
||||
QVector <PointRecv> point_recv_tas;
|
||||
QVector <QVector<Trust_Track>> trust_track; //可靠航迹 按航迹区存入
|
||||
QVector <QVector<QVector<Temp_track>>> temp_track; //临时航迹 按临时航迹区存入
|
||||
QVector <PointRecv> Data_buffer; //接收点迹 TWS 输入凝聚
|
||||
QVector <PointRecv> Data_buffer_tas; //接收点迹 TAS 输入凝聚
|
||||
|
||||
QVector <PointRecv> point_recv; //凝聚后输出的点迹 按点迹区存入 输入航迹关联、起始
|
||||
QVector <PointRecv> point_recv_tas; //凝聚后输出的点迹 TAS
|
||||
QVector <Trust_Track> trust_track; //可靠航迹 按航迹区存入
|
||||
QVector <QVector<Temp_track>> temp_track; //临时航迹 按临时航迹区存入
|
||||
|
||||
|
||||
int Beam_num; //波位数
|
||||
int beam_count; //波位计数
|
||||
int data_num_1; //波位1点数
|
||||
int data_num_2; //波位2点数
|
||||
int data_num_3; //波位3点数
|
||||
int data_num; //波位1点数
|
||||
|
||||
int beam_index[3]; //3个波位号(当前帧)
|
||||
|
||||
int TAS_track_idx; //TAS数据的目标批号
|
||||
@@ -97,9 +103,9 @@ private:
|
||||
|
||||
Track_Die track_die; //航迹消亡
|
||||
|
||||
TAS_Ctrl tas_ctrl; //TAS波束控制
|
||||
Track_Die_Tas track_die_tas;
|
||||
|
||||
void Track_section_sort(); //航迹区整理
|
||||
TAS_Ctrl tas_ctrl; //TAS波束控制
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -23,6 +23,11 @@ Data_process_class_dll *Data_Process_Factory::GetB()
|
||||
|
||||
void Data_Process_Factory::Destroy()
|
||||
{
|
||||
if (p)
|
||||
// if (p)
|
||||
// delete p;
|
||||
|
||||
if (p) {
|
||||
delete p;
|
||||
p = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -17,13 +17,19 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT DataRev
|
||||
float Snr; //目标信噪比
|
||||
float RCS; //目标RCS
|
||||
int CPI_time; //CPI时间 该波位无点迹输入时也需要给出当前时间
|
||||
int Beam_index; //波位号 该波位无输入点迹时 也需要给出当前波位
|
||||
int PRI; //PRI(us)
|
||||
int Freq_index; //频点号
|
||||
int Beam_index_azi_0; //上一个cpi波位
|
||||
int Beam_index_aiz; //方位波位号（0-24） 该波位无输入点迹时 也需要给出当前波位
|
||||
int Beam_index_elev; //俯仰波位号（0-13）
|
||||
int PRI; //PRI(us) 给0
|
||||
int Freq_index; //频点号 给0
|
||||
int Point_Sum; //输入点迹总数 若该波位无点迹输入 置为0
|
||||
int Point_Num; //点迹号(1,2,3,4...)
|
||||
int UseFlag; //点迹使用标记 使用1/未使用0 主程序存入数据时置为0
|
||||
int TAS_track_index; //TAS的航迹号
|
||||
int point_type; //点迹类型 TAS 1/TWS 0
|
||||
int pitch_num; // 俯仰波位号
|
||||
float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点
|
||||
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
|
||||
};
|
||||
|
||||
|
||||
@@ -66,6 +72,10 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Track
|
||||
float track_time; //航迹时间 (单位 s)
|
||||
float track_snr; //信噪比
|
||||
float track_rcs; //rcs
|
||||
|
||||
int pitch_num; // 俯仰波位号
|
||||
float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点
|
||||
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
|
||||
};
|
||||
|
||||
|
||||
@@ -83,6 +93,18 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT TrackingBeam
|
||||
int TAS_track_index; //目标批号
|
||||
};
|
||||
|
||||
//引导跟踪信息的结构体
|
||||
struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Target_direct_tracking
|
||||
{
|
||||
int Track_ID; //引导目标批号
|
||||
float Range; //引导目标距离
|
||||
float Azi; //引导目标方位
|
||||
float Elev; //引导目标俯仰
|
||||
int open_flag; // 1 引导跟踪 0 结束引导跟踪
|
||||
};
|
||||
|
||||
|
||||
|
||||
// 雷达参数结构体
|
||||
struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
|
||||
{
|
||||
@@ -122,6 +144,13 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
|
||||
int work_mode; //工作模式 0进程 1中程 3远程
|
||||
int north_angle; //北偏角
|
||||
|
||||
float V_MAX; //最大速度
|
||||
float V_MIN; //最小速度
|
||||
|
||||
float DATA_RATE_SHORT; //三种不同模式下的 数据率
|
||||
float DATA_RATE_MIDDLE;
|
||||
float DATA_RATE_FAR;
|
||||
|
||||
|
||||
//数据关联参数
|
||||
int track_start_point_num; //起批点数(典型值 3或4)
|
||||
@@ -175,6 +204,28 @@ public:
|
||||
|
||||
}
|
||||
|
||||
//引导跟踪处理函数
|
||||
virtual int direct_tracking_process(struct Target_direct_tracking Target_direct_info, //引导信息
|
||||
struct DataRev Data_Input[150], //雷达的输入点迹
|
||||
struct Track Trust_Track_Output[][10], //数据处理输出的航迹更新信息 主程序创建全局变量 数据处理函数更新其中数据
|
||||
int *Trust_track_num_Output, //数据处理后输出的航迹数目 主程序创建全局变量 数据处理函数更新其中数据
|
||||
int Track_die_Index_Output[], //数据处理后要消亡的航迹 主程序创建全局变量 数据处理函数更新其中数据
|
||||
int *Track_die_num_Output, //数据处理后要消亡的航迹数目
|
||||
struct TrackingBeam *Tracking_beam //波束控制信息
|
||||
)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
//航迹清空函数
|
||||
virtual int track_clear_all(void)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//手动航迹删除函数
|
||||
virtual int track_delete(int delete_track_num, //手动删除的航迹数目
|
||||
|
||||
@@ -21,7 +21,9 @@ SOURCES += data_process_class_dll.cpp \
|
||||
tas_ctrl.cpp \
|
||||
track_die.cpp \
|
||||
dot_coh_tas.cpp \
|
||||
track_asso_tas.cpp
|
||||
track_asso_tas.cpp \
|
||||
track_init_direct_tracking.cpp \
|
||||
track_die_tas.cpp
|
||||
|
||||
HEADERS += data_process_class_dll.h\
|
||||
data_process_class_dll_global.h \
|
||||
@@ -37,7 +39,9 @@ HEADERS += data_process_class_dll.h\
|
||||
tas_ctrl.h \
|
||||
track_die.h \
|
||||
dot_coh_tas.h \
|
||||
track_asso_tas.h
|
||||
track_asso_tas.h \
|
||||
track_init_direct_tracking.h \
|
||||
track_die_tas.h
|
||||
|
||||
unix {
|
||||
target.path = /usr/lib
|
||||
|
||||
@@ -1,10 +1,10 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 4.0.2, 2022-06-15T20:03:24. -->
|
||||
<!-- Written by QtCreator 4.0.2, 2026-06-17T16:00:38. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{0b80abbd-012d-4338-8c64-507c85ad099b}</value>
|
||||
<value type="QByteArray">{bda90df5-9664-4f48-ab0c-f665f06226f6}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
@@ -54,9 +54,7 @@
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuelist type="QVariantList" key="ClangStaticAnalyzer.SuppressedDiagnostics"/>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap"/>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
@@ -122,7 +120,7 @@
|
||||
<value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">D:/hhyk_data_process/2003/build-data_process_class_dll-Desktop_Qt_5_7_0_MSVC2013_32bit-Release</value>
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">E:/Demo/QT/simianzhen256-v1.5.5/release</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
|
||||
@@ -0,0 +1,317 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 4.0.2, 2025-04-27T16:03:48. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{0b80abbd-012d-4338-8c64-507c85ad099b}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="int">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuelist type="QVariantList" key="ClangStaticAnalyzer.SuppressedDiagnostics"/>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop Qt 5.7.0 MSVC2013 32bit</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop Qt 5.7.0 MSVC2013 32bit</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">qt.57.win32_msvc2013_kit</value>
|
||||
<value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">1</value>
|
||||
<value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">E:/data_process_qt_vision_2003/build-data_process_class_dll-Desktop_Qt_5_7_0_MSVC2013_32bit-Debug</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
|
||||
<value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
|
||||
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
|
||||
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">构建</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
|
||||
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
|
||||
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">清理</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Debug</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
|
||||
<value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">2</value>
|
||||
<value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">D:/007_hhyk_data_process/data_process_mianzhen576/mianzhen576-v1.4.2/build-data_process_class_dll-Desktop_Qt_5_7_0_MSVC2013_32bit-Release</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
|
||||
<value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
|
||||
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
|
||||
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">构建</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
|
||||
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
|
||||
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">清理</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
|
||||
<value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
|
||||
<value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.2">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">E:/data_process_qt_vision_2003/build-data_process_class_dll-Desktop_Qt_5_7_0_MSVC2013_32bit-Profile</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
|
||||
<value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">true</value>
|
||||
<value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
|
||||
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
|
||||
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">构建</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
|
||||
<valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments"/>
|
||||
<value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
|
||||
<value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">清理</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Profile</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
|
||||
<value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
|
||||
<value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">3</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">部署</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">在本地部署</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
|
||||
<value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
|
||||
<value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
|
||||
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
|
||||
<value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
|
||||
<value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
|
||||
<value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
|
||||
<valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
|
||||
<value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
|
||||
<value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
|
||||
<valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
|
||||
<value type="int">0</value>
|
||||
<value type="int">1</value>
|
||||
<value type="int">2</value>
|
||||
<value type="int">3</value>
|
||||
<value type="int">4</value>
|
||||
<value type="int">5</value>
|
||||
<value type="int">6</value>
|
||||
<value type="int">7</value>
|
||||
<value type="int">8</value>
|
||||
<value type="int">9</value>
|
||||
<value type="int">10</value>
|
||||
<value type="int">11</value>
|
||||
<value type="int">12</value>
|
||||
<value type="int">13</value>
|
||||
<value type="int">14</value>
|
||||
</valuelist>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Arguments"></value>
|
||||
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Executable"></value>
|
||||
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.WorkingDirectory">%{buildDir}</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">自定义执行档</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="int">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">18</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">18</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
@@ -6,66 +6,29 @@ using namespace std;
|
||||
|
||||
|
||||
int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
|
||||
int beam_input[3],
|
||||
QVector <QVector<PointRecv>> *point_recv,
|
||||
QVector <PointRecv> *point_recv,
|
||||
struct RadarPara Work_Parameter)
|
||||
{
|
||||
|
||||
//数据移动一位
|
||||
for (int i=0;i<data_buffer_2.size();i++) //data_buffer_2 ---> point_recv
|
||||
{
|
||||
(*point_recv)[floor(data_buffer_2[i].beam_index/BEAM_NUM_DOT_SECTION)].push_back(data_buffer_2[i]);
|
||||
}
|
||||
QVector<PointRecv>().swap(data_buffer_2);
|
||||
|
||||
|
||||
|
||||
for (int i=0;i<data_buffer_1.size();i++) //data_buffer_1 ---> data_buffer_2
|
||||
{
|
||||
data_buffer_2.push_back(data_buffer_1[i]);
|
||||
}
|
||||
QVector<PointRecv>().swap(data_buffer_1);
|
||||
|
||||
|
||||
|
||||
for (int i=0;i<data_input->size();i++) //data_input ---> data_buffer_1
|
||||
{
|
||||
data_buffer_1.push_back((*data_input)[i]);
|
||||
}
|
||||
|
||||
|
||||
//凝聚data_buffer_1 data_buffer_2中的数据
|
||||
QVector <PointRecv> data_buffer;
|
||||
|
||||
for (int i=0;i<data_buffer_1.size();i++)
|
||||
data_buffer.push_back(data_buffer_1[i]);
|
||||
|
||||
for (int i=0;i<data_buffer_2.size();i++)
|
||||
data_buffer.push_back(data_buffer_2[i]);
|
||||
|
||||
QVector<PointRecv>().swap(data_buffer_1);
|
||||
QVector<PointRecv>().swap(data_buffer_2);
|
||||
|
||||
|
||||
if(data_buffer.size()>1)
|
||||
if(data_input->size()>1)
|
||||
{
|
||||
|
||||
for (unsigned int loop_of_point=0; loop_of_point<data_buffer.size()-1;loop_of_point++ )
|
||||
for (unsigned int loop_of_point=0; loop_of_point<data_input->size()-1;loop_of_point++ )
|
||||
{
|
||||
if( data_buffer[loop_of_point].Use_Flag!=1)
|
||||
if( (*data_input)[loop_of_point].Use_Flag!=1)
|
||||
{
|
||||
float point_0_R=data_buffer[loop_of_point].Range;
|
||||
float point_0_V=data_buffer[loop_of_point].Velocity;
|
||||
float point_0_F=data_buffer[loop_of_point].Azimuth;
|
||||
float point_0_A=data_buffer[loop_of_point].Amplitude;
|
||||
for (unsigned int i=loop_of_point+1;i<data_buffer.size();i++)
|
||||
float point_0_R=(*data_input)[loop_of_point].Range;
|
||||
float point_0_V=(*data_input)[loop_of_point].Velocity;
|
||||
float point_0_F=(*data_input)[loop_of_point].Azimuth;
|
||||
float point_0_A=(*data_input)[loop_of_point].Amplitude;
|
||||
for (unsigned int i=loop_of_point+1;i<data_input->size();i++)
|
||||
{
|
||||
if(data_buffer[loop_of_point].Use_Flag!=1)
|
||||
if((*data_input)[loop_of_point].Use_Flag!=1)
|
||||
{
|
||||
float point_1_R=data_buffer[i].Range;
|
||||
float point_1_V=data_buffer[i].Velocity;
|
||||
float point_1_F=data_buffer[i].Azimuth;
|
||||
float point_1_A=data_buffer[i].Amplitude;
|
||||
float point_1_R=(*data_input)[i].Range;
|
||||
float point_1_V=(*data_input)[i].Velocity;
|
||||
float point_1_F=(*data_input)[i].Azimuth;
|
||||
float point_1_A=(*data_input)[i].Amplitude;
|
||||
|
||||
//凝聚条件： 距离、方位接近
|
||||
if(Work_Parameter.work_mode == 0)
|
||||
@@ -73,33 +36,34 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
||||
&& point_0_A <point_1_A )//
|
||||
{
|
||||
point_0_R=data_buffer[i].Range;
|
||||
point_0_V=data_buffer[i].Velocity;
|
||||
point_0_A=data_buffer[i].Amplitude;
|
||||
point_0_F=data_buffer[i].Azimuth;
|
||||
data_buffer[loop_of_point].Use_Flag=1;
|
||||
point_0_R=(*data_input)[i].Range;
|
||||
point_0_V=(*data_input)[i].Velocity;
|
||||
point_0_A=(*data_input)[i].Amplitude;
|
||||
point_0_F=(*data_input)[i].Azimuth;
|
||||
(*data_input)[loop_of_point].Use_Flag=1;
|
||||
}
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
||||
&& point_0_A >= point_1_A)//
|
||||
{
|
||||
data_buffer[i].Use_Flag=1;
|
||||
(*data_input)[i].Use_Flag=1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI )//
|
||||
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
||||
&& point_0_A <point_1_A )//
|
||||
{
|
||||
point_0_R=data_buffer[i].Range;
|
||||
point_0_V=data_buffer[i].Velocity;
|
||||
point_0_A=data_buffer[i].Amplitude;
|
||||
point_0_F=data_buffer[i].Azimuth;
|
||||
data_buffer[loop_of_point].Use_Flag=1;
|
||||
point_0_R=(*data_input)[i].Range;
|
||||
point_0_V=(*data_input)[i].Velocity;
|
||||
point_0_A=(*data_input)[i].Amplitude;
|
||||
point_0_F=(*data_input)[i].Azimuth;
|
||||
(*data_input)[loop_of_point].Use_Flag=1;
|
||||
}
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI )//
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
||||
&& point_0_A >= point_1_A)//
|
||||
{
|
||||
data_buffer[i].Use_Flag=1;
|
||||
(*data_input)[i].Use_Flag=1;
|
||||
}
|
||||
|
||||
|
||||
@@ -111,15 +75,14 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//删除凝聚点 和 量程范围外点
|
||||
QVector <PointRecv>::iterator Iter;
|
||||
for (Iter=data_buffer.begin(); Iter!=data_buffer.end();)
|
||||
for (Iter=data_input->begin(); Iter!=data_input->end();)
|
||||
{
|
||||
if((*Iter).Use_Flag==1 || (*Iter).Range <R_MIN || (*Iter).Range > R_MAX )
|
||||
{
|
||||
data_buffer.erase(Iter);
|
||||
Iter=data_buffer.begin();
|
||||
data_input->erase(Iter);
|
||||
Iter=data_input->begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -127,15 +90,9 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
|
||||
}
|
||||
}
|
||||
|
||||
//凝聚后的数据分别存入data_buffer_1 data_buffer_2
|
||||
for(int i=0;i<data_buffer.size();i++)
|
||||
for (int i=0;i<data_input->size();i++) //data_buffer_2 ---> point_recv
|
||||
{
|
||||
if(data_buffer[i].beam_index == beam_input[0]
|
||||
|| data_buffer[i].beam_index == beam_input[1]
|
||||
|| data_buffer[i].beam_index == beam_input[2])
|
||||
data_buffer_1.push_back(data_buffer[i]);
|
||||
else
|
||||
data_buffer_2.push_back(data_buffer[i]);
|
||||
(*point_recv).push_back((*data_input)[i]);
|
||||
}
|
||||
|
||||
return 0;
|
||||
@@ -143,6 +100,160 @@ int Dot_Coh::dot_coh_process(QVector <PointRecv> *data_input,
|
||||
|
||||
|
||||
|
||||
|
||||
int Dot_Coh::dot_coh_process_buff( QVector <PointRecv> *data_input, //输入的点迹
|
||||
QVector <PointRecv> *point_recv, //输出点迹
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
)
|
||||
{
|
||||
|
||||
|
||||
|
||||
//1. data_input与data_input_buff进行凝聚
|
||||
|
||||
//1.1 data_input、data_input_buff中的数据放在一起
|
||||
QVector <PointRecv> data_tmp;
|
||||
for (int i=0;i<data_input_buff.size();i++)
|
||||
{
|
||||
data_tmp.push_back(data_input_buff[i]);
|
||||
data_tmp[data_tmp.size()-1].point_section_asso = 1;
|
||||
|
||||
}
|
||||
for (int i=0;i<data_input->size();i++)
|
||||
{
|
||||
data_tmp.push_back((*data_input)[i]);
|
||||
data_tmp[data_tmp.size()-1].point_section_asso = 2;
|
||||
}
|
||||
|
||||
//1.2 把data_input、data_input_buff清空
|
||||
|
||||
QVector<PointRecv>().swap(data_input_buff);
|
||||
QVector<PointRecv>().swap(*data_input);
|
||||
|
||||
|
||||
//1.3 对data_tmp进行凝聚
|
||||
if(data_tmp.size()>1)
|
||||
{
|
||||
|
||||
for (unsigned int loop_of_point=0; loop_of_point<data_tmp.size()-1;loop_of_point++ )
|
||||
{
|
||||
if( data_tmp[loop_of_point].Use_Flag!=1)
|
||||
{
|
||||
float point_0_R=data_tmp[loop_of_point].Range;
|
||||
float point_0_V=data_tmp[loop_of_point].Velocity;
|
||||
float point_0_F=data_tmp[loop_of_point].Azimuth;
|
||||
float point_0_A=data_tmp[loop_of_point].Amplitude;
|
||||
for (unsigned int i=loop_of_point+1;i<data_tmp.size();i++)
|
||||
{
|
||||
if(data_tmp[loop_of_point].Use_Flag!=1)
|
||||
{
|
||||
float point_1_R=data_tmp[i].Range;
|
||||
float point_1_V=data_tmp[i].Velocity;
|
||||
float point_1_F=data_tmp[i].Azimuth;
|
||||
float point_1_A=data_tmp[i].Amplitude;
|
||||
|
||||
//凝聚条件： 距离、方位接近
|
||||
if(Work_Parameter.work_mode == 0)
|
||||
{
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
||||
&& point_0_A <point_1_A )//
|
||||
{
|
||||
point_0_R=data_tmp[i].Range;
|
||||
point_0_V=data_tmp[i].Velocity;
|
||||
point_0_A=data_tmp[i].Amplitude;
|
||||
point_0_F=data_tmp[i].Azimuth;
|
||||
data_tmp[loop_of_point].Use_Flag=1;
|
||||
}
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
||||
&& point_0_A >= point_1_A)//
|
||||
{
|
||||
data_tmp[i].Use_Flag=1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
||||
&& point_0_A <point_1_A )//
|
||||
{
|
||||
point_0_R=data_tmp[i].Range;
|
||||
point_0_V=data_tmp[i].Velocity;
|
||||
point_0_A=data_tmp[i].Amplitude;
|
||||
point_0_F=data_tmp[i].Azimuth;
|
||||
data_tmp[loop_of_point].Use_Flag=1;
|
||||
}
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
||||
&& point_0_A >= point_1_A)//
|
||||
{
|
||||
data_tmp[i].Use_Flag=1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//1.4 删除凝聚点 和 量程范围外点
|
||||
QVector <PointRecv>::iterator Iter;
|
||||
for (Iter=data_tmp.begin(); Iter!=data_tmp.end();)
|
||||
{
|
||||
if((*Iter).Use_Flag==1 || (*Iter).Range <R_MIN || (*Iter).Range > R_MAX )
|
||||
{
|
||||
data_tmp.erase(Iter);
|
||||
Iter=data_tmp.begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter++;
|
||||
}
|
||||
}
|
||||
|
||||
//1.5 将data_tmp中凝聚后的点再分到 data_input_buff和data_input中
|
||||
for (int i=0;i<data_tmp.size();i++)
|
||||
{
|
||||
if(data_tmp[i].point_section_asso==1)
|
||||
{
|
||||
data_input_buff.push_back(data_tmp[i]);
|
||||
}
|
||||
|
||||
if(data_tmp[i].point_section_asso==2)
|
||||
{
|
||||
(*data_input).push_back(data_tmp[i]);
|
||||
}
|
||||
}
|
||||
QVector<PointRecv>().swap(data_tmp);
|
||||
|
||||
|
||||
|
||||
//2.data_input_buff数据输出给point_recv
|
||||
|
||||
for (int i=0;i<data_input_buff.size();i++)
|
||||
{
|
||||
(*point_recv).push_back(data_input_buff[i]);
|
||||
}
|
||||
|
||||
QVector<PointRecv>().swap(data_input_buff);
|
||||
|
||||
|
||||
//3.data_input数据输出给data_input_buff
|
||||
for (int i=0;i<data_input->size();i++)
|
||||
{
|
||||
data_input_buff.push_back((*data_input)[i]);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
Dot_Coh::Dot_Coh(void)
|
||||
{
|
||||
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
#include "parameters.h"
|
||||
#include "struct.h"
|
||||
#include <QVector>
|
||||
|
||||
#include <QDebug>
|
||||
|
||||
using namespace std;
|
||||
|
||||
@@ -15,21 +15,28 @@ public:
|
||||
|
||||
// 点迹凝聚函数
|
||||
int dot_coh_process( QVector <PointRecv> *data_input, //输入的点迹
|
||||
int beam_input[3], //输入点迹的波位号
|
||||
QVector <QVector<PointRecv>> *point_recv, //输出点迹
|
||||
QVector <PointRecv> *point_recv, //输出点迹
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
);
|
||||
|
||||
//点迹凝聚函数，缓存一帧，一边输出，一边进行滑窗凝聚
|
||||
int dot_coh_process_buff( QVector <PointRecv> *data_input, //输入的点迹
|
||||
QVector <PointRecv> *point_recv, //输出点迹
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
);
|
||||
|
||||
|
||||
|
||||
|
||||
//构造函数
|
||||
Dot_Coh();
|
||||
|
||||
|
||||
|
||||
|
||||
private:
|
||||
QVector <PointRecv> data_input_buff;
|
||||
|
||||
QVector <PointRecv> data_buffer_1; //数据缓存
|
||||
|
||||
QVector <PointRecv> data_buffer_2;
|
||||
|
||||
QVector <PointRecv> data_buffer_3;
|
||||
|
||||
};
|
||||
|
||||
|
||||
+203
-203
@@ -11,7 +11,7 @@ using namespace std;
|
||||
|
||||
|
||||
|
||||
void kalman:: kalman_filter_init_2dots(float Z0[2], float Z1[2], float T,float X[4], float P[4][4])
|
||||
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4])
|
||||
{
|
||||
|
||||
X[0]=Z1[0];
|
||||
@@ -19,13 +19,13 @@ void kalman:: kalman_filter_init_2dots(float Z0[2], float Z1[2], float T,float X
|
||||
X[2]=Z1[1];
|
||||
X[3]=(Z1[1]-Z0[1])/T;
|
||||
|
||||
float rho,theta;
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z1[0],Z1[1],&rho,&theta);
|
||||
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
|
||||
float R[2][2];
|
||||
double R[2][2];
|
||||
R[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
@@ -56,68 +56,68 @@ void kalman:: kalman_filter_init_2dots(float Z0[2], float Z1[2], float T,float X
|
||||
}
|
||||
|
||||
|
||||
float kalman::d_cal_track_init(float Z[2],float X[4],float P[4][4],float T)
|
||||
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
|
||||
{
|
||||
|
||||
//X(k+1|k)
|
||||
Matrix4f F;
|
||||
Matrix4d F;
|
||||
F<< 1, T, 0, 0,
|
||||
0, 1, 0, 0,
|
||||
0, 0, 1, T,
|
||||
0, 0, 0, 1;
|
||||
Vector4f X_present = Vector4f(X[0],X[1],X[2],X[3]);
|
||||
Vector4f X_pred;
|
||||
Vector4d X_present = Vector4d(X[0],X[1],X[2],X[3]);
|
||||
Vector4d X_pred;
|
||||
X_pred=F*X_present;
|
||||
|
||||
//Z(k+1|k)
|
||||
MatrixXf H(2,4);
|
||||
MatrixXd H(2,4);
|
||||
H<< 1,0,0,0,
|
||||
0,0,1,0;
|
||||
|
||||
Vector2f Z_pred;
|
||||
Vector2d Z_pred;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
//P(k+1|k)
|
||||
Matrix2f Q;
|
||||
Matrix2d Q;
|
||||
Q<< 0.03*0.03, 0,
|
||||
0, 0.03*0.03;
|
||||
|
||||
MatrixXf G(4,2);
|
||||
MatrixXd G(4,2);
|
||||
G<< T*T/2, 0,
|
||||
T, 0,
|
||||
0, T*T/2,
|
||||
0, T;
|
||||
|
||||
float rho,theta;
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
|
||||
Matrix2f R;
|
||||
Matrix2d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R(1,0)=R(0,1);
|
||||
|
||||
Matrix4f P_present;
|
||||
Matrix4d P_present;
|
||||
for (int i=0;i<4;i++)
|
||||
for (int j=0;j<4;j++)
|
||||
P_present(i,j)=P[i][j];
|
||||
Matrix4f P_pred;
|
||||
Matrix4d P_pred;
|
||||
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
|
||||
|
||||
//S
|
||||
Matrix2f S;
|
||||
Matrix2d S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
|
||||
|
||||
//d
|
||||
Vector2f Z_presnet= Vector2f(Z[0],Z[1]);
|
||||
Vector2f delta_z;
|
||||
Vector2d Z_presnet= Vector2d(Z[0],Z[1]);
|
||||
Vector2d delta_z;
|
||||
delta_z=Z_presnet-Z_pred;
|
||||
float d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
double d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
|
||||
return d;
|
||||
|
||||
@@ -125,14 +125,14 @@ float kalman::d_cal_track_init(float Z[2],float X[4],float P[4][4],float T)
|
||||
|
||||
|
||||
|
||||
void kalman::kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], float T1,float T2, float X[6], float P[6][6])
|
||||
void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6])
|
||||
{
|
||||
float x0=Z0[0];
|
||||
float y0=Z0[1];
|
||||
float x1=Z1[0];
|
||||
float y1=Z1[1];
|
||||
float x2=Z2[0];
|
||||
float y2=Z2[1];
|
||||
double x0=Z0[0];
|
||||
double y0=Z0[1];
|
||||
double x1=Z1[0];
|
||||
double y1=Z1[1];
|
||||
double x2=Z2[0];
|
||||
double y2=Z2[1];
|
||||
|
||||
X[0]=x2;
|
||||
X[1]=(x2-x1)/T2 ;
|
||||
@@ -143,15 +143,15 @@ void kalman::kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], floa
|
||||
|
||||
|
||||
|
||||
float R0[2][2];
|
||||
float R1[2][2];
|
||||
float R2[2][2];
|
||||
double R0[2][2];
|
||||
double R1[2][2];
|
||||
double R2[2][2];
|
||||
|
||||
float rho,theta;
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z0[0],Z0[1],&rho,&theta);
|
||||
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
R0[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R0[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R0[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
@@ -171,15 +171,15 @@ void kalman::kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], floa
|
||||
R2[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R2[1][0]=R2[0][1];
|
||||
|
||||
float P11[3][3]={{R2[0][0], R2[0][0]/T2, (R2[0][0]/T2)/((T2+T1)/2)} ,
|
||||
double P11[3][3]={{R2[0][0], R2[0][0]/T2, (R2[0][0]/T2)/((T2+T1)/2)} ,
|
||||
{R2[0][0]/T2, (R2[0][0]+R1[0][0])/pow(T2,2), ((R2[0][0]+R1[0][0])/pow(T2,2)+R1[0][0]/(T1*T2))/((T1+T2)/2)} ,
|
||||
{(R2[0][0]/T2)/((T2+T1)/2), ((R2[0][0]+R1[0][0])/pow(T2,2)+R1[0][0]/(T1*T2))/((T1+T2)/2) , 4*((R2[0][0]+R1[0][0])/pow(T2,2)+(R1[0][0]+R0[0][0])/pow(T1,2)+2*R1[0][0]/(T1*T2))/pow(T1+T2,2)}};
|
||||
|
||||
float P12[3][3]={{R2[0][1], R2[0][1]/T2, (R2[0][1]/T2)/((T2+T1)/2)} ,
|
||||
double P12[3][3]={{R2[0][1], R2[0][1]/T2, (R2[0][1]/T2)/((T2+T1)/2)} ,
|
||||
{R2[0][1]/T2, (R2[0][1]+R1[0][1])/pow(T2,2), ((R2[0][1]+R1[0][1])/pow(T2,2)+R1[0][1]/(T1*T2))/((T1+T2)/2)} ,
|
||||
{(R2[0][1]/T2)/((T2+T1)/2), ((R2[0][1]+R1[0][1])/pow(T2,2)+R1[0][1]/(T1*T2))/((T1+T2)/2) , 4*((R2[0][1]+R1[0][1])/pow(T2,2)+(R1[0][1]+R0[0][1])/pow(T1,2)+2*R1[0][1]/(T1*T2))/pow(T1+T2,2)}};
|
||||
|
||||
float P22[3][3]={{R2[1][1], R2[1][1]/T2, (R2[1][1]/T2)/((T2+T1)/2)} ,
|
||||
double P22[3][3]={{R2[1][1], R2[1][1]/T2, (R2[1][1]/T2)/((T2+T1)/2)} ,
|
||||
{R2[1][1]/T2, (R2[1][1]+R1[1][1])/pow(T2,2), ((R2[1][1]+R1[1][1])/pow(T2,2)+R1[1][1]/(T1*T2))/((T1+T2)/2)} ,
|
||||
{(R2[1][1]/T2)/((T2+T1)/2), ((R2[1][1]+R1[1][1])/pow(T2,2)+R1[1][1]/(T1*T2))/((T1+T2)/2) , 4*((R2[1][1]+R1[1][1])/pow(T2,2)+(R1[1][1]+R0[1][1])/pow(T1,2)+2*R1[1][1]/(T1*T2))/pow(T1+T2,2)}};
|
||||
for (int i=0;i<3;i++)
|
||||
@@ -203,11 +203,11 @@ void kalman::kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], floa
|
||||
|
||||
|
||||
|
||||
float kalman::d_cal(float Z[2],float X[6],float P[6][6])
|
||||
double kalman::d_cal(double Z[2],double X[6],double P[6][6])
|
||||
{
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf P_pred(6,6);
|
||||
Vector2f Z_mea(Z[0],Z[1]);
|
||||
VectorXd X_pred(6);
|
||||
MatrixXd P_pred(6,6);
|
||||
Vector2d Z_mea(Z[0],Z[1]);
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
X_pred(i)=X[i];
|
||||
@@ -217,46 +217,46 @@ float kalman::d_cal(float Z[2],float X[6],float P[6][6])
|
||||
P_pred(i,j)=P[i][j];
|
||||
|
||||
//Z(k+1|k)
|
||||
MatrixXf H(2,6);
|
||||
MatrixXd H(2,6);
|
||||
H<< 1,0,0,0,0,0,
|
||||
0,0,0,1,0,0;
|
||||
|
||||
Vector2f Z_pred;
|
||||
Vector2d Z_pred;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
//R
|
||||
float rho,theta;
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
|
||||
Matrix2f R;
|
||||
Matrix2d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R(1,0)=R(0,1);
|
||||
|
||||
//S
|
||||
Matrix2f S;
|
||||
Matrix2d S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
|
||||
//d
|
||||
Vector2f delta_z;
|
||||
Vector2d delta_z;
|
||||
delta_z=Z_mea-Z_pred;
|
||||
float d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
double d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
|
||||
|
||||
return d;
|
||||
|
||||
};
|
||||
|
||||
float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],float P[6][6],float prt,float freq_ind)
|
||||
double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind)
|
||||
{
|
||||
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf Q1(6,6);
|
||||
MatrixXd F1(6,6);
|
||||
MatrixXd Q1(6,6);
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
for (int j=0;j<6;j++)
|
||||
@@ -264,8 +264,8 @@ float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],floa
|
||||
F1(i,j) = F[i][j];
|
||||
Q1(i,j) = Q[i][j] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf P1(6,6);
|
||||
VectorXd X1(6);
|
||||
MatrixXd P1(6,6);
|
||||
for (int i=0;i<6;i++)
|
||||
X1(i)=X[i];
|
||||
|
||||
@@ -274,29 +274,29 @@ float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],floa
|
||||
P1(i,j)=P[i][j];
|
||||
|
||||
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf P_pred(6,6);
|
||||
VectorXd X_pred(6);
|
||||
MatrixXd P_pred(6,6);
|
||||
X_pred = F1*X1;
|
||||
P_pred = F1*P1*F1.transpose()+Q1;
|
||||
|
||||
Vector3f Z_pred;
|
||||
float x=X_pred(0);
|
||||
float vx=X_pred(1);
|
||||
float y=X_pred(3);
|
||||
float vy=X_pred(4);
|
||||
Vector3d Z_pred;
|
||||
double x=X_pred(0);
|
||||
double vx=X_pred(1);
|
||||
double y=X_pred(3);
|
||||
double vy=X_pred(4);
|
||||
Z_pred(0) = sqrt(x*x+y*y);
|
||||
Z_pred(1)=atan2(y,x);
|
||||
if(Z_pred(1)<0)
|
||||
Z_pred(1)=Z_pred(1)+2*PI;
|
||||
Z_pred(2)= -(x*vx+y*vy)/sqrt(x*x+y*y);
|
||||
|
||||
MatrixXf H(3,6);
|
||||
MatrixXd H(3,6);
|
||||
H(0,0) = x/sqrt(x*x+y*y); H(0,1)=0; H(0,2)=0; H(0,3)=y/sqrt(x*x+y*y); H(0,4)=0; H(0,5)=0;
|
||||
H(1,0) = -y/(x*x+y*y); H(1,1)=0; H(1,2)=0; H(1,3)=x/(x*x+y*y); H(1,4)=0; H(1,5)=0;
|
||||
H(2,0) = -y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,1)=-x/sqrt(x*x+y*y); H(2,2)=0;
|
||||
H(2,3) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,4)=-y/sqrt(x*x+y*y); H(2,5)=0;
|
||||
|
||||
Matrix3f R;
|
||||
Matrix3d R;
|
||||
R(0,0)=SIGMA_R*SIGMA_R;
|
||||
R(1,1)=SIGMA_A*SIGMA_A;
|
||||
R(2,2)=SIGMA_V*SIGMA_V;
|
||||
@@ -308,18 +308,18 @@ float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],floa
|
||||
R(1,2)=0;
|
||||
|
||||
//S
|
||||
Matrix3f S;
|
||||
Matrix3d S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
//d
|
||||
Vector3f Z_mea(Z[0],Z[1],Z[2]);
|
||||
Vector3f delta_z;
|
||||
Vector3d Z_mea(Z[0],Z[1],Z[2]);
|
||||
Vector3d delta_z;
|
||||
delta_z=Z_mea-Z_pred;
|
||||
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
|
||||
float d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
double d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
|
||||
return d;
|
||||
|
||||
@@ -327,11 +327,11 @@ float kalman::d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],floa
|
||||
}
|
||||
|
||||
|
||||
float kalman:: d_cal_with_doppler(float F[6][6], float Q[6][6] , float Z[2],float X[6],float P[6][6],float vr,float prt,float freq_ind)
|
||||
double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind)
|
||||
{
|
||||
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf Q1(6,6);
|
||||
MatrixXd F1(6,6);
|
||||
MatrixXd Q1(6,6);
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
for (int j=0;j<6;j++)
|
||||
@@ -339,8 +339,8 @@ float kalman:: d_cal_with_doppler(float F[6][6], float Q[6][6] , float Z[2],floa
|
||||
F1(i,j) = F[i][j];
|
||||
Q1(i,j) = Q[i][j] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf P1(6,6);
|
||||
VectorXd X1(6);
|
||||
MatrixXd P1(6,6);
|
||||
for (int i=0;i<6;i++)
|
||||
X1(i)=X[i];
|
||||
|
||||
@@ -349,39 +349,39 @@ float kalman:: d_cal_with_doppler(float F[6][6], float Q[6][6] , float Z[2],floa
|
||||
P1(i,j)=P[i][j];
|
||||
|
||||
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf P_pred(6,6);
|
||||
Vector3f Z_mea(Z[0],Z[1],vr);
|
||||
VectorXd X_pred(6);
|
||||
MatrixXd P_pred(6,6);
|
||||
Vector3d Z_mea(Z[0],Z[1],vr);
|
||||
|
||||
X_pred = F1*X1;
|
||||
P_pred = F1*P1*F1.transpose()+Q1;
|
||||
|
||||
|
||||
//Z(k+1|k)
|
||||
float x=X[0];
|
||||
float vx=X[1];
|
||||
float y=X[3];
|
||||
float vy=X[4];
|
||||
float h31=-y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y));
|
||||
float h32=-x/sqrt(x*x+y*y);
|
||||
float h34=-x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y));
|
||||
float h35=-y/sqrt(x*x+y*y);
|
||||
MatrixXf H(3,6);
|
||||
double x=X[0];
|
||||
double vx=X[1];
|
||||
double y=X[3];
|
||||
double vy=X[4];
|
||||
double h31=-y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y));
|
||||
double h32=-x/sqrt(x*x+y*y);
|
||||
double h34=-x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y));
|
||||
double h35=-y/sqrt(x*x+y*y);
|
||||
MatrixXd H(3,6);
|
||||
H(0,0)=1;H(0,1)=0;H(0,2)=0;H(0,3)=0;H(0,4)=0;H(0,5)=0;
|
||||
H(1,0)=0;H(1,1)=0;H(1,2)=0;H(1,3)=1;H(1,4)=0;H(1,5)=0;
|
||||
H(2,0)=h31;H(2,1)=h32;H(2,2)=0;H(2,3)=h34;H(2,4)=h35;H(2,5)=0;
|
||||
|
||||
Vector3f Z_pred;
|
||||
Vector3d Z_pred;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
//R
|
||||
float rho,theta;
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
|
||||
Matrix3f R;
|
||||
Matrix3d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
@@ -393,29 +393,29 @@ float kalman:: d_cal_with_doppler(float F[6][6], float Q[6][6] , float Z[2],floa
|
||||
R(1,2)=0;
|
||||
|
||||
//S
|
||||
Matrix3f S;
|
||||
Matrix3d S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
// bind_speed
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
|
||||
//d
|
||||
Vector3f delta_z;
|
||||
Vector3d delta_z;
|
||||
delta_z=Z_mea-Z_pred;
|
||||
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
|
||||
float d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
double d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
|
||||
|
||||
return d;
|
||||
|
||||
};
|
||||
|
||||
void kalman::kalman_pred(float F[6][6], float Q[6][6] ,float X[6],float P[6][6],float X_pred[6],float P_pred[6][6])
|
||||
void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6][6],double X_pred[6],double P_pred[6][6])
|
||||
{
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf Q1(6,6);
|
||||
MatrixXd F1(6,6);
|
||||
MatrixXd Q1(6,6);
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
for (int j=0;j<6;j++)
|
||||
@@ -423,8 +423,8 @@ void kalman::kalman_pred(float F[6][6], float Q[6][6] ,float X[6],float P[6][6],
|
||||
F1(i,j) = F[i][j];
|
||||
Q1(i,j) = Q[i][j] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf P1(6,6);
|
||||
VectorXd X1(6);
|
||||
MatrixXd P1(6,6);
|
||||
for (int i=0;i<6;i++)
|
||||
X1(i)=X[i];
|
||||
|
||||
@@ -432,8 +432,8 @@ void kalman::kalman_pred(float F[6][6], float Q[6][6] ,float X[6],float P[6][6],
|
||||
for (int j=0;j<6;j++)
|
||||
P1(i,j)=P[i][j];
|
||||
|
||||
VectorXf X1_pred(6);
|
||||
MatrixXf P1_pred(6,6);
|
||||
VectorXd X1_pred(6);
|
||||
MatrixXd P1_pred(6,6);
|
||||
|
||||
|
||||
X1_pred = F1*X1;
|
||||
@@ -449,12 +449,12 @@ void kalman::kalman_pred(float F[6][6], float Q[6][6] ,float X[6],float P[6][6],
|
||||
|
||||
};
|
||||
|
||||
void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[6][6],float Z[3],
|
||||
float X_filter[6],float P_filter[6][6],float S_filter[2][2],
|
||||
float prt,float freq_ind )
|
||||
void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],double P_cur[6][6],double Z[3],
|
||||
double X_filter[6],double P_filter[6][6],double S_filter[2][2],
|
||||
double prt,double freq_ind )
|
||||
{
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf Q1(6,6);
|
||||
MatrixXd F1(6,6);
|
||||
MatrixXd Q1(6,6);
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
for (int j=0;j<6;j++)
|
||||
@@ -462,17 +462,17 @@ void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[
|
||||
F1(i,j) = F[i][j];
|
||||
Q1(i,j) = Q[i][j] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf P1(6,6);
|
||||
VectorXd X1(6);
|
||||
MatrixXd P1(6,6);
|
||||
for (int i=0;i<6;i++)
|
||||
X1(i)=X[i];
|
||||
X1(i)=X_cur[i];
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
for (int j=0;j<6;j++)
|
||||
P1(i,j)=P[i][j];
|
||||
P1(i,j)=P_cur[i][j];
|
||||
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf P_pred(6,6);
|
||||
VectorXd X_pred(6);
|
||||
MatrixXd P_pred(6,6);
|
||||
|
||||
|
||||
X_pred = F1*X1;
|
||||
@@ -480,26 +480,26 @@ void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[
|
||||
|
||||
|
||||
|
||||
Vector3f Z_mea(Z[0],Z[1],Z[2]);
|
||||
Vector3d Z_mea(Z[0],Z[1],Z[2]);
|
||||
|
||||
Vector3f Z_pred;
|
||||
float x=X_pred(0);
|
||||
float vx=X_pred(1);
|
||||
float y=X_pred(3);
|
||||
float vy=X_pred(4);
|
||||
Vector3d Z_pred;
|
||||
double x=X_pred(0);
|
||||
double vx=X_pred(1);
|
||||
double y=X_pred(3);
|
||||
double vy=X_pred(4);
|
||||
Z_pred(0) = sqrt(x*x+y*y);
|
||||
Z_pred(1)=atan2(y,x);
|
||||
if(Z_pred(1)<0)
|
||||
Z_pred(1)=Z_pred(1)+2*PI;
|
||||
Z_pred(2)= -(x*vx+y*vy)/sqrt(x*x+y*y);
|
||||
|
||||
MatrixXf H(3,6);
|
||||
MatrixXd H(3,6);
|
||||
H(0,0) = x/sqrt(x*x+y*y); H(0,1)=0; H(0,2)=0; H(0,3)=y/sqrt(x*x+y*y); H(0,4)=0; H(0,5)=0;
|
||||
H(1,0) = -y/(x*x+y*y); H(1,1)=0; H(1,2)=0; H(1,3)=x/(x*x+y*y); H(1,4)=0; H(1,5)=0;
|
||||
H(2,0) = -y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,1)=-x/sqrt(x*x+y*y); H(2,2)=0;
|
||||
H(2,3) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,4)=-y/sqrt(x*x+y*y); H(2,5)=0;
|
||||
|
||||
Matrix3f R;
|
||||
Matrix3d R;
|
||||
R(0,0)=SIGMA_R*SIGMA_R;
|
||||
R(1,1)=SIGMA_A*SIGMA_A;
|
||||
R(2,2)=SIGMA_V*SIGMA_V;
|
||||
@@ -511,29 +511,29 @@ void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[
|
||||
R(1,2)=0;
|
||||
|
||||
//S
|
||||
Matrix3f S;
|
||||
Matrix3d S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//kalmam gain
|
||||
MatrixXf K;
|
||||
MatrixXd K;
|
||||
K=P_pred*H.transpose()*S.inverse();
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
|
||||
//d
|
||||
Vector3f delta_z;
|
||||
Vector3d delta_z;
|
||||
delta_z=Z_mea-Z_pred;
|
||||
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
|
||||
|
||||
//X(k+1|k+1)
|
||||
VectorXf X;
|
||||
VectorXd X;
|
||||
X=X_pred+K*delta_z;
|
||||
|
||||
//P(k+1|k+1)
|
||||
MatrixXf P;
|
||||
MatrixXf I;
|
||||
MatrixXd P;
|
||||
MatrixXd I;
|
||||
I.setIdentity(6, 6);
|
||||
|
||||
|
||||
@@ -554,12 +554,12 @@ void kalman::kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[
|
||||
}
|
||||
|
||||
|
||||
void kalman::kalman_filter(float F[6][6], float Q[6][6],
|
||||
float X[6],float P[6][6],
|
||||
float Z[2],float X_filter[6],float P_filter[6][6],float S_filter[2][2])
|
||||
void kalman::kalman_filter(double F[6][6], double Q[6][6],
|
||||
double X_cur[6],double P_cur[6][6],
|
||||
double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2])
|
||||
{
|
||||
MatrixXf F1(6,6);
|
||||
MatrixXf Q1(6,6);
|
||||
MatrixXd F1(6,6);
|
||||
MatrixXd Q1(6,6);
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
for (int j=0;j<6;j++)
|
||||
@@ -567,17 +567,17 @@ void kalman::kalman_filter(float F[6][6], float Q[6][6],
|
||||
F1(i,j) = F[i][j];
|
||||
Q1(i,j) = Q[i][j] ;
|
||||
}
|
||||
VectorXf X1(6);
|
||||
MatrixXf P1(6,6);
|
||||
VectorXd X1(6);
|
||||
MatrixXd P1(6,6);
|
||||
for (int i=0;i<6;i++)
|
||||
X1(i)=X[i];
|
||||
X1(i)=X_cur[i];
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
for (int j=0;j<6;j++)
|
||||
P1(i,j)=P[i][j];
|
||||
P1(i,j)=P_cur[i][j];
|
||||
|
||||
VectorXf X_pred(6);
|
||||
MatrixXf P_pred(6,6);
|
||||
VectorXd X_pred(6);
|
||||
MatrixXd P_pred(6,6);
|
||||
|
||||
|
||||
X_pred = F1*X1;
|
||||
@@ -585,48 +585,48 @@ void kalman::kalman_filter(float F[6][6], float Q[6][6],
|
||||
|
||||
|
||||
|
||||
Vector2f Z_mea(Z[0],Z[1]);
|
||||
Vector2d Z_mea(Z[0],Z[1]);
|
||||
|
||||
|
||||
|
||||
//Z(k+1|k)
|
||||
MatrixXf H(2,6);
|
||||
MatrixXd H(2,6);
|
||||
H<< 1,0,0,0,0,0,
|
||||
0,0,0,1,0,0;
|
||||
|
||||
Vector2f Z_pred;
|
||||
Vector2d Z_pred;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
//R
|
||||
float rho,theta;
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
|
||||
Matrix2f R;
|
||||
Matrix2d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R(1,0)=R(0,1);
|
||||
|
||||
//S
|
||||
Matrix2f S;
|
||||
Matrix2d S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
|
||||
//kalmam gain
|
||||
|
||||
MatrixXf K;
|
||||
MatrixXd K;
|
||||
K=P_pred*H.transpose()*S.inverse();
|
||||
|
||||
//X(k+1|k+1)
|
||||
VectorXf X;
|
||||
VectorXd X;
|
||||
X=X_pred+K*(Z_mea-Z_pred);
|
||||
|
||||
//P(k+1|k+1)
|
||||
MatrixXf P;
|
||||
MatrixXf I;
|
||||
MatrixXd P;
|
||||
MatrixXd I;
|
||||
I.setIdentity(6, 6);
|
||||
|
||||
|
||||
@@ -646,57 +646,57 @@ void kalman::kalman_filter(float F[6][6], float Q[6][6],
|
||||
|
||||
};
|
||||
|
||||
float kalman::d_cal_track_init_EKF(float Z[3],float X[4],float P[4][4],float T,float prt,float freq_ind)
|
||||
double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind)
|
||||
{
|
||||
|
||||
//X(k+1|k)
|
||||
Matrix4f F;
|
||||
Matrix4d F;
|
||||
F<< 1, T, 0, 0,
|
||||
0, 1, 0, 0,
|
||||
0, 0, 1, T,
|
||||
0, 0, 0, 1;
|
||||
Vector4f X_present = Vector4f(X[0],X[1],X[2],X[3]);
|
||||
Vector4f X_pred;
|
||||
Vector4d X_present = Vector4d(X[0],X[1],X[2],X[3]);
|
||||
Vector4d X_pred;
|
||||
X_pred=F*X_present;
|
||||
|
||||
//P(k+1|k)
|
||||
Matrix2f Q;
|
||||
Matrix2d Q;
|
||||
Q<< 0.03*0.03, 0,
|
||||
0, 0.03*0.03;
|
||||
|
||||
MatrixXf G(4,2);
|
||||
MatrixXd G(4,2);
|
||||
G<< T*T/2, 0,
|
||||
T, 0,
|
||||
0, T*T/2,
|
||||
0, T;
|
||||
|
||||
Matrix4f P_present;
|
||||
Matrix4d P_present;
|
||||
for (int i=0;i<4;i++)
|
||||
for (int j=0;j<4;j++)
|
||||
P_present(i,j)=P[i][j];
|
||||
|
||||
Matrix4f P_pred;
|
||||
Matrix4d P_pred;
|
||||
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
|
||||
|
||||
//Z(k+1|k)
|
||||
Vector3f Z_pred;
|
||||
float x=X_pred(0);
|
||||
float vx=X_pred(1);
|
||||
float y=X_pred(2);
|
||||
float vy=X_pred(3);
|
||||
Vector3d Z_pred;
|
||||
double x=X_pred(0);
|
||||
double vx=X_pred(1);
|
||||
double y=X_pred(2);
|
||||
double vy=X_pred(3);
|
||||
Z_pred(0) = sqrt(x*x+y*y);
|
||||
Z_pred(1)=atan2(y,x);
|
||||
if(Z_pred(1)<0)
|
||||
Z_pred(1)=Z_pred(1)+2*PI;
|
||||
Z_pred(2)= -(x*vx+y*vy)/sqrt(x*x+y*y);
|
||||
|
||||
MatrixXf H(3,4);
|
||||
MatrixXd H(3,4);
|
||||
H(0,0) = x/sqrt(x*x+y*y); H(0,1)=0; H(0,2)=y/sqrt(x*x+y*y); H(0,3)=0;
|
||||
H(1,0) = -y/(x*x+y*y); H(1,1)=0; H(1,2)=x/(x*x+y*y); H(1,3)=0;
|
||||
H(2,0) = -y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,1)=-x/sqrt(x*x+y*y);
|
||||
H(2,2) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,3)=-y/sqrt(x*x+y*y);
|
||||
|
||||
Matrix3f R;
|
||||
Matrix3d R;
|
||||
R(0,0)=SIGMA_R*SIGMA_R;
|
||||
R(1,1)=SIGMA_A*SIGMA_A;
|
||||
R(2,2)=SIGMA_V*SIGMA_V;
|
||||
@@ -708,80 +708,80 @@ float kalman::d_cal_track_init_EKF(float Z[3],float X[4],float P[4][4],float T,f
|
||||
R(1,2)=0;
|
||||
|
||||
//S
|
||||
Matrix3f S;
|
||||
Matrix3d S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
//d
|
||||
Vector3f Z_presnet= Vector3f(Z[0],Z[1],Z[2]);
|
||||
Vector3f delta_z;
|
||||
Vector3d Z_presnet= Vector3d(Z[0],Z[1],Z[2]);
|
||||
Vector3d delta_z;
|
||||
delta_z=Z_presnet-Z_pred;
|
||||
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
|
||||
float d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
double d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
|
||||
return d;
|
||||
|
||||
}
|
||||
|
||||
float kalman::d_cal_track_init_with_doppler(float Z[2],float X[4],float P[4][4],float T,float vr,float prt,float freq_ind)
|
||||
double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind)
|
||||
{
|
||||
//X(k+1|k)
|
||||
Matrix4f F;
|
||||
Matrix4d F;
|
||||
F<< 1, T, 0, 0,
|
||||
0, 1, 0, 0,
|
||||
0, 0, 1, T,
|
||||
0, 0, 0, 1;
|
||||
Vector4f X_present = Vector4f(X[0],X[1],X[2],X[3]);
|
||||
Vector4f X_pred;
|
||||
Vector4d X_present = Vector4d(X[0],X[1],X[2],X[3]);
|
||||
Vector4d X_pred;
|
||||
X_pred=F*X_present;
|
||||
|
||||
//Z(k+1|k)
|
||||
float x=X_pred(0);
|
||||
float vx=X_pred(1);
|
||||
float y=X_pred(2);
|
||||
float vy=X_pred(3);
|
||||
float h31=-y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y));
|
||||
float h32=-x/sqrt(x*x+y*y);
|
||||
float h33=-x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y));
|
||||
float h34=-y/sqrt(x*x+y*y);
|
||||
MatrixXf H(3,4);
|
||||
double x=X_pred(0);
|
||||
double vx=X_pred(1);
|
||||
double y=X_pred(2);
|
||||
double vy=X_pred(3);
|
||||
double h31=-y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y));
|
||||
double h32=-x/sqrt(x*x+y*y);
|
||||
double h33=-x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y));
|
||||
double h34=-y/sqrt(x*x+y*y);
|
||||
MatrixXd H(3,4);
|
||||
H(0,0)=1;H(0,1)=0;H(0,2)=0;H(0,3)=0;
|
||||
H(1,0)=0;H(1,1)=0;H(1,2)=1;H(1,3)=0;
|
||||
H(2,0)=h31;H(2,1)=h32;H(2,2)=h33;H(2,3)=h34;
|
||||
|
||||
Vector3f Z_pred;
|
||||
Vector3d Z_pred;
|
||||
Z_pred=H*X_pred;
|
||||
|
||||
|
||||
//P(k+1|k)
|
||||
Matrix2f Q;
|
||||
Matrix2d Q;
|
||||
Q<< 0.03*0.03, 0,
|
||||
0, 0.03*0.03;
|
||||
|
||||
MatrixXf G(4,2);
|
||||
MatrixXd G(4,2);
|
||||
G<< T*T/2, 0,
|
||||
T, 0,
|
||||
0, T*T/2,
|
||||
0, T;
|
||||
|
||||
Matrix4f P_present;
|
||||
Matrix4d P_present;
|
||||
for (int i=0;i<4;i++)
|
||||
for (int j=0;j<4;j++)
|
||||
P_present(i,j)=P[i][j];
|
||||
|
||||
Matrix4f P_pred;
|
||||
Matrix4d P_pred;
|
||||
P_pred=F*P_present*F.transpose()+G*Q*G.transpose();
|
||||
|
||||
//R
|
||||
float rho,theta;
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
float lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
float lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
|
||||
Matrix3f R;
|
||||
Matrix3d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
@@ -793,27 +793,27 @@ float kalman::d_cal_track_init_with_doppler(float Z[2],float X[4],float P[4][4],
|
||||
R(1,2)=0;
|
||||
|
||||
//S
|
||||
Matrix3f S;
|
||||
Matrix3d S;
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//bind_speed
|
||||
float v_bind=Bind_speed(prt, freq_ind);
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
|
||||
//d
|
||||
Vector3f Z_presnet= Vector3f(Z[0],Z[1],vr);
|
||||
Vector3f delta_z;
|
||||
Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr);
|
||||
Vector3d delta_z;
|
||||
delta_z=Z_presnet-Z_pred;
|
||||
delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind;
|
||||
float d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
double d=delta_z.transpose()*S.inverse()*delta_z;
|
||||
|
||||
return d;
|
||||
|
||||
};
|
||||
|
||||
|
||||
float kalman::Bind_speed(float prt,float freq_ind)
|
||||
double kalman::Bind_speed(double prt,double freq_ind)
|
||||
{
|
||||
float freq=FREQ0+freq_ind*0.02;
|
||||
double freq=FREQ0+freq_ind*0.02;
|
||||
|
||||
|
||||
return 150000.0/(freq*prt);
|
||||
|
||||
@@ -12,32 +12,32 @@ class kalman
|
||||
{
|
||||
public:
|
||||
|
||||
void kalman_pred(float F[6][6], float Q[6][6], float X[6], float P[6][6], float X_pred[6], float P_pred[6][6]);
|
||||
void kalman_pred(double F[6][6], double Q[6][6], double X[6], double P[6][6], double X_pred[6], double P_pred[6][6]);
|
||||
|
||||
void kalman_filter_init_2dots(float Z0[2], float Z1[2], float T,float X[4], float P[4][4]);//两点初始化卡尔曼滤波器
|
||||
void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4]);//两点初始化卡尔曼滤波器
|
||||
|
||||
|
||||
float d_cal_track_init(float Z[2],float X[4],float P[4][4],float T); //计算点 临时航迹的d
|
||||
double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T); //计算点 临时航迹的d
|
||||
|
||||
float d_cal_track_init_with_doppler(float Z[2],float X[4],float P[4][4],float T,float vr,float prt,float freq_ind);
|
||||
double d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind);
|
||||
|
||||
float d_cal_track_init_EKF(float Z[3],float X[4],float P[4][4],float T,float prt,float freq_ind);
|
||||
double d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind);
|
||||
|
||||
void kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], float T1,float T2, float X[6], float P[6][6]);
|
||||
void kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6]);
|
||||
|
||||
float d_cal(float Z[2],float X[6],float P[6][6]);
|
||||
double d_cal(double Z[2],double X[6],double P[6][6]);
|
||||
|
||||
float d_cal_with_doppler(float F[6][6], float Q[6][6] ,float Z[2],float X[6],float P[6][6],float vr,float prt,float freq_ind);
|
||||
double d_cal_with_doppler(double F[6][6], double Q[6][6] ,double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind);
|
||||
|
||||
float d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],float P[6][6],float prt,float freq_ind);
|
||||
double d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind);
|
||||
|
||||
void kalman_filter(float F[6][6], float Q[6][6], float X[6],float P[6][6],float Z[2],float X_filter[6],float P_filter[6][6],float S_filter[2][2]);
|
||||
void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2]);
|
||||
|
||||
void kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[6][6],float Z[3],
|
||||
float X_filter[6],float P_filter[6][6],float S_filter[2][2],
|
||||
float prt,float freq_ind );
|
||||
void kalman_filter_EKF(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[3],
|
||||
double X_filter[6],double P_filter[6][6],double S_filter[2][2],
|
||||
double prt,double freq_ind );
|
||||
|
||||
float Bind_speed(float prt,float freq_ind); //根据PRT 和 频率 计算不模糊速度
|
||||
double Bind_speed(double prt,double freq_ind); //根据PRT 和 频率 计算不模糊速度
|
||||
|
||||
};
|
||||
#endif // KALMAN_H
|
||||
|
||||
@@ -5,72 +5,72 @@
|
||||
#define PI 3.1415926f
|
||||
|
||||
//频点(GHz)
|
||||
#define FREQ0 5.262
|
||||
#define FREQ1 5.282
|
||||
#define FREQ2 5.302
|
||||
#define FREQ3 5.322
|
||||
#define FREQ4 5.342
|
||||
#define FREQ5 5.362
|
||||
#define FREQ6 5.382
|
||||
#define FREQ7 5.402
|
||||
#define FREQ8 5.422
|
||||
#define FREQ9 5.442
|
||||
#define FREQ10 5.462
|
||||
#define FREQ11 5.482
|
||||
#define FREQ12 5.502
|
||||
#define FREQ13 5.522
|
||||
#define FREQ14 5.542
|
||||
#define FREQ15 5.560
|
||||
#define FREQ16 5.582
|
||||
#define FREQ17 5.602
|
||||
#define FREQ18 5.622
|
||||
#define FREQ19 5.642
|
||||
#define FREQ20 5.662
|
||||
#define FREQ0 16.8
|
||||
#define FREQ1 16.8
|
||||
#define FREQ2 16.8
|
||||
#define FREQ3 16.8
|
||||
#define FREQ4 16.8
|
||||
#define FREQ5 16.8
|
||||
#define FREQ6 16.8
|
||||
#define FREQ7 16.8
|
||||
#define FREQ8 16.8
|
||||
#define FREQ9 16.8
|
||||
#define FREQ10 16.8
|
||||
#define FREQ11 16.8
|
||||
#define FREQ12 16.8
|
||||
#define FREQ13 16.8
|
||||
#define FREQ14 16.8
|
||||
#define FREQ15 16.8
|
||||
#define FREQ16 16.8
|
||||
#define FREQ17 16.8
|
||||
#define FREQ18 16.8
|
||||
#define FREQ19 16.8
|
||||
#define FREQ20 16.8
|
||||
|
||||
|
||||
//波位
|
||||
#define BEAM_NUM 54
|
||||
//#define BEAM_NUM 54
|
||||
|
||||
#define BEAM_WIDTH 6.666
|
||||
//#define BEAM_WIDTH 6.666
|
||||
|
||||
//一个点迹区包含波位
|
||||
#define DOT_SECTION_NUM 9
|
||||
#define BEAM_NUM_DOT_SECTION 6
|
||||
////一个点迹区包含波位
|
||||
//#define DOT_SECTION_NUM 9
|
||||
//#define BEAM_NUM_DOT_SECTION 6
|
||||
|
||||
|
||||
#define DOT_SECTION_1 5
|
||||
#define DOT_SECTION_2 11
|
||||
#define DOT_SECTION_3 17
|
||||
#define DOT_SECTION_4 23
|
||||
#define DOT_SECTION_5 29
|
||||
#define DOT_SECTION_6 35
|
||||
#define DOT_SECTION_7 41
|
||||
#define DOT_SECTION_8 47
|
||||
#define DOT_SECTION_9 53
|
||||
//#define DOT_SECTION_1 5
|
||||
//#define DOT_SECTION_2 11
|
||||
//#define DOT_SECTION_3 17
|
||||
//#define DOT_SECTION_4 23
|
||||
//#define DOT_SECTION_5 29
|
||||
//#define DOT_SECTION_6 35
|
||||
//#define DOT_SECTION_7 41
|
||||
//#define DOT_SECTION_8 47
|
||||
//#define DOT_SECTION_9 53
|
||||
|
||||
|
||||
//航迹区划分
|
||||
#define TRACK_SECTION_NUM 9
|
||||
#define TRACK_SECTION_WIDTH 40
|
||||
//#define TRACK_SECTION_NUM 9
|
||||
//#define TRACK_SECTION_WIDTH 40
|
||||
|
||||
#define TRACK_SECTION_1_START 13.2
|
||||
#define TRACK_SECTION_2_START 53.2
|
||||
#define TRACK_SECTION_3_START 93.2
|
||||
#define TRACK_SECTION_4_START 133.2
|
||||
#define TRACK_SECTION_5_START 173.2
|
||||
#define TRACK_SECTION_6_START 213.2
|
||||
#define TRACK_SECTION_7_START 253.2
|
||||
#define TRACK_SECTION_8_START 293.2
|
||||
#define TRACK_SECTION_9_START 333.2
|
||||
//#define TRACK_SECTION_1_START 13.2
|
||||
//#define TRACK_SECTION_2_START 53.2
|
||||
//#define TRACK_SECTION_3_START 93.2
|
||||
//#define TRACK_SECTION_4_START 133.2
|
||||
//#define TRACK_SECTION_5_START 173.2
|
||||
//#define TRACK_SECTION_6_START 213.2
|
||||
//#define TRACK_SECTION_7_START 253.2
|
||||
//#define TRACK_SECTION_8_START 293.2
|
||||
//#define TRACK_SECTION_9_START 333.2
|
||||
|
||||
|
||||
/******************************数据处理参数**************************************/
|
||||
|
||||
#define DATA_RATE_SHORT 5.076 //三种不同模式下的 数据率
|
||||
#define DATA_RATE_MIDDLE 4.986
|
||||
#define DATA_RATE_FAR 3.024
|
||||
//#define DATA_RATE_SHORT 1 //三种不同模式下的 数据率
|
||||
//#define DATA_RATE_MIDDLE 1
|
||||
//#define DATA_RATE_FAR 1
|
||||
|
||||
#define DATA_RATE_TAS 2
|
||||
#define DATA_RATE_TAS 0.3
|
||||
|
||||
#define SIGMA_R 10.0 //测量误差
|
||||
#define SIGMA_A 0.02
|
||||
@@ -79,43 +79,44 @@
|
||||
|
||||
#define DOT_COH_RANGE 80 //点迹凝聚
|
||||
#define DOT_COH_V 2
|
||||
#define DOT_COH_AZI 4
|
||||
#define DOT_COH_AZI 6
|
||||
|
||||
#define MAX_BEAM_NUM 100 //最大TWS波位数
|
||||
|
||||
|
||||
|
||||
#define MAX_TRACK_NUM 200
|
||||
#define MAX_TRACK_INDEX 200 //最大航迹批号
|
||||
#define MAX_TRACK_NUM 500
|
||||
#define MAX_TRACK_INDEX 500 //最大航迹批号
|
||||
|
||||
#define ASSOCIATE_THRESHOLD_MIN 1
|
||||
#define ASSOCIATE_THRESHOLD_MID 3 //关联波门
|
||||
#define ASSOCIATE_THRESHOLD_MAX 10
|
||||
|
||||
#define R_MIN 100
|
||||
#define R_MAX 60000
|
||||
#define R_MAX 100000
|
||||
|
||||
#define V_MAX 550 //最大速度
|
||||
#define V_MIN 1 //最小速度
|
||||
#define TRACK_START_THRESHOLD 3 //起航波门
|
||||
#define ALPHA_START 70 //起航夹角
|
||||
//#define V_MAX 500 //最大速度
|
||||
//#define V_MIN 1 //速度最小
|
||||
#define TRACK_START_THRESHOLD 8 //起航波门 3 越大越容易起批
|
||||
#define ALPHA_START 60 //起航夹角 120 越大越容易起批
|
||||
|
||||
|
||||
#define TRACK_DIE_ROUND 3 // 航迹消亡时间
|
||||
#define TRACK_DIE_ROUND 4 // 航迹消亡时间
|
||||
#define TRACK_DIE_ROUND_TAS 5
|
||||
|
||||
#define ASSO_THORD 3
|
||||
|
||||
|
||||
#define H_F_WIN_LEN 10
|
||||
#define H_F_WIN_LEN 3
|
||||
|
||||
|
||||
#define TAS_QUEUE_LENGTH 8
|
||||
#define TAS_QUEUE_LENGTH 4 //Tas调用的cpi数
|
||||
#define MAX_TAS_NUM 4
|
||||
#define T_TAS_PRED 1
|
||||
#define T_TAS_PRED 0.7
|
||||
|
||||
#define UNCONF_TARGET 0
|
||||
|
||||
#define DIRECT_TRACKING_QUEUE_LENGTH 10
|
||||
|
||||
|
||||
#define Round(x) (((x) > 0) \
|
||||
? (((int)((x) + 0.5) > (int)(x)) ? ((int)(x) + 1) : ((int)(x))) \
|
||||
|
||||
@@ -25,6 +25,10 @@ struct PointRecv
|
||||
int Freq_index; //频点
|
||||
int track_mode; //TAS 1 TWS0
|
||||
int point_section_asso; //点迹区
|
||||
|
||||
int pitch_num; // 俯仰波位号
|
||||
float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点
|
||||
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
|
||||
};
|
||||
|
||||
|
||||
@@ -33,8 +37,8 @@ struct Trust_Track
|
||||
{
|
||||
int Track_Index; //航迹号
|
||||
|
||||
float Amplitude; //幅度
|
||||
float Height; //高度
|
||||
double Amplitude; //幅度
|
||||
double Height; //高度
|
||||
|
||||
int T_track; //航迹时间
|
||||
|
||||
@@ -56,29 +60,33 @@ struct Trust_Track
|
||||
|
||||
int Track_section_idx; //航迹区号
|
||||
|
||||
QVector <float> Hight_smooth; //高度平滑
|
||||
QVector <double> Hight_smooth; //高度平滑
|
||||
|
||||
|
||||
float range_point; //关联上的点信息（距离、方位、俯仰）
|
||||
float azi_point;
|
||||
float elev_point;
|
||||
float vr_point;
|
||||
float prf_point;
|
||||
double range_point; //关联上的点信息（距离、方位、俯仰）
|
||||
double azi_point;
|
||||
double elev_point;
|
||||
double vr_point;
|
||||
double prf_point;
|
||||
int point_type; //TWS TAS
|
||||
float snr_point; //信噪比
|
||||
double snr_point; //信噪比
|
||||
|
||||
float RCS;
|
||||
double RCS;
|
||||
|
||||
float X[6]; //IMM
|
||||
float P[6][6];
|
||||
int pitch_num; // 俯仰波位号
|
||||
float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点
|
||||
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
|
||||
|
||||
float X1[6];
|
||||
float X2[6];
|
||||
float X3[6];
|
||||
float P1[6][6];
|
||||
float P2[6][6];
|
||||
float P3[6][6];
|
||||
float u[3];
|
||||
double X[6]; //IMM
|
||||
double P[6][6];
|
||||
|
||||
double X1[6];
|
||||
double X2[6];
|
||||
double X3[6];
|
||||
double P1[6][6];
|
||||
double P2[6][6];
|
||||
double P3[6][6];
|
||||
double u[3];
|
||||
|
||||
};
|
||||
|
||||
@@ -88,23 +96,29 @@ struct Trust_Track
|
||||
struct Temp_track
|
||||
{
|
||||
|
||||
float X[4]; //状态
|
||||
float P[4][4]; //协方差
|
||||
double X[4]; //状态
|
||||
double P[4][4]; //协方差
|
||||
|
||||
float r; //距离
|
||||
float azi; //方位
|
||||
float vr; //径向速度
|
||||
float height; //高度
|
||||
float snr; //信噪比
|
||||
float RCS; //RCS
|
||||
float Amp; //幅度
|
||||
double r; //距离
|
||||
double azi; //方位
|
||||
double vr; //径向速度
|
||||
double height; //高度
|
||||
double snr; //信噪比
|
||||
double RCS; //RCS
|
||||
double Amp; //幅度
|
||||
int T; //时间戳
|
||||
|
||||
double d; //关联上的点的d
|
||||
|
||||
int buff_round; //已缓存圈数
|
||||
|
||||
int Temp_track_section_idx; //临时航迹区号 与点迹区的划分相同
|
||||
|
||||
int asso_flag; //关联标志 已关联：1 未关联：0
|
||||
|
||||
int pitch_num; // 俯仰波位号
|
||||
float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点
|
||||
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
|
||||
};
|
||||
|
||||
|
||||
@@ -121,6 +135,17 @@ struct Tracking_Target
|
||||
};
|
||||
|
||||
|
||||
//引导跟踪目标结构体
|
||||
struct Direct_Tracking_Target
|
||||
{
|
||||
int ID;
|
||||
double Azi;
|
||||
double Elev;
|
||||
double Range;
|
||||
int empty_flag; //是否为空标志位 1非空 0空
|
||||
// int track_init_flag; //是否已经建航 1是 0否
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
+104
-109
@@ -18,8 +18,7 @@ TAS_Ctrl::TAS_Ctrl()
|
||||
tas_target_num=0;
|
||||
}
|
||||
|
||||
|
||||
void TAS_Ctrl::tas_ctrl_process(QVector <QVector<Trust_Track>> *trust_track,
|
||||
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,
|
||||
@@ -43,27 +42,26 @@ void TAS_Ctrl::tas_ctrl_process(QVector <QVector<Trust_Track>> *trust_track,
|
||||
};
|
||||
|
||||
|
||||
void TAS_Ctrl::tas_target_add(QVector <QVector<Trust_Track>> *trust_track,
|
||||
void TAS_Ctrl::tas_target_add(QVector<Trust_Track> *trust_track,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter)
|
||||
{
|
||||
|
||||
for (int i=0;i<trust_track->size();i++)
|
||||
for (int k=0;k<(*trust_track)[i].size();k++)
|
||||
{
|
||||
float r=sqrt(pow((*trust_track)[i][k].X[0],2)+pow((*trust_track)[i][k].X[3],2));
|
||||
float v=sqrt(pow((*trust_track)[i][k].X[1],2)+pow((*trust_track)[i][k].X[4],2));
|
||||
float h=(*trust_track)[i][k].Height+Work_Parameter.Height;
|
||||
float azi = atan2((*trust_track)[i][k].X[3],(*trust_track)[i][k].X[0]);
|
||||
double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
|
||||
double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
|
||||
double h=(*trust_track)[i].Height+Work_Parameter.Height;
|
||||
double azi = atan2((*trust_track)[i].X[3],(*trust_track)[i].X[0]);
|
||||
if(azi<0)
|
||||
azi=azi+2*PI;
|
||||
azi=azi/PI*180;
|
||||
|
||||
//进入跟踪的条件： 1.手动跟踪的目标 或 满足速度、距离、高度条件满足 关联点数大于2 2.不在禁止跟踪区域
|
||||
if( ( (*trust_track)[i][k].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, Work_Parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
|
||||
if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, Work_Parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
|
||||
&& tas_target_num < MAX_TAS_NUM
|
||||
&& (*trust_track)[i][k].Track_Mode == 0
|
||||
&& (*trust_track)[i].Track_Mode == 0
|
||||
&& tas_prohibite_area( r, azi, v, h, Work_Parameter) == 0)
|
||||
{
|
||||
for(int j=0;j<TAS_QUEUE_LENGTH;j++)
|
||||
@@ -71,33 +69,33 @@ void TAS_Ctrl::tas_target_add(QVector <QVector<Trust_Track>> *trust_track,
|
||||
if(tas_target_queue[j].empty_flag==0 && j%2==0)
|
||||
{
|
||||
//插入跟踪队列
|
||||
tas_target_queue[j].Index=(*trust_track)[i][k].Track_Index;
|
||||
tas_target_queue[j].Index=(*trust_track)[i].Track_Index;
|
||||
tas_target_queue[j].empty_flag=1;
|
||||
|
||||
//跟踪目标数目加一
|
||||
tas_target_num=tas_target_num+1;
|
||||
|
||||
//TAS跟踪标志置1
|
||||
(*trust_track)[i][k].Track_Mode=1;
|
||||
(*trust_track)[i].Track_Mode=1;
|
||||
|
||||
//目标跟踪状态发生改变 对外输出航迹更新信息
|
||||
*Trust_track_num_Output= *Trust_track_num_Output+1;
|
||||
float r_output,azmi_output;
|
||||
double r_output,azmi_output;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar((*trust_track)[i][k].X[0],(*trust_track)[i][k].X[3],&r_output,&azmi_output);
|
||||
Coor_trans.cart2polar((*trust_track)[i].X[0],(*trust_track)[i].X[3],&r_output,&azmi_output);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i][k].Track_Index; //航迹号
|
||||
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][k].X[1]*(*trust_track)[i][k].X[1]+(*trust_track)[i][k].X[4]*(*trust_track)[i][k].X[4]);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i][k].Amplitude;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i][k].point_flag;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i][k].Track_Mode;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i][k].Height/r_output)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i][k].Height+Work_Parameter.Height;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i][k].Target_Type;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i][k].snr_point;
|
||||
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].Flag_Point=(*trust_track)[i].point_flag;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i].Height/r_output)/PI*180;
|
||||
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].Target_Type=(*trust_track)[i].Target_Type;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
|
||||
break;
|
||||
|
||||
}
|
||||
@@ -111,28 +109,28 @@ void TAS_Ctrl::tas_target_add(QVector <QVector<Trust_Track>> *trust_track,
|
||||
|
||||
|
||||
|
||||
int TAS_Ctrl:: tas_auto_start(float v, float r, float h, struct RadarPara Work_Parameter)
|
||||
{
|
||||
//int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter)
|
||||
//{
|
||||
|
||||
// if(r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
|
||||
// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) )
|
||||
// )
|
||||
if( r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
|
||||
&& ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) ) )
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
//// if(r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
|
||||
//// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) )
|
||||
//// )
|
||||
// if( r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
|
||||
// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) ) )
|
||||
// {
|
||||
// return 1;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// return 0;
|
||||
//// return 0;
|
||||
|
||||
};
|
||||
//};
|
||||
|
||||
|
||||
int TAS_Ctrl::tas_prohibite_area(float r,float azi, float v, float h,struct RadarPara Work_Parameter)
|
||||
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter)
|
||||
{
|
||||
|
||||
for (int i=0;i<Work_Parameter.TAS_prohibite_area_num;i++)
|
||||
@@ -148,7 +146,7 @@ int TAS_Ctrl::tas_prohibite_area(float r,float azi, float v, float h,struct Rad
|
||||
|
||||
}
|
||||
|
||||
void TAS_Ctrl::tas_target_del(QVector <QVector<Trust_Track>> *trust_track,
|
||||
void TAS_Ctrl::tas_target_del(QVector<Trust_Track> *trust_track,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter)
|
||||
@@ -162,9 +160,8 @@ void TAS_Ctrl::tas_target_del(QVector <QVector<Trust_Track>> *trust_track,
|
||||
//查找TAS队列里的目标是否存在
|
||||
int flag=0;
|
||||
for (int j=0;j<trust_track->size();j++)
|
||||
for (int k=0;k<(*trust_track)[j].size();k++)
|
||||
{
|
||||
if(tas_target_queue[i].Index==(*trust_track)[j][k].Track_Index && (*trust_track)[j][k].Track_Mode == 1)
|
||||
if(tas_target_queue[i].Index==(*trust_track)[j].Track_Index && (*trust_track)[j].Track_Mode == 1)
|
||||
{
|
||||
flag=1;
|
||||
}
|
||||
@@ -179,104 +176,102 @@ void TAS_Ctrl::tas_target_del(QVector <QVector<Trust_Track>> *trust_track,
|
||||
}
|
||||
|
||||
//判断TAS目标是否满足自动跟踪条件 若不满足移除TAS队列 通知界面改变目标状态
|
||||
for ( int i=0;i<trust_track->size();i++)
|
||||
for (int j=0;j<(*trust_track)[i].size();j++)
|
||||
{
|
||||
float r=sqrt(pow((*trust_track)[i][j].X[0],2)+pow((*trust_track)[i][j].X[3],2));
|
||||
float v=sqrt(pow((*trust_track)[i][j].X[1],2)+pow((*trust_track)[i][j].X[4],2));
|
||||
float h=(*trust_track)[i][j].Height+Work_Parameter.Height;
|
||||
// for ( int i=0;i<trust_track->size();i++)
|
||||
// {
|
||||
// double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
|
||||
// double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
|
||||
// double h=(*trust_track)[i].Height+Work_Parameter.Height;
|
||||
|
||||
if((*trust_track)[i][j].manual_tracking_flag == 0 && (*trust_track)[i][j].Track_Mode == 1 && tas_auto_end(v,r,h,Work_Parameter) == 1) //满足结束跟踪条件
|
||||
{
|
||||
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h,Work_Parameter) == 1) //满足结束跟踪条件
|
||||
// {
|
||||
|
||||
int track_index=(*trust_track)[i][j].Track_Index;
|
||||
// int track_index=(*trust_track)[i].Track_Index;
|
||||
|
||||
for (int ii=0;ii<TAS_QUEUE_LENGTH;ii++)
|
||||
{
|
||||
if(tas_target_queue[ii].Index == track_index)
|
||||
{
|
||||
//从队列中删除
|
||||
memset(&tas_target_queue[ii],0,sizeof(Tracking_Target));
|
||||
tas_target_num=tas_target_num-1;
|
||||
// for (int ii=0;ii<TAS_QUEUE_LENGTH;ii++)
|
||||
// {
|
||||
// if(tas_target_queue[ii].Index == track_index)
|
||||
// {
|
||||
// //从队列中删除
|
||||
// memset(&tas_target_queue[ii],0,sizeof(Tracking_Target));
|
||||
// tas_target_num=tas_target_num-1;
|
||||
|
||||
//TAS跟踪标志置零
|
||||
(*trust_track)[i][j].Track_Mode=0;
|
||||
// //TAS跟踪标志置零
|
||||
// (*trust_track)[i].Track_Mode=0;
|
||||
|
||||
//目标类型设为地面目标
|
||||
(*trust_track)[i][j].Target_Type = 0;
|
||||
// //目标类型设为地面目标
|
||||
// (*trust_track)[i].Target_Type = 0;
|
||||
|
||||
//航迹状态发生改变 对外输出航迹更新信息
|
||||
*Trust_track_num_Output= *Trust_track_num_Output+1;
|
||||
float r_output,azmi_output;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar((*trust_track)[i][j].X[0],(*trust_track)[i][j].X[3],&r_output,&azmi_output);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i][j].Track_Index; //航迹号
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=sqrt((*trust_track)[i][j].X[1]*(*trust_track)[i][j].X[1]+(*trust_track)[i][j].X[4]*(*trust_track)[i][j].X[4]);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i][j].Amplitude;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i][j].point_flag;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i][j].Track_Mode;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i][j].Height/r_output)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i][j].Height+Work_Parameter.Height;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i][j].Target_Type;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i][j].snr_point;
|
||||
}
|
||||
}
|
||||
}
|
||||
// //航迹状态发生改变 对外输出航迹更新信息
|
||||
// *Trust_track_num_Output= *Trust_track_num_Output+1;
|
||||
// double r_output,azmi_output;
|
||||
// coor_trans Coor_trans;
|
||||
// Coor_trans.cart2polar((*trust_track)[i].X[0],(*trust_track)[i].X[3],&r_output,&azmi_output);
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/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].Flag_Point=(*trust_track)[i].point_flag;
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=asin((*trust_track)[i].Height/r_output)/PI*180;
|
||||
// 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].Target_Type=(*trust_track)[i].Target_Type;
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
}
|
||||
// }
|
||||
|
||||
|
||||
};
|
||||
|
||||
int TAS_Ctrl::tas_auto_end(float v, float r, float h, struct RadarPara Work_Parameter)
|
||||
{
|
||||
if((r<=1000 && h<Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h<Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h<Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h<Work_Parameter.TAS_Height_4) || (r>4000 && h<Work_Parameter.TAS_Height_5)
|
||||
|| v<Work_Parameter.V_TAS_min || v>Work_Parameter.V_TAS_max
|
||||
)
|
||||
{
|
||||
return 1;
|
||||
//int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter)
|
||||
//{
|
||||
// if((r<=1000 && h<Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h<Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h<Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h<Work_Parameter.TAS_Height_4) || (r>4000 && h<Work_Parameter.TAS_Height_5)
|
||||
// || v<Work_Parameter.V_TAS_min || v>Work_Parameter.V_TAS_max
|
||||
// )
|
||||
// {
|
||||
// return 1;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
|
||||
return 0;
|
||||
}
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
return 0;
|
||||
// return 0;
|
||||
|
||||
};
|
||||
//};
|
||||
|
||||
|
||||
|
||||
void TAS_Ctrl::tas_beam_output(QVector <QVector<Trust_Track>> *trust_track,
|
||||
void TAS_Ctrl::tas_beam_output(QVector<Trust_Track> *trust_track,
|
||||
struct TrackingBeam *Tracking_beam)
|
||||
{
|
||||
if(tas_target_queue[0].empty_flag==1)
|
||||
{
|
||||
|
||||
float H_track;
|
||||
float X_now[6];
|
||||
double H_track;
|
||||
double X_now[6];
|
||||
for (int i=0;i<trust_track->size();i++ )
|
||||
for (int j=0;j<(*trust_track)[i].size();j++)
|
||||
{
|
||||
if((*trust_track)[i][j].Track_Index == tas_target_queue[0].Index)
|
||||
if((*trust_track)[i].Track_Index == tas_target_queue[0].Index)
|
||||
{
|
||||
H_track = (*trust_track)[i][j].Height;
|
||||
H_track = (*trust_track)[i].Height;
|
||||
for(int ii=0;ii<6;ii++)
|
||||
X_now[ii]=(*trust_track)[i][j].X[ii];
|
||||
X_now[ii]=(*trust_track)[i].X[ii];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//预测目标位置 计算跟踪波束波位号 俯仰角
|
||||
|
||||
float x_track=X_now[0]+X_now[1]*T_TAS_PRED;
|
||||
float y_track=X_now[3]+X_now[4]*T_TAS_PRED;
|
||||
float amzi,range;
|
||||
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 amzi,range;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(x_track,y_track,&range,&amzi);
|
||||
|
||||
@@ -286,7 +281,7 @@ void TAS_Ctrl::tas_beam_output(QVector <QVector<Trust_Track>> *trust_track,
|
||||
//目标方位
|
||||
Tracking_beam->Azi=amzi/PI*180;
|
||||
//目标俯仰角
|
||||
float elev=asin(H_track/range)/PI*180;
|
||||
double elev=asin(H_track/range)/PI*180;
|
||||
|
||||
|
||||
if(elev<=0)
|
||||
|
||||
@@ -14,7 +14,7 @@ public:
|
||||
|
||||
TAS_Ctrl();
|
||||
|
||||
void tas_ctrl_process( QVector <QVector<Trust_Track>> *trust_track,
|
||||
void tas_ctrl_process( QVector<Trust_Track> *trust_track,
|
||||
struct TrackingBeam *Tracking_beam,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
@@ -24,29 +24,29 @@ public:
|
||||
private:
|
||||
|
||||
//添加TAS目标
|
||||
void tas_target_add(QVector <QVector<Trust_Track>> *trust_track,
|
||||
void tas_target_add(QVector<Trust_Track> *trust_track,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter);
|
||||
|
||||
//删除TAS目标
|
||||
void tas_target_del(QVector <QVector<Trust_Track>> *trust_track,
|
||||
void tas_target_del(QVector<Trust_Track> *trust_track,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter);
|
||||
|
||||
//TAS队列信息输出
|
||||
void tas_beam_output(QVector <QVector<Trust_Track>> *trust_track,
|
||||
void tas_beam_output(QVector<Trust_Track> *trust_track,
|
||||
struct TrackingBeam *Tracking_beam);
|
||||
|
||||
//TAS 自动开启条件
|
||||
int tas_auto_start(float v, float r, float h, struct RadarPara Work_Parameter);
|
||||
int tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter);
|
||||
|
||||
//TAS 自动退出条件
|
||||
int tas_auto_end(float v, float r, float h, struct RadarPara Work_Parameter);
|
||||
int tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter);
|
||||
|
||||
// 禁止跟踪区域判断
|
||||
int tas_prohibite_area(float r,float azi, float v, float h,struct RadarPara Work_Parameter);
|
||||
int tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter);
|
||||
|
||||
struct Tracking_Target tas_target_queue[TAS_QUEUE_LENGTH]; //跟踪目标信息队列
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -6,7 +6,7 @@
|
||||
#include "parameters.h"
|
||||
#include "struct.h"
|
||||
#include <QVector>
|
||||
|
||||
#include <QDebug>
|
||||
using namespace std;
|
||||
|
||||
|
||||
@@ -15,8 +15,7 @@ using namespace std;
|
||||
class Track_Asso
|
||||
{
|
||||
public:
|
||||
int track_asso_process(QVector<PointRecv> *point_recv_1, //点迹文件
|
||||
QVector<PointRecv> *point_recv_2,
|
||||
int track_asso_process(QVector <PointRecv> *point_recv, //点迹文件
|
||||
QVector <Trust_Track> *trust_track, //航迹文件
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
@@ -41,18 +40,18 @@ private:
|
||||
|
||||
void model_output(QVector <Trust_Track> *trust_track ); //模型输出
|
||||
|
||||
void IMM_d_cal(float v_track,
|
||||
float X1[6], float P1[6][6],
|
||||
float X2[6], float P2[6][6],
|
||||
float X3[6], float P3[6][6],
|
||||
float Z[3], float prt,float freq_ind,
|
||||
float delta_T,
|
||||
float *d1,float *d2,float *d3,
|
||||
void 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);
|
||||
|
||||
void IMM_F_Q_gen(float v_track, float delta_T,float F[6][6], float Q1[6][6], float Q2[6][6], float Q3[6][6],struct RadarPara Work_Parameter);
|
||||
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter);
|
||||
|
||||
float Pt[3][3]; //模型转移概率
|
||||
double Pt[3][3]; //模型转移概率
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,724 @@
|
||||
#include "track_asso_direct_tracking.h"
|
||||
#include "kalman.h"
|
||||
#include "coor_trans.h"
|
||||
#include <qmath.h>
|
||||
#include "memory.h"
|
||||
#include <QVector>
|
||||
#include <Eigen/Dense>
|
||||
using namespace Eigen;
|
||||
using namespace std;
|
||||
|
||||
|
||||
int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector<PointRecv> *point_recv, //点迹
|
||||
Trust_Track *trust_track, //航迹
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
)
|
||||
{
|
||||
|
||||
//取出点迹
|
||||
for (int i=0;i< point_recv->size();i++)
|
||||
{
|
||||
point_process.push_back((*point_recv)[i]);
|
||||
}
|
||||
|
||||
//IMM算法
|
||||
model_interaction(trust_track);
|
||||
|
||||
model_filter(trust_track, Work_Parameter);
|
||||
|
||||
model_output(trust_track);
|
||||
|
||||
//point_process清空
|
||||
QVector<PointRecv>().swap(point_process);
|
||||
|
||||
|
||||
//输出航迹
|
||||
|
||||
*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).X[0]+(*trust_track).X[1]*Work_Parameter.Sys_delay;
|
||||
y=(*trust_track).X[3]+(*trust_track).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).Height/r)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track).Track_Index;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V =
|
||||
sqrt((*trust_track).X[1]*(*trust_track).X[1]+(*trust_track).X[4]*(*trust_track).X[4]);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = (*trust_track).Amplitude;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track).Track_Mode;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track).T_track/1000.0;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track).point_flag;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track).Height+Work_Parameter.Height;
|
||||
|
||||
double Direction_Angle;
|
||||
Direction_Angle=atan((*trust_track).X[4]/(*trust_track).X[1]);
|
||||
if((*trust_track).X[1]<0){Direction_Angle=Direction_Angle+PI;}
|
||||
if((*trust_track).X[1]>0&&(*trust_track).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).range_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track).azi_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track).elev_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track).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).prf_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track).snr_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track).X[0];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track).X[3];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track).X[1];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track).X[4];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*trust_track).RCS;
|
||||
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track) //模型交互
|
||||
{
|
||||
|
||||
double u_last[3];
|
||||
u_last[0]=(*trust_track).u[0];
|
||||
u_last[1]=(*trust_track).u[1];
|
||||
u_last[2]=(*trust_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];
|
||||
|
||||
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).X1[i];
|
||||
X2[i]=(*trust_track).X2[i];
|
||||
X3[i]=(*trust_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];
|
||||
}
|
||||
|
||||
|
||||
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++)
|
||||
{
|
||||
|
||||
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];
|
||||
}
|
||||
|
||||
|
||||
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).P1[i][j]+X11[i][j])*u_t[0][0]+((*trust_track).P2[i][j]+X21[i][j])*u_t[1][0]+((*trust_track).P3[i][j]+X31[i][j])*u_t[2][0];
|
||||
Po2_last[i][j]=((*trust_track).P1[i][j]+X12[i][j])*u_t[0][1]+((*trust_track).P2[i][j]+X22[i][j])*u_t[1][1]+((*trust_track).P3[i][j]+X32[i][j])*u_t[2][1];
|
||||
Po3_last[i][j]=((*trust_track).P1[i][j]+X13[i][j])*u_t[0][2]+((*trust_track).P2[i][j]+X23[i][j])*u_t[1][2]+((*trust_track).P3[i][j]+X33[i][j])*u_t[2][2];
|
||||
}
|
||||
|
||||
for(int i=0;i<6;i++)
|
||||
{
|
||||
(*trust_track).X1[i]=Xo1_last[i];
|
||||
(*trust_track).X2[i]=Xo2_last[i];
|
||||
(*trust_track).X3[i]=Xo3_last[i];
|
||||
}
|
||||
for(int i=0;i<6;i++)
|
||||
for (int j=0;j<6;j++)
|
||||
{
|
||||
(*trust_track).P1[i][j]=Po1_last[i][j];
|
||||
(*trust_track).P2[i][j]=Po2_last[i][j];
|
||||
(*trust_track).P3[i][j]=Po3_last[i][j];
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, //滤波
|
||||
struct RadarPara Work_Parameter)
|
||||
{
|
||||
//存储关联信息
|
||||
struct asso_info
|
||||
{
|
||||
double d1;
|
||||
double d2;
|
||||
double d3;
|
||||
double d_min;
|
||||
int point_index;
|
||||
};
|
||||
QVector <asso_info> associated_info;
|
||||
|
||||
|
||||
//航迹信息
|
||||
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
|
||||
double T_track;
|
||||
double v_track;
|
||||
double r_track;
|
||||
double h_track;
|
||||
(*trust_track).point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
|
||||
memcpy(X1,(*trust_track).X1,6*sizeof(double));
|
||||
memcpy(X2,(*trust_track).X2,6*sizeof(double));
|
||||
memcpy(X3,(*trust_track).X3,6*sizeof(double));
|
||||
memcpy(P1,(*trust_track).P1,6*6*sizeof(double));
|
||||
memcpy(P2,(*trust_track).P2,6*6*sizeof(double));
|
||||
memcpy(P3,(*trust_track).P3,6*6*sizeof(double));
|
||||
T_track = (*trust_track).T_track;
|
||||
v_track = sqrt(pow((*trust_track).X[1],2)+pow((*trust_track).X[4],2));
|
||||
r_track = sqrt(pow((*trust_track).X[0],2)+pow((*trust_track).X[3],2));
|
||||
h_track = (*trust_track).Height;
|
||||
|
||||
// 计算量测和航迹统计距离
|
||||
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[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;
|
||||
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;
|
||||
double h_point = point_process[loop_of_point].Height;
|
||||
|
||||
//计算统计距离d
|
||||
double d1,d2,d3;
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
|
||||
|
||||
//计算高度门限
|
||||
double d_h;
|
||||
if (r_track<1000)
|
||||
{
|
||||
if(abs(h_track-h_point)<=150)
|
||||
{
|
||||
d_h = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_h = 0;
|
||||
}
|
||||
|
||||
}
|
||||
else if (r_track<2000 && r_track>=1000)
|
||||
{
|
||||
if(abs(h_track-h_point)<=200)
|
||||
{
|
||||
d_h = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_h = 0;
|
||||
}
|
||||
|
||||
}
|
||||
else if(r_track>=2000 && r_track<3000 )
|
||||
{
|
||||
if(abs(h_track-h_point)<=200)
|
||||
{
|
||||
d_h = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_h = 0;
|
||||
}
|
||||
}
|
||||
else if(r_track>=3000 && r_track<5000)
|
||||
{
|
||||
if(abs(h_track-h_point)<=300)
|
||||
{
|
||||
d_h = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_h = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
d_h = 1;
|
||||
}
|
||||
|
||||
//小于关联门限 保存关联信息
|
||||
if((d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD)
|
||||
&& d_h == 1 )
|
||||
{
|
||||
struct asso_info associated_info_tmp;
|
||||
associated_info_tmp.d1=d1;
|
||||
associated_info_tmp.d2=d2;
|
||||
associated_info_tmp.d3=d3;
|
||||
associated_info_tmp.point_index=loop_of_point+1;
|
||||
if(d1<=d2&&d1<=d3)
|
||||
associated_info_tmp.d_min=d1;
|
||||
else if(d2<=d1&& d2<=d3)
|
||||
associated_info_tmp.d_min=d2;
|
||||
else
|
||||
associated_info_tmp.d_min=d3;
|
||||
|
||||
associated_info.push_back(associated_info_tmp);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//有关联 滤波
|
||||
if (associated_info.size()>0)
|
||||
{
|
||||
//找最近点
|
||||
int min_index=1;
|
||||
double min_d=associated_info[0].d_min;
|
||||
for (int ii=0;ii<associated_info.size();ii++)
|
||||
{
|
||||
if(associated_info[ii].d_min<min_d)
|
||||
{
|
||||
min_d=associated_info[ii].d_min;
|
||||
min_index=ii+1;
|
||||
}
|
||||
}
|
||||
int point_index=associated_info[min_index-1].point_index;
|
||||
double d1=associated_info[min_index-1].d1;
|
||||
double d2=associated_info[min_index-1].d2;
|
||||
double d3=associated_info[min_index-1].d3;
|
||||
|
||||
//卡尔曼滤波
|
||||
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 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);
|
||||
|
||||
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_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 );
|
||||
Matrix2f S1_out,S2_out,S3_out;
|
||||
for (int ii=0;ii<2;ii++)
|
||||
for (int jj=0;jj<2;jj++)
|
||||
{
|
||||
S1_out(ii,jj)=S1[ii][jj];
|
||||
S2_out(ii,jj)=S2[ii][jj];
|
||||
S3_out(ii,jj)=S3[ii][jj];
|
||||
}
|
||||
double det_S1=S1_out.determinant();
|
||||
double Possibility1=1/sqrt(2*PI*det_S1)*exp(-0.5*d1);
|
||||
double det_S2=S2_out.determinant();
|
||||
double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2);
|
||||
double det_S3=S3_out.determinant();
|
||||
double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3);
|
||||
|
||||
//更新航迹
|
||||
|
||||
//更新模型概率
|
||||
double u_last[3];
|
||||
u_last[0]=(*trust_track).u[0];
|
||||
u_last[1]=(*trust_track).u[1];
|
||||
u_last[2]=(*trust_track).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).u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
|
||||
(*trust_track).u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
|
||||
(*trust_track).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).X1[ii]=X1_filter[ii];
|
||||
(*trust_track).X2[ii]=X2_filter[ii];
|
||||
(*trust_track).X3[ii]=X3_filter[ii];
|
||||
}
|
||||
for(int ii=0;ii<6;ii++)
|
||||
for (int jj=0;jj<6;jj++)
|
||||
{
|
||||
(*trust_track).P1[ii][jj]=P1_filter[ii][jj];
|
||||
(*trust_track).P2[ii][jj]=P2_filter[ii][jj];
|
||||
(*trust_track).P3[ii][jj]=P3_filter[ii][jj];
|
||||
}
|
||||
|
||||
//更新航迹时间
|
||||
(*trust_track).T_track = point_process[point_index-1].CPI_Time;
|
||||
|
||||
|
||||
|
||||
//更新关联上的点迹信息
|
||||
(*trust_track).range_point=point_process[point_index-1].Range;
|
||||
(*trust_track).azi_point=point_process[point_index-1].Azimuth/PI*180;
|
||||
(*trust_track).elev_point= asin(point_process[point_index-1].Height/point_process[point_index-1].Range)/PI*180;
|
||||
(*trust_track).vr_point=point_process[point_index-1].Velocity;
|
||||
(*trust_track).point_type = 1;
|
||||
(*trust_track).prf_point = point_process[point_index-1].PRF_index;
|
||||
(*trust_track).snr_point = point_process[point_index-1].snr;
|
||||
(*trust_track).Extrapolate_round = 0; //连续未用实点更新时间
|
||||
(*trust_track).point_flag=1; //实点
|
||||
(*trust_track).Amplitude= point_process[point_index-1].Amplitude; //幅度
|
||||
(*trust_track).associate_point_number = (*trust_track).associate_point_number+1; //关联点数+1
|
||||
(*trust_track).RCS = point_process[point_index-1].RCS;
|
||||
|
||||
//更新高度
|
||||
track_hight_update(point_index,trust_track);
|
||||
}
|
||||
|
||||
|
||||
//未关联上，航迹外推
|
||||
else
|
||||
{
|
||||
double delta_T = DATA_RATE_TAS;
|
||||
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);
|
||||
|
||||
double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
|
||||
kalman Kalman;
|
||||
Kalman.kalman_pred(F, Q1, X1, P1, X1_pred, P1_pred);
|
||||
Kalman.kalman_pred(F, Q2, X2, P2, X2_pred, P2_pred);
|
||||
Kalman.kalman_pred(F, Q3, X3, P3, X3_pred, P3_pred);
|
||||
memcpy((*trust_track).X1,X1_pred,6*sizeof(double));
|
||||
memcpy((*trust_track).X2,X2_pred,6*sizeof(double));
|
||||
memcpy((*trust_track).X3,X3_pred,6*sizeof(double));
|
||||
memcpy((*trust_track).P1,P1_pred,6*6*sizeof(double));
|
||||
memcpy((*trust_track).P2,P2_pred,6*6*sizeof(double));
|
||||
memcpy((*trust_track).P3,P3_pred,6*6*sizeof(double));
|
||||
|
||||
(*trust_track).T_track = (*trust_track).T_track+delta_T*1000.0; //更新航迹时间
|
||||
(*trust_track).point_flag=0; //虚点
|
||||
(*trust_track).Extrapolate_round =(*trust_track).Extrapolate_round+1;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
void Track_Asso_Direct_Tracking::model_output(Trust_Track *trust_track)
|
||||
{
|
||||
|
||||
double u_now[3];
|
||||
u_now[0]=(*trust_track).u[0];
|
||||
u_now[1]=(*trust_track).u[1];
|
||||
u_now[2]=(*trust_track).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++)
|
||||
{
|
||||
X1_filter[ii]=(*trust_track).X1[ii];
|
||||
X2_filter[ii]=(*trust_track).X2[ii];
|
||||
X3_filter[ii]=(*trust_track).X3[ii];
|
||||
}
|
||||
for (int ii=0;ii<6;ii++)
|
||||
for(int jj=0;jj<6;jj++)
|
||||
{
|
||||
P1_filter[ii][jj]=(*trust_track).P1[ii][jj];
|
||||
P2_filter[ii][jj]=(*trust_track).P2[ii][jj];
|
||||
P3_filter[ii][jj]=(*trust_track).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 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++)
|
||||
{
|
||||
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]);
|
||||
|
||||
|
||||
//本地航迹文件更新
|
||||
(*trust_track).X[0]=X_filter[0]; //位置 速度
|
||||
(*trust_track).X[1]=X_filter[1];
|
||||
(*trust_track).X[2]=X_filter[2];
|
||||
(*trust_track).X[3]=X_filter[3];
|
||||
(*trust_track).X[4]=X_filter[4];
|
||||
(*trust_track).X[5]=X_filter[5];
|
||||
|
||||
for(int ii=0;ii<6;ii++) //协方差矩阵
|
||||
for (int jj=0;jj<6;jj++)
|
||||
(*trust_track).P[ii][jj]=P_filter[ii][jj];
|
||||
|
||||
|
||||
}
|
||||
|
||||
// 产生 F Q 矩阵
|
||||
void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter)
|
||||
{
|
||||
|
||||
//F
|
||||
double alpha=0.2;
|
||||
memset(F,0,36*sizeof(double));
|
||||
F[0][0]=1;
|
||||
F[0][1]=delta_T;
|
||||
F[0][2]=(alpha*delta_T-1+exp(-alpha*delta_T))/pow(alpha,2);
|
||||
F[1][0]=0;
|
||||
F[1][1]=1;
|
||||
F[1][2]=(1-exp(-alpha*delta_T))/alpha;
|
||||
F[2][0]=0;
|
||||
F[2][1]=0;
|
||||
F[2][2]=exp(-alpha*delta_T);
|
||||
|
||||
F[3][3]=1;
|
||||
F[3][4]=delta_T;
|
||||
F[3][5]=(alpha*delta_T-1+exp(-alpha*delta_T))/pow(alpha,2);
|
||||
F[4][3]=0;
|
||||
F[4][4]=1;
|
||||
F[4][5]=(1-exp(-alpha*delta_T))/alpha;
|
||||
F[5][3]=0;
|
||||
F[5][4]=0;
|
||||
F[5][5]=exp(-alpha*delta_T);
|
||||
|
||||
|
||||
|
||||
//模型1
|
||||
//Q
|
||||
double q1=Work_Parameter.Model1_Q_slow;
|
||||
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
||||
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
||||
double q221=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
|
||||
double q231=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
|
||||
double q331=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
|
||||
memset(Q1,0,36*sizeof(double));
|
||||
Q1[0][0]=q1*q111; Q1[0][1]=q1*q121; Q1[0][2]=q1*q131;
|
||||
Q1[1][0]=q1*q121; Q1[1][1]=q1*q221; Q1[1][2]=q1*q231;
|
||||
Q1[2][0]=q1*q131; Q1[2][1]=q1*q231; Q1[2][2]=q1*q331;
|
||||
|
||||
Q1[3][3]=q1*q111; Q1[3][4]=q1*q121; Q1[3][5]=q1*q131;
|
||||
Q1[4][3]=q1*q121; Q1[4][4]=q1*q221; Q1[4][5]=q1*q231;
|
||||
Q1[5][3]=q1*q131; Q1[5][4]=q1*q231; Q1[5][5]=q1*q331;
|
||||
|
||||
|
||||
//模型2
|
||||
//Q
|
||||
double q2;
|
||||
if(v_track<=5)
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_slow/20;
|
||||
}
|
||||
else if(v_track<=100&&v_track>50)
|
||||
{
|
||||
q2=10.0*Work_Parameter.Model2_Q_fast;
|
||||
}
|
||||
else
|
||||
{
|
||||
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));
|
||||
|
||||
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;
|
||||
|
||||
|
||||
//模型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;
|
||||
|
||||
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;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
// 计算三个 d
|
||||
void Track_Asso_Direct_Tracking::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 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_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
|
||||
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
|
||||
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
//高度维更新
|
||||
void Track_Asso_Direct_Tracking::track_hight_update(int asso_point_index, //点迹号
|
||||
Trust_Track *trust_track //航迹
|
||||
)
|
||||
{
|
||||
|
||||
(*trust_track).Hight_smooth.push_back(point_process[asso_point_index-1].Height);
|
||||
|
||||
|
||||
int height_win_length;
|
||||
if((*trust_track).range_point <= 1000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+2;
|
||||
}
|
||||
else if((*trust_track).range_point <= 2000 && (*trust_track).range_point > 1000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+3;
|
||||
}
|
||||
else if((*trust_track).range_point <= 4000 && (*trust_track).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).Hight_smooth.size()<height_win_length)
|
||||
{
|
||||
|
||||
for (int ii=0;ii<(*trust_track).Hight_smooth.size();ii++)
|
||||
{
|
||||
sum=sum+(*trust_track).Hight_smooth[ii];
|
||||
|
||||
}
|
||||
|
||||
(*trust_track).Height=sum/(*trust_track).Hight_smooth.size();
|
||||
|
||||
}
|
||||
else if((*trust_track).Hight_smooth.size()>=height_win_length)
|
||||
{
|
||||
|
||||
int N=(*trust_track).Hight_smooth.size();
|
||||
for (int ii=0;ii<height_win_length;ii++)
|
||||
{
|
||||
sum=sum+(*trust_track).Hight_smooth[N-1-ii];
|
||||
}
|
||||
(*trust_track).Height=sum/height_win_length;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Track_Asso_Direct_Tracking::Track_Asso_Direct_Tracking()
|
||||
{
|
||||
Pt[0][0]=0.8;Pt[0][1]=0.15;Pt[0][2]=0.05; //IMM 转移概率
|
||||
Pt[1][0]=0.3;Pt[1][1]=0.4;Pt[1][2]=0.3;
|
||||
Pt[2][0]=0.05;Pt[2][1]=0.15;Pt[2][2]=0.8;
|
||||
|
||||
|
||||
};
|
||||
@@ -0,0 +1,59 @@
|
||||
#ifndef TRACK_ASSO_DIRECT_TRACKING_H
|
||||
#define TRACK_ASSO_DIRECT_TRACKING_H
|
||||
#include "data_process_class_dll.h"
|
||||
#include "parameters.h"
|
||||
#include "struct.h"
|
||||
#include <QVector>
|
||||
using namespace std;
|
||||
|
||||
|
||||
class Track_Asso_Direct_Tracking
|
||||
{
|
||||
public:
|
||||
int track_asso_process_direct_tracking(QVector<PointRecv> *point_recv, //点迹
|
||||
Trust_Track *trust_track, //航迹
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
);
|
||||
|
||||
Track_Asso_Direct_Tracking();
|
||||
|
||||
|
||||
private:
|
||||
|
||||
//要处理的点迹
|
||||
QVector<PointRecv> point_process;
|
||||
|
||||
|
||||
//IMM
|
||||
void model_interaction(Trust_Track *trust_track); //模型交互
|
||||
|
||||
|
||||
void model_filter(Trust_Track *trust_track, //滤波
|
||||
struct RadarPara Work_Parameter
|
||||
);
|
||||
|
||||
void model_output(Trust_Track *trust_track); //模型输出
|
||||
|
||||
void 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);
|
||||
|
||||
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter);
|
||||
|
||||
double Pt[3][3]; //模型转移概率
|
||||
|
||||
//高度维更新
|
||||
void track_hight_update(int asso_point_index, //点迹号
|
||||
Trust_Track *trust_track //航迹
|
||||
);
|
||||
};
|
||||
|
||||
|
||||
#endif // TRACK_ASSO_DIRECT_TRACKING_H
|
||||
@@ -10,7 +10,7 @@ using namespace std;
|
||||
|
||||
|
||||
int Track_Asso_Tas::track_asso_process_tas(QVector<PointRecv> *point_recv_tas, //点迹文件
|
||||
QVector <QVector<Trust_Track>> *trust_track, //航迹文件
|
||||
QVector<Trust_Track> *trust_track, //航迹文件
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
struct RadarPara Work_Parameter, //工作参数
|
||||
@@ -39,53 +39,54 @@ int Track_Asso_Tas::track_asso_process_tas(QVector<PointRecv> *poi
|
||||
//输出航迹
|
||||
for (int i=0;i<trust_track->size();i++)
|
||||
{ // 输出更新航迹条件：
|
||||
for (int j=0;j<(*trust_track)[i].size();j++)
|
||||
{
|
||||
if((*trust_track)[i][j].Track_Index == tas_track_idx)
|
||||
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;
|
||||
float x,y;
|
||||
x=(*trust_track)[i][j].X[0]+(*trust_track)[i][j].X[1]*Work_Parameter.Sys_delay;
|
||||
y=(*trust_track)[i][j].X[3]+(*trust_track)[i][j].X[4]*Work_Parameter.Sys_delay;
|
||||
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;
|
||||
float r, azi;
|
||||
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][j].Height/r)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i][j].Track_Index;
|
||||
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][j].X[1]*(*trust_track)[i][j].X[1]+(*trust_track)[i][j].X[4]*(*trust_track)[i][j].X[4]);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = (*trust_track)[i][j].Amplitude;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i][j].Track_Mode;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track)[i][j].T_track/1000.0;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track)[i][j].point_flag;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i][j].Height+Work_Parameter.Height;
|
||||
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;
|
||||
|
||||
float Direction_Angle;
|
||||
Direction_Angle=atan((*trust_track)[i][j].X[4]/(*trust_track)[i][j].X[1]);
|
||||
if((*trust_track)[i][j].X[1]<0){Direction_Angle=Direction_Angle+PI;}
|
||||
if((*trust_track)[i][j].X[1]>0&&(*trust_track)[i][j].X[4]<0){Direction_Angle=Direction_Angle+2*PI;}
|
||||
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][j].range_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track)[i][j].azi_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track)[i][j].elev_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track)[i][j].vr_point;
|
||||
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][j].prf_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i][j].snr_point;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track)[i][j].X[0];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track)[i][j].X[3];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track)[i][j].X[1];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track)[i][j].X[4];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*trust_track)[i][j].RCS;
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
@@ -95,25 +96,31 @@ int Track_Asso_Tas::track_asso_process_tas(QVector<PointRecv> *poi
|
||||
|
||||
|
||||
//进行多模型交互
|
||||
void Track_Asso_Tas:: model_interaction(QVector <QVector<Trust_Track>> *trust_track, int tas_track_idx)
|
||||
void Track_Asso_Tas:: model_interaction( QVector<Trust_Track> *trust_track, int tas_track_idx)
|
||||
{
|
||||
|
||||
|
||||
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
|
||||
{
|
||||
|
||||
for (int loop_of_track_1=0;loop_of_track_1<(*trust_track)[loop_of_track].size();loop_of_track_1++)
|
||||
{
|
||||
|
||||
if((*trust_track)[loop_of_track][loop_of_track_1].Track_Index == tas_track_idx)
|
||||
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
|
||||
{
|
||||
|
||||
float u_last[3];
|
||||
u_last[0]=(*trust_track)[loop_of_track][loop_of_track_1].u[0];
|
||||
u_last[1]=(*trust_track)[loop_of_track][loop_of_track_1].u[1];
|
||||
u_last[2]=(*trust_track)[loop_of_track][loop_of_track_1].u[2];
|
||||
|
||||
float u_t[3][3];
|
||||
// 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_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];
|
||||
@@ -130,12 +137,12 @@ void Track_Asso_Tas:: model_interaction(QVector <QVector<Trust_Track>> *trust_tr
|
||||
u_t[2][2]=Pt[2][2]*u_last[2]/c[2];
|
||||
|
||||
|
||||
float Xo1_last[6],Xo2_last[6],Xo3_last[6],X1[6],X2[6],X3[6];
|
||||
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][loop_of_track_1].X1[i];
|
||||
X2[i]=(*trust_track)[loop_of_track][loop_of_track_1].X2[i];
|
||||
X3[i]=(*trust_track)[loop_of_track][loop_of_track_1].X3[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];
|
||||
}
|
||||
|
||||
|
||||
@@ -147,12 +154,12 @@ void Track_Asso_Tas:: model_interaction(QVector <QVector<Trust_Track>> *trust_tr
|
||||
}
|
||||
|
||||
|
||||
float X1_sub_Xo1[6],X2_sub_Xo1[6],X3_sub_Xo1[6];
|
||||
float X1_sub_Xo2[6],X2_sub_Xo2[6],X3_sub_Xo2[6];
|
||||
float X1_sub_Xo3[6],X2_sub_Xo3[6],X3_sub_Xo3[6];
|
||||
float X11[6][6],X21[6][6],X31[6][6];
|
||||
float X12[6][6],X22[6][6],X32[6][6];
|
||||
float 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++)
|
||||
@@ -186,35 +193,35 @@ void Track_Asso_Tas:: model_interaction(QVector <QVector<Trust_Track>> *trust_tr
|
||||
}
|
||||
|
||||
|
||||
float Po1_last[6][6],Po2_last[6][6],Po3_last[6][6];
|
||||
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][loop_of_track_1].P1[i][j]+X11[i][j])*u_t[0][0]+((*trust_track)[loop_of_track][loop_of_track_1].P2[i][j]+X21[i][j])*u_t[1][0]+((*trust_track)[loop_of_track][loop_of_track_1].P3[i][j]+X31[i][j])*u_t[2][0];
|
||||
Po2_last[i][j]=((*trust_track)[loop_of_track][loop_of_track_1].P1[i][j]+X12[i][j])*u_t[0][1]+((*trust_track)[loop_of_track][loop_of_track_1].P2[i][j]+X22[i][j])*u_t[1][1]+((*trust_track)[loop_of_track][loop_of_track_1].P3[i][j]+X32[i][j])*u_t[2][1];
|
||||
Po3_last[i][j]=((*trust_track)[loop_of_track][loop_of_track_1].P1[i][j]+X13[i][j])*u_t[0][2]+((*trust_track)[loop_of_track][loop_of_track_1].P2[i][j]+X23[i][j])*u_t[1][2]+((*trust_track)[loop_of_track][loop_of_track_1].P3[i][j]+X33[i][j])*u_t[2][2];
|
||||
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][loop_of_track_1].X1[i]=Xo1_last[i];
|
||||
(*trust_track)[loop_of_track][loop_of_track_1].X2[i]=Xo2_last[i];
|
||||
(*trust_track)[loop_of_track][loop_of_track_1].X3[i]=Xo3_last[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][loop_of_track_1].P1[i][j]=Po1_last[i][j];
|
||||
(*trust_track)[loop_of_track][loop_of_track_1].P2[i][j]=Po2_last[i][j];
|
||||
(*trust_track)[loop_of_track][loop_of_track_1].P3[i][j]=Po3_last[i][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 <QVector<Trust_Track>> *trust_track, //滤波
|
||||
void Track_Asso_Tas::model_filter( QVector<Trust_Track> *trust_track, //滤波
|
||||
struct RadarPara Work_Parameter,
|
||||
int tas_track_idx
|
||||
)
|
||||
@@ -222,10 +229,10 @@ void Track_Asso_Tas::model_filter(QVector <QVector<Trust_Track>> *trust_
|
||||
//存储关联信息
|
||||
struct asso_info
|
||||
{
|
||||
float d1;
|
||||
float d2;
|
||||
float d3;
|
||||
float d_min;
|
||||
double d1;
|
||||
double d2;
|
||||
double d3;
|
||||
double d_min;
|
||||
int point_index;
|
||||
};
|
||||
QVector <asso_info> associated_info;
|
||||
@@ -233,28 +240,35 @@ void Track_Asso_Tas::model_filter(QVector <QVector<Trust_Track>> *trust_
|
||||
|
||||
|
||||
//航迹信息
|
||||
float X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
|
||||
float T_track;
|
||||
float v_track;
|
||||
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
|
||||
double T_track;
|
||||
double v_track;
|
||||
double r_track;
|
||||
double h_track;
|
||||
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
|
||||
{
|
||||
for (int loop_of_track_1=0;loop_of_track_1<(*trust_track)[loop_of_track].size();loop_of_track_1++)
|
||||
{
|
||||
if((*trust_track)[loop_of_track][loop_of_track_1].Track_Index == tas_track_idx)
|
||||
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
|
||||
{
|
||||
|
||||
(*trust_track)[loop_of_track][loop_of_track_1].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
|
||||
memcpy(X1,(*trust_track)[loop_of_track][loop_of_track_1].X1,6*sizeof(float));
|
||||
memcpy(X2,(*trust_track)[loop_of_track][loop_of_track_1].X2,6*sizeof(float));
|
||||
memcpy(X3,(*trust_track)[loop_of_track][loop_of_track_1].X3,6*sizeof(float));
|
||||
memcpy(P1,(*trust_track)[loop_of_track][loop_of_track_1].P1,6*6*sizeof(float));
|
||||
memcpy(P2,(*trust_track)[loop_of_track][loop_of_track_1].P2,6*6*sizeof(float));
|
||||
memcpy(P3,(*trust_track)[loop_of_track][loop_of_track_1].P3,6*6*sizeof(float));
|
||||
// if(point_process.size()>0)
|
||||
// {
|
||||
// qDebug() << "model_filter point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
|
||||
// }
|
||||
|
||||
T_track = (*trust_track)[loop_of_track][loop_of_track_1].T_track;
|
||||
v_track = sqrt(pow((*trust_track)[loop_of_track][loop_of_track_1].X[1],2)+pow((*trust_track)[loop_of_track][loop_of_track_1].X[4],2));
|
||||
(*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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -263,23 +277,85 @@ void Track_Asso_Tas::model_filter(QVector <QVector<Trust_Track>> *trust_
|
||||
for (int loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
|
||||
{
|
||||
//量测信息
|
||||
// float Z[2]={point_process[loop_of_point].Range*cos(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)};
|
||||
float Z[3] = {point_process[loop_of_point].Range,
|
||||
double Z[3] = {point_process[loop_of_point].Range,
|
||||
point_process[loop_of_point].Azimuth,
|
||||
point_process[loop_of_point].Velocity};
|
||||
// float v_point = point_process[loop_of_point].Velocity;
|
||||
float prt = point_process[loop_of_point].PRF_index;
|
||||
float freq_ind = point_process[loop_of_point].Freq_index;
|
||||
float delta_T = (point_process[loop_of_point].CPI_Time - T_track)/1000.0;
|
||||
// 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;
|
||||
double h_point = point_process[loop_of_point].Height;
|
||||
|
||||
//计算统计距离d
|
||||
float d1,d2,d3;
|
||||
double d1,d2,d3;
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
|
||||
|
||||
//小于关联门限 保存关联信息
|
||||
if((d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD))
|
||||
//计算高度门限
|
||||
double d_h;
|
||||
if (r_track<1000)
|
||||
{
|
||||
if(abs(h_track-h_point)<=150)
|
||||
{
|
||||
d_h = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_h = 0;
|
||||
}
|
||||
|
||||
}
|
||||
else if (r_track<2000 && r_track>=1000)
|
||||
{
|
||||
if(abs(h_track-h_point)<=200)
|
||||
{
|
||||
d_h = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_h = 0;
|
||||
}
|
||||
|
||||
}
|
||||
else if(r_track>=2000 && r_track<3000 )
|
||||
{
|
||||
if(abs(h_track-h_point)<=200)
|
||||
{
|
||||
d_h = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_h = 0;
|
||||
}
|
||||
}
|
||||
else if(r_track>=3000 && r_track<5000)
|
||||
{
|
||||
if(abs(h_track-h_point)<=300)
|
||||
{
|
||||
d_h = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_h = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
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;
|
||||
|
||||
|
||||
//小于关联门限 保存关联信息
|
||||
if((d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD)
|
||||
)// && d_h == 1
|
||||
{
|
||||
|
||||
// qDebug() << "TAS asoooooo111" ;
|
||||
struct asso_info associated_info_tmp;
|
||||
associated_info_tmp.d1=d1;
|
||||
associated_info_tmp.d2=d2;
|
||||
@@ -299,9 +375,12 @@ void Track_Asso_Tas::model_filter(QVector <QVector<Trust_Track>> *trust_
|
||||
//有关联 滤波
|
||||
if (associated_info.size()>0)
|
||||
{
|
||||
|
||||
// qDebug() << "TAS asoooooo222" ;
|
||||
|
||||
//找最近点
|
||||
int min_index=1;
|
||||
float min_d=associated_info[0].d_min;
|
||||
double min_d=associated_info[0].d_min;
|
||||
for (int ii=0;ii<associated_info.size();ii++)
|
||||
{
|
||||
if(associated_info[ii].d_min<min_d)
|
||||
@@ -311,23 +390,23 @@ void Track_Asso_Tas::model_filter(QVector <QVector<Trust_Track>> *trust_
|
||||
}
|
||||
}
|
||||
int point_index=associated_info[min_index-1].point_index;
|
||||
float d1=associated_info[min_index-1].d1;
|
||||
float d2=associated_info[min_index-1].d2;
|
||||
float d3=associated_info[min_index-1].d3;
|
||||
double d1=associated_info[min_index-1].d1;
|
||||
double d2=associated_info[min_index-1].d2;
|
||||
double d3=associated_info[min_index-1].d3;
|
||||
|
||||
//卡尔曼滤波
|
||||
float Z[3]={point_process[point_index-1].Range,point_process[point_index-1].Azimuth,point_process[point_index-1].Velocity};
|
||||
float prt = point_process[point_index-1].PRF_index;
|
||||
float freq_ind = point_process[point_index-1].Freq_index;
|
||||
// float Z[2];
|
||||
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);
|
||||
float delta_T = (point_process[point_index-1].CPI_Time - T_track)/1000.0;
|
||||
float F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
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);
|
||||
|
||||
float X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
|
||||
float S1[2][2],S2[2][2],S3[2][2];
|
||||
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);
|
||||
@@ -343,101 +422,105 @@ void Track_Asso_Tas::model_filter(QVector <QVector<Trust_Track>> *trust_
|
||||
S2_out(ii,jj)=S2[ii][jj];
|
||||
S3_out(ii,jj)=S3[ii][jj];
|
||||
}
|
||||
float det_S1=S1_out.determinant();
|
||||
float Possibility1=1/sqrt(2*PI*det_S1)*exp(-0.5*d1);
|
||||
float det_S2=S2_out.determinant();
|
||||
float Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2);
|
||||
float det_S3=S3_out.determinant();
|
||||
float Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3);
|
||||
double det_S1=S1_out.determinant();
|
||||
double Possibility1=1/sqrt(2*PI*det_S1)*exp(-0.5*d1);
|
||||
double det_S2=S2_out.determinant();
|
||||
double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2);
|
||||
double det_S3=S3_out.determinant();
|
||||
double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3);
|
||||
|
||||
//更新航迹
|
||||
for(int i=0;i<trust_track->size();i++)
|
||||
for (int j=0;j<(*trust_track)[i].size();j++)
|
||||
{
|
||||
if((*trust_track)[i][j].Track_Index == tas_track_idx)
|
||||
|
||||
if((*trust_track)[i].Track_Index == tas_track_idx)
|
||||
{
|
||||
//更新模型概率
|
||||
float u_last[3];
|
||||
u_last[0]=(*trust_track)[i][j].u[0];
|
||||
u_last[1]=(*trust_track)[i][j].u[1];
|
||||
u_last[2]=(*trust_track)[i][j].u[2];
|
||||
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];
|
||||
|
||||
float c[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];
|
||||
|
||||
(*trust_track)[i][j].u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
|
||||
(*trust_track)[i][j].u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]);
|
||||
(*trust_track)[i][j].u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[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][j].X1[ii]=X1_filter[ii];
|
||||
(*trust_track)[i][j].X2[ii]=X2_filter[ii];
|
||||
(*trust_track)[i][j].X3[ii]=X3_filter[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][j].P1[ii][jj]=P1_filter[ii][jj];
|
||||
(*trust_track)[i][j].P2[ii][jj]=P2_filter[ii][jj];
|
||||
(*trust_track)[i][j].P3[ii][jj]=P3_filter[ii][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][j].T_track = point_process[point_index-1].CPI_Time;
|
||||
(*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][j].range_point=point_process[point_index-1].Range;
|
||||
(*trust_track)[i][j].azi_point=point_process[point_index-1].Azimuth/PI*180;
|
||||
(*trust_track)[i][j].elev_point= asin(point_process[point_index-1].Height/point_process[point_index-1].Range)/PI*180;
|
||||
(*trust_track)[i][j].vr_point=point_process[point_index-1].Velocity;
|
||||
(*trust_track)[i][j].point_type = 1;
|
||||
(*trust_track)[i][j].prf_point = point_process[point_index-1].PRF_index;
|
||||
(*trust_track)[i][j].snr_point = point_process[point_index-1].snr;
|
||||
(*trust_track)[i][j].Extrapolate_round = 0; //连续未用实点更新时间
|
||||
(*trust_track)[i][j].point_flag=1; //实点
|
||||
(*trust_track)[i][j].Amplitude= point_process[point_index-1].Amplitude; //幅度
|
||||
(*trust_track)[i][j].associate_point_number = (*trust_track)[i][j].associate_point_number+1; //关联点数+1
|
||||
(*trust_track)[i][j].RCS = point_process[point_index-1].RCS;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
//未关联上，航迹外推
|
||||
else
|
||||
{
|
||||
|
||||
for(int i=0;i<trust_track->size();i++)
|
||||
for (int j=0;j<(*trust_track)[i].size();j++)
|
||||
if((*trust_track)[i].Track_Index == tas_track_idx)
|
||||
{
|
||||
if((*trust_track)[i][j].Track_Index == tas_track_idx)
|
||||
{
|
||||
float delta_T = DATA_RATE_TAS;
|
||||
float 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);
|
||||
double delta_T = DATA_RATE_TAS;
|
||||
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);
|
||||
|
||||
float X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
|
||||
kalman Kalman;
|
||||
Kalman.kalman_pred(F, Q1, X1, P1, X1_pred, P1_pred);
|
||||
Kalman.kalman_pred(F, Q2, X2, P2, X2_pred, P2_pred);
|
||||
Kalman.kalman_pred(F, Q3, X3, P3, X3_pred, P3_pred);
|
||||
memcpy((*trust_track)[i][j].X1,X1_pred,6*sizeof(float));
|
||||
memcpy((*trust_track)[i][j].X2,X2_pred,6*sizeof(float));
|
||||
memcpy((*trust_track)[i][j].X3,X3_pred,6*sizeof(float));
|
||||
memcpy((*trust_track)[i][j].P1,P1_pred,6*6*sizeof(float));
|
||||
memcpy((*trust_track)[i][j].P2,P2_pred,6*6*sizeof(float));
|
||||
memcpy((*trust_track)[i][j].P3,P3_pred,6*6*sizeof(float));
|
||||
double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
|
||||
kalman Kalman;
|
||||
Kalman.kalman_pred(F, Q1, X1, P1, X1_pred, P1_pred);
|
||||
Kalman.kalman_pred(F, Q2, X2, P2, X2_pred, P2_pred);
|
||||
Kalman.kalman_pred(F, Q3, X3, P3, X3_pred, P3_pred);
|
||||
memcpy((*trust_track)[i].X1,X1_pred,6*sizeof(double));
|
||||
memcpy((*trust_track)[i].X2,X2_pred,6*sizeof(double));
|
||||
memcpy((*trust_track)[i].X3,X3_pred,6*sizeof(double));
|
||||
memcpy((*trust_track)[i].P1,P1_pred,6*6*sizeof(double));
|
||||
memcpy((*trust_track)[i].P2,P2_pred,6*6*sizeof(double));
|
||||
memcpy((*trust_track)[i].P3,P3_pred,6*6*sizeof(double));
|
||||
|
||||
(*trust_track)[i][j].T_track = (*trust_track)[i][j].T_track+delta_T*1000.0; //更新航迹时间
|
||||
(*trust_track)[i][j].point_flag=0; //虚点
|
||||
(*trust_track)[i][j].Extrapolate_round =(*trust_track)[i][j].Extrapolate_round+1;
|
||||
}
|
||||
(*trust_track)[i].T_track = (*trust_track)[i].T_track+delta_T*1000.0; //更新航迹时间
|
||||
(*trust_track)[i].point_flag=0; //虚点
|
||||
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -446,41 +529,39 @@ void Track_Asso_Tas::model_filter(QVector <QVector<Trust_Track>> *trust_
|
||||
|
||||
|
||||
|
||||
void Track_Asso_Tas::model_output(QVector <QVector<Trust_Track>> *trust_track, int tas_track_idx )
|
||||
void Track_Asso_Tas::model_output(QVector<Trust_Track> *trust_track, int tas_track_idx )
|
||||
{
|
||||
for(int i=0; i<trust_track->size(); i++)
|
||||
for (int j=0;j<(*trust_track)[i].size();j++)
|
||||
{
|
||||
if((*trust_track)[i][j].Track_Index==tas_track_idx)
|
||||
if((*trust_track)[i].Track_Index==tas_track_idx)
|
||||
{
|
||||
|
||||
float u_now[3];
|
||||
u_now[0]=(*trust_track)[i][j].u[0];
|
||||
u_now[1]=(*trust_track)[i][j].u[1];
|
||||
u_now[2]=(*trust_track)[i][j].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];
|
||||
|
||||
float X1_filter[6],X2_filter[6],X3_filter[6];
|
||||
float P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
|
||||
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][j].X1[ii];
|
||||
X2_filter[ii]=(*trust_track)[i][j].X2[ii];
|
||||
X3_filter[ii]=(*trust_track)[i][j].X3[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++)
|
||||
{
|
||||
P1_filter[ii][jj]=(*trust_track)[i][j].P1[ii][jj];
|
||||
P2_filter[ii][jj]=(*trust_track)[i][j].P2[ii][jj];
|
||||
P3_filter[ii][jj]=(*trust_track)[i][j].P3[ii][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];
|
||||
}
|
||||
|
||||
float X_filter[6], P_filter[6][6];
|
||||
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];
|
||||
|
||||
float X1_sub_X[6], X2_sub_X[6], X3_sub_X[6];
|
||||
float X1X[6][6], X2X[6][6],X3X[6][6];
|
||||
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];
|
||||
@@ -500,65 +581,37 @@ void Track_Asso_Tas::model_output(QVector <QVector<Trust_Track>> *trust_track
|
||||
|
||||
|
||||
//本地航迹文件更新
|
||||
(*trust_track)[i][j].X[0]=X_filter[0]; //位置 速度
|
||||
(*trust_track)[i][j].X[1]=X_filter[1];
|
||||
(*trust_track)[i][j].X[2]=X_filter[2];
|
||||
(*trust_track)[i][j].X[3]=X_filter[3];
|
||||
(*trust_track)[i][j].X[4]=X_filter[4];
|
||||
(*trust_track)[i][j].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][j].P[ii][jj]=P_filter[ii][jj];
|
||||
(*trust_track)[i].P[ii][jj]=P_filter[ii][jj];
|
||||
|
||||
//航迹区更新
|
||||
float r, azi;
|
||||
double r, azi;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar((*trust_track)[i][j].X[0], (*trust_track)[i][j].X[3], &r, &azi);
|
||||
Coor_trans.cart2polar((*trust_track)[i].X[0], (*trust_track)[i].X[3], &r, &azi);
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_1_START && azi/PI*180<TRACK_SECTION_2_START)
|
||||
(*trust_track)[i][j].Track_section_idx = 1;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_2_START && azi/PI*180<TRACK_SECTION_3_START)
|
||||
(*trust_track)[i][j].Track_section_idx = 2;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_3_START && azi/PI*180<TRACK_SECTION_4_START)
|
||||
(*trust_track)[i][j].Track_section_idx = 3;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_4_START && azi/PI*180<TRACK_SECTION_5_START)
|
||||
(*trust_track)[i][j].Track_section_idx = 4;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_5_START && azi/PI*180<TRACK_SECTION_6_START)
|
||||
(*trust_track)[i][j].Track_section_idx = 5;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_6_START && azi/PI*180<TRACK_SECTION_7_START)
|
||||
(*trust_track)[i][j].Track_section_idx = 6;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_7_START && azi/PI*180<TRACK_SECTION_8_START)
|
||||
(*trust_track)[i][j].Track_section_idx = 7;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_8_START && azi/PI*180<TRACK_SECTION_9_START)
|
||||
(*trust_track)[i][j].Track_section_idx = 8;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_9_START && azi/PI*180<TRACK_SECTION_1_START)
|
||||
(*trust_track)[i][j].Track_section_idx = 9;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// 产生 F Q 矩阵
|
||||
void Track_Asso_Tas::IMM_F_Q_gen(float v_track, float delta_T,float F[6][6], float Q1[6][6], float Q2[6][6], float Q3[6][6],struct RadarPara Work_Parameter)
|
||||
void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter)
|
||||
{
|
||||
|
||||
//F
|
||||
float alpha=0.2;
|
||||
memset(F,0,36*sizeof(float));
|
||||
double alpha=0.2;
|
||||
memset(F,0,36*sizeof(double));
|
||||
F[0][0]=1;
|
||||
F[0][1]=delta_T;
|
||||
F[0][2]=(alpha*delta_T-1+exp(-alpha*delta_T))/pow(alpha,2);
|
||||
@@ -583,14 +636,14 @@ void Track_Asso_Tas::IMM_F_Q_gen(float v_track, float delta_T,float F[6][6], flo
|
||||
|
||||
//模型1
|
||||
//Q
|
||||
float q1=Work_Parameter.Model1_Q_slow;
|
||||
float q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
float q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
||||
float q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
||||
float q221=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
|
||||
float q231=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
|
||||
float q331=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
|
||||
memset(Q1,0,36*sizeof(float));
|
||||
double q1=Work_Parameter.Model1_Q_slow;
|
||||
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
||||
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
||||
double q221=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
|
||||
double q231=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
|
||||
double q331=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
|
||||
memset(Q1,0,36*sizeof(double));
|
||||
Q1[0][0]=q1*q111; Q1[0][1]=q1*q121; Q1[0][2]=q1*q131;
|
||||
Q1[1][0]=q1*q121; Q1[1][1]=q1*q221; Q1[1][2]=q1*q231;
|
||||
Q1[2][0]=q1*q131; Q1[2][1]=q1*q231; Q1[2][2]=q1*q331;
|
||||
@@ -602,7 +655,7 @@ void Track_Asso_Tas::IMM_F_Q_gen(float v_track, float delta_T,float F[6][6], flo
|
||||
|
||||
//模型2
|
||||
//Q
|
||||
float q2;
|
||||
double q2;
|
||||
if(v_track<=5)
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_slow/20;
|
||||
@@ -613,17 +666,17 @@ void Track_Asso_Tas::IMM_F_Q_gen(float v_track, float delta_T,float F[6][6], flo
|
||||
}
|
||||
else
|
||||
{
|
||||
q2=20.0*Work_Parameter.Model2_Q_fast;
|
||||
q2=Work_Parameter.Model2_Q_fast;
|
||||
}
|
||||
|
||||
float 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));
|
||||
float 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));
|
||||
float q132=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
||||
float q222=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
|
||||
float q232=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
|
||||
float 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(float));
|
||||
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;
|
||||
@@ -635,7 +688,7 @@ void Track_Asso_Tas::IMM_F_Q_gen(float v_track, float delta_T,float F[6][6], flo
|
||||
|
||||
//模型3
|
||||
//Q
|
||||
float q3;
|
||||
double q3;
|
||||
if(v_track<=5)
|
||||
{
|
||||
q3=Work_Parameter.Model3_Q_slow/20;
|
||||
@@ -646,17 +699,17 @@ void Track_Asso_Tas::IMM_F_Q_gen(float v_track, float delta_T,float F[6][6], flo
|
||||
}
|
||||
else
|
||||
{
|
||||
q3=20.0*Work_Parameter.Model3_Q_fast;
|
||||
q3=Work_Parameter.Model3_Q_fast;
|
||||
}
|
||||
float 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));
|
||||
float 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));
|
||||
float q133=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
||||
float q223=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2*alpha*delta_T);
|
||||
float q233=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T));
|
||||
float q333=1/(2*alpha)*(1-exp(-2*alpha*delta_T));
|
||||
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(float));
|
||||
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;
|
||||
@@ -670,16 +723,16 @@ void Track_Asso_Tas::IMM_F_Q_gen(float v_track, float delta_T,float F[6][6], flo
|
||||
|
||||
|
||||
// 计算三个 d
|
||||
void Track_Asso_Tas::IMM_d_cal(float v_track,
|
||||
float X1[6], float P1[6][6],
|
||||
float X2[6], float P2[6][6],
|
||||
float X3[6], float P3[6][6],
|
||||
float Z[3], float prt,float freq_ind,
|
||||
float delta_T,
|
||||
float *d1,float *d2,float *d3,
|
||||
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)
|
||||
{
|
||||
float F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
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;
|
||||
@@ -697,42 +750,60 @@ void Track_Asso_Tas::IMM_d_cal(float v_track,
|
||||
//高度维更新
|
||||
void Track_Asso_Tas::track_hight_update(int tas_track_idx, //更新的航迹号
|
||||
int asso_point_index, //点迹号
|
||||
QVector <QVector<Trust_Track>> *trust_track //航迹
|
||||
QVector<Trust_Track> *trust_track //航迹
|
||||
)
|
||||
{
|
||||
for(int i=0; i<trust_track->size(); i++)
|
||||
for (int j=0;j<(*trust_track)[i].size();j++)
|
||||
{
|
||||
if((*trust_track)[i][j].Track_Index==tas_track_idx)
|
||||
if((*trust_track)[i].Track_Index==tas_track_idx)
|
||||
{
|
||||
(*trust_track)[i][j].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);
|
||||
|
||||
float sum=0;
|
||||
if((*trust_track)[i][j].Hight_smooth.size()<H_F_WIN_LEN)
|
||||
|
||||
int height_win_length;
|
||||
if((*trust_track)[i].range_point <= 1000)
|
||||
{
|
||||
|
||||
for (int ii=0;ii<(*trust_track)[i][j].Hight_smooth.size();ii++)
|
||||
{
|
||||
sum=sum+(*trust_track)[i][j].Hight_smooth[ii];
|
||||
|
||||
}
|
||||
|
||||
(*trust_track)[i][j].Height=sum/(*trust_track)[i][j].Hight_smooth.size();
|
||||
|
||||
height_win_length = H_F_WIN_LEN+2;
|
||||
}
|
||||
else if((*trust_track)[i][j].Hight_smooth.size()>=H_F_WIN_LEN)
|
||||
else if((*trust_track)[i].range_point <= 2000 && (*trust_track)[i].range_point > 1000)
|
||||
{
|
||||
|
||||
int N=(*trust_track)[i][j].Hight_smooth.size();
|
||||
for (int ii=0;ii<H_F_WIN_LEN;ii++)
|
||||
{
|
||||
sum=sum+(*trust_track)[i][j].Hight_smooth[N-1-ii];
|
||||
}
|
||||
(*trust_track)[i][j].Height=sum/H_F_WIN_LEN;
|
||||
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)
|
||||
{
|
||||
|
||||
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();
|
||||
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
#include "parameters.h"
|
||||
#include "struct.h"
|
||||
#include <QVector>
|
||||
#include <QDebug>
|
||||
using namespace std;
|
||||
|
||||
|
||||
@@ -12,7 +13,7 @@ class Track_Asso_Tas
|
||||
{
|
||||
public:
|
||||
int track_asso_process_tas(QVector<PointRecv> *point_recv_tas, //点迹文件
|
||||
QVector <QVector<Trust_Track>> *trust_track, //航迹文件
|
||||
QVector<Trust_Track> *trust_track, //航迹文件
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
struct RadarPara Work_Parameter, //工作参数
|
||||
@@ -28,35 +29,33 @@ private:
|
||||
|
||||
|
||||
//IMM
|
||||
void model_interaction(QVector <QVector<Trust_Track>> *trust_track, int tas_track_idx); //模型交互
|
||||
void model_interaction(QVector<Trust_Track> *trust_track, int tas_track_idx); //模型交互
|
||||
|
||||
|
||||
void model_filter(QVector <QVector<Trust_Track>> *trust_track, //滤波
|
||||
void model_filter(QVector<Trust_Track> *trust_track, //滤波
|
||||
struct RadarPara Work_Parameter,
|
||||
int tas_track_idx
|
||||
);
|
||||
|
||||
void model_output(QVector <QVector<Trust_Track>> *trust_track, int tas_track_idx ); //模型输出
|
||||
void model_output(QVector<Trust_Track> *trust_track, int tas_track_idx ); //模型输出
|
||||
|
||||
void IMM_d_cal(float v_track,
|
||||
float X1[6], float P1[6][6],
|
||||
float X2[6], float P2[6][6],
|
||||
float X3[6], float P3[6][6],
|
||||
float Z[3], float prt,float freq_ind,
|
||||
float delta_T,
|
||||
float *d1,float *d2,float *d3,
|
||||
void 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);
|
||||
|
||||
void IMM_F_Q_gen(float v_track, float delta_T,float F[6][6], float Q1[6][6], float Q2[6][6], float Q3[6][6],struct RadarPara Work_Parameter);
|
||||
|
||||
float Pt[3][3]; //模型转移概率
|
||||
|
||||
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter);
|
||||
|
||||
double Pt[3][3]; //模型转移概率
|
||||
|
||||
//高度维更新
|
||||
void track_hight_update(int tas_track_index, //更新的航迹号
|
||||
int asso_point_index, //点迹号
|
||||
QVector <QVector<Trust_Track>> *trust_track //航迹
|
||||
QVector<Trust_Track> *trust_track //航迹
|
||||
);
|
||||
|
||||
|
||||
|
||||
@@ -16,8 +16,10 @@ void Track_Die::track_die_process( QVector <Trust_Track> *trust_track,
|
||||
for (Iter=trust_track->begin(); Iter!=trust_track->end();)
|
||||
{
|
||||
if( ((*Iter).Track_Mode == 0 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND)
|
||||
||((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND_TAS)
|
||||
// ||((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND_TAS)
|
||||
||(*Iter).manual_delete_flag == 1)
|
||||
|
||||
|
||||
{
|
||||
//输出消亡信息
|
||||
*Track_die_num_Output=*Track_die_num_Output+1;
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
#include "track_die_tas.h"
|
||||
#include "kalman.h"
|
||||
#include "coor_trans.h"
|
||||
#include <qmath.h>
|
||||
#include"memory.h"
|
||||
#include <QVector>
|
||||
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)
|
||||
{
|
||||
|
||||
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)
|
||||
|
||||
|
||||
{
|
||||
//输出消亡信息
|
||||
*Track_die_num_Output=*Track_die_num_Output+1;
|
||||
Track_die_Index_Output[*Track_die_num_Output-1]=(*Iter).Track_Index;
|
||||
|
||||
trust_track->erase(Iter);
|
||||
Iter=trust_track->begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter++;
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
@@ -0,0 +1,22 @@
|
||||
#ifndef TRACK_DIE_TAS_H
|
||||
#define TRACK_DIE_TAS_H
|
||||
#include "data_process_class_dll.h"
|
||||
#include "parameters.h"
|
||||
#include "struct.h"
|
||||
#include <QVector>
|
||||
using namespace std;
|
||||
|
||||
|
||||
|
||||
class Track_Die_Tas
|
||||
{
|
||||
public:
|
||||
void track_die_process_tas( QVector <Trust_Track> *trust_track,
|
||||
int Track_die_Index_Output[],
|
||||
int *Track_die_num_Output);
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
#endif // TRACK_DIE_TAS_H
|
||||
@@ -7,21 +7,18 @@ using namespace std;
|
||||
|
||||
|
||||
|
||||
int Track_Ind_Mangement ::track_ind_get(QVector <QVector<Trust_Track>> *trust_track)
|
||||
int Track_Ind_Mangement ::track_ind_get( QVector<Trust_Track> *trust_track)
|
||||
{
|
||||
|
||||
int isempty = 1;
|
||||
|
||||
for (int i = 0; i < trust_track->size(); i++)
|
||||
if((*trust_track).size()>0)
|
||||
{
|
||||
if((*trust_track)[i].size()>0)
|
||||
{
|
||||
|
||||
isempty = 0;
|
||||
}
|
||||
isempty = 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (isempty == 1)
|
||||
{
|
||||
lastest_index = 1;
|
||||
@@ -32,10 +29,9 @@ int Track_Ind_Mangement ::track_ind_get(QVector <QVector<Trust_Track>> *trust_tr
|
||||
//建立航迹号列表
|
||||
int List[MAX_TRACK_INDEX]={0};
|
||||
for (int i=0;i<trust_track->size();i++)
|
||||
for (int j=0;j<(*trust_track)[i].size();j++)
|
||||
{
|
||||
List[(*trust_track)[i][j].Track_Index-1]=1;
|
||||
}
|
||||
{
|
||||
List[(*trust_track)[i].Track_Index-1]=1;
|
||||
}
|
||||
|
||||
//分配航迹号
|
||||
if(lastest_index<MAX_TRACK_INDEX)
|
||||
@@ -48,6 +44,17 @@ int Track_Ind_Mangement ::track_ind_get(QVector <QVector<Trust_Track>> *trust_tr
|
||||
return lastest_index;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i=1;i<MAX_TRACK_INDEX;i++)
|
||||
{
|
||||
if(List[i-1]==0)
|
||||
{
|
||||
lastest_index = i;
|
||||
return lastest_index;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
else if(lastest_index == MAX_TRACK_INDEX)
|
||||
{
|
||||
|
||||
@@ -12,7 +12,7 @@ class Track_Ind_Mangement
|
||||
{
|
||||
public:
|
||||
|
||||
int track_ind_get(QVector <QVector<Trust_Track>> *trust_track);
|
||||
int track_ind_get( QVector<Trust_Track> *trust_track);
|
||||
|
||||
|
||||
private:
|
||||
|
||||
@@ -10,7 +10,7 @@ using namespace std;
|
||||
|
||||
|
||||
int Track_Init::track_init_process_logic( QVector <PointRecv> *point_recv, //输入点迹
|
||||
QVector <QVector<Trust_Track>> *trust_track, //可靠航迹
|
||||
QVector<Trust_Track> *trust_track, //可靠航迹
|
||||
QVector <QVector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
@@ -19,6 +19,23 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *poi
|
||||
|
||||
{
|
||||
|
||||
// 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)
|
||||
// {
|
||||
// qDebug() << "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 (int i=0;i<(*point_recv).size();i++)
|
||||
point_process.push_back((*point_recv)[i]);
|
||||
@@ -73,6 +90,9 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *poi
|
||||
temp_track_tmp.vr = point_process[i].Velocity;
|
||||
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_num;
|
||||
std::memcpy(temp_track_tmp.speed_dim, point_process[i].speed_dim, sizeof(temp_track_tmp.speed_dim));
|
||||
std::memcpy(temp_track_tmp.range_dim, point_process[i].range_dim, sizeof(temp_track_tmp.range_dim));
|
||||
temp_track_tmp.asso_flag = 0;
|
||||
temp_track_tmp.X[0]=point_process[i].Range*cos(point_process[i].Azimuth);
|
||||
temp_track_tmp.X[1]=0;
|
||||
@@ -80,11 +100,22 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *poi
|
||||
temp_track_tmp.X[3]=0;
|
||||
temp_track_tmp.T = point_process[i].CPI_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;
|
||||
// temp_track_tmp.Temp_track_section_idx = floor(point_process[i].beam_index/BEAM_NUM_DOT_SECTION)+1;
|
||||
temp_track->push_back( QVector <Temp_track> ());
|
||||
int n=temp_track->size();
|
||||
(*temp_track)[n-1].push_back(temp_track_tmp);
|
||||
|
||||
|
||||
// if(point_process[i].CPI_Time == 30675948 || point_process[i].CPI_Time == 30682320)
|
||||
// {
|
||||
// qDebug() << "point_process_azi=" << point_process[i].Azimuth
|
||||
// << "point_process_dis=" << point_process[i].Range
|
||||
// << "point_process_h=" << point_process[i].Height
|
||||
// << "point_process_v=" << point_process[i].Velocity
|
||||
// << "point_process_wave=" << point_process[i].beam_index
|
||||
// << "point_process_time=" << point_process[i].CPI_Time;
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
//清空点迹
|
||||
@@ -104,6 +135,20 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *poi
|
||||
tmp_track_die(temp_track);
|
||||
|
||||
|
||||
// qDebug() << "-----------------------------------------------";
|
||||
// for(int i=0;i<temp_track->size();i++)
|
||||
// {
|
||||
// for(int j=0;j<(*temp_track)[i].size();j++)
|
||||
// {
|
||||
// qDebug() << "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;
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
@@ -120,6 +165,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
|
||||
{
|
||||
int track_idx;
|
||||
int point_idx;
|
||||
double d;
|
||||
};
|
||||
QVector <Asso_info> asso_info;
|
||||
|
||||
@@ -129,56 +175,74 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
|
||||
{
|
||||
int L = (*temp_track)[j].size();
|
||||
|
||||
if( (*temp_track)[j].size() > 1 && (*temp_track)[j][L-1].buff_round == 2)
|
||||
if( (*temp_track)[j].size() > 1 && (*temp_track)[j][L-1].buff_round >= 2)
|
||||
{
|
||||
|
||||
//点迹信息
|
||||
// float Z[2]={ point_process[i].Range*cos(point_process[i].Azimuth),point_process[i].Range*sin(point_process[i].Azimuth)} ;
|
||||
float Z[3]={point_process[i].Range,
|
||||
// double Z[2]={ point_process[i].Range*cos(point_process[i].Azimuth),point_process[i].Range*sin(point_process[i].Azimuth)} ;
|
||||
double Z[3]={point_process[i].Range,
|
||||
point_process[i].Azimuth,
|
||||
point_process[i].Velocity};
|
||||
float vr_point=point_process[i].Velocity;
|
||||
float r_point=point_process[i].Range;
|
||||
float prt = point_process[i].PRF_index;
|
||||
float freq_ind = point_process[i].Freq_index;
|
||||
float h_point = point_process[i].Height;
|
||||
float T_point = point_process[i].CPI_Time;
|
||||
double vr_point=point_process[i].Velocity;
|
||||
double r_point=point_process[i].Range;
|
||||
double prt = point_process[i].PRF_index;
|
||||
double freq_ind = point_process[i].Freq_index;
|
||||
double h_point = point_process[i].Height;
|
||||
double T_point = point_process[i].CPI_Time;
|
||||
|
||||
//航迹信息
|
||||
float X[4];
|
||||
float P[4][4];
|
||||
memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(float));
|
||||
memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(float));
|
||||
float v_temp_track=(*temp_track)[j][L-1].vr;
|
||||
float r_temp_track=(*temp_track)[j][L-1].r;
|
||||
float h_temp_track=(*temp_track)[j][L-1].height;
|
||||
float T_track_head = (*temp_track)[j][L-1].T;
|
||||
double X[4];
|
||||
double P[4][4];
|
||||
memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(double));
|
||||
memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(double));
|
||||
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 = (*temp_track)[j][L-1].T;
|
||||
|
||||
|
||||
float delta_T = (T_point - T_track_head)/1000.0;
|
||||
double delta_T = (T_point - T_track_head)/1000.0;
|
||||
|
||||
if( fabs(r_point-r_temp_track)>=5 )
|
||||
{
|
||||
//计算d
|
||||
kalman Kalman;
|
||||
float d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
|
||||
//float d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
|
||||
//float d = Kalman.d_cal_track_init(Z,X,P,delta_T);
|
||||
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
|
||||
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
|
||||
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
|
||||
//计算夹角
|
||||
float x0=(*temp_track)[j][L-2].X[0];
|
||||
float y0=(*temp_track)[j][L-2].X[2];
|
||||
float x1=(*temp_track)[j][L-1].X[0];
|
||||
float y1=(*temp_track)[j][L-1].X[2];
|
||||
float x2=Z[0]*cos(Z[1]);
|
||||
float y2=Z[0]*sin(Z[1]);
|
||||
double x0=(*temp_track)[j][L-2].X[0];
|
||||
double y0=(*temp_track)[j][L-2].X[2];
|
||||
double x1=(*temp_track)[j][L-1].X[0];
|
||||
double y1=(*temp_track)[j][L-1].X[2];
|
||||
double x2=Z[0]*cos(Z[1]);
|
||||
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 )
|
||||
// {
|
||||
// qDebug() << "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;
|
||||
asso_info_tmp.point_idx = i+1;
|
||||
asso_info_tmp.track_idx = j+1;
|
||||
asso_info_tmp.d = d;
|
||||
asso_info.push_back(asso_info_tmp);
|
||||
(*temp_track)[j][L-1].asso_flag = 1;
|
||||
point_process[i].Use_Flag = 1;
|
||||
@@ -188,8 +252,6 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// temp_track中加入新关联上的临时航迹
|
||||
for (int i = 0 ; i<asso_info.size();i++ )
|
||||
{
|
||||
@@ -215,19 +277,23 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
|
||||
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;
|
||||
asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;
|
||||
float Z0[2]={(*temp_track)[asso_info[i].track_idx-1][L-1].r*cos((*temp_track)[asso_info[i].track_idx-1][L-1].azi),
|
||||
asso_track_info_tmp.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num;
|
||||
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));
|
||||
asso_track_info_tmp.d = asso_info[i].d;
|
||||
// asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;
|
||||
double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][L-1].r*cos((*temp_track)[asso_info[i].track_idx-1][L-1].azi),
|
||||
(*temp_track)[asso_info[i].track_idx-1][L-1].r*sin((*temp_track)[asso_info[i].track_idx-1][L-1].azi)};
|
||||
float Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth),
|
||||
double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth),
|
||||
point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)};
|
||||
float X[4];
|
||||
float P[4][4];
|
||||
double X[4];
|
||||
double P[4][4];
|
||||
kalman Kalman;
|
||||
float 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].T)/1000.0;
|
||||
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(float));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(float));
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
|
||||
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
|
||||
}
|
||||
|
||||
@@ -279,49 +345,89 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
|
||||
for ( int j=0;j<temp_track->size();j++)
|
||||
{
|
||||
|
||||
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round == 2)
|
||||
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
|
||||
{
|
||||
|
||||
|
||||
//点迹信息
|
||||
float x_point, y_point, vr_point;
|
||||
double x_point, y_point, vr_point;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.polar2cart(&x_point,&y_point,point_process[i].Range,point_process[i].Azimuth);
|
||||
vr_point=point_process[i].Velocity;
|
||||
float r_point=point_process[i].Range;
|
||||
float h_point = point_process[i].Height;
|
||||
float T_point = point_process[i].CPI_Time;
|
||||
double r_point=point_process[i].Range;
|
||||
double h_point = point_process[i].Height;
|
||||
double T_point = point_process[i].CPI_Time;
|
||||
|
||||
//航迹信息
|
||||
float x_track_head, y_track_head;
|
||||
double x_track_head, y_track_head;
|
||||
x_track_head=(*temp_track)[j][0].X[0];
|
||||
y_track_head=(*temp_track)[j][0].X[2];
|
||||
float v_track_head=(*temp_track)[j][0].vr;
|
||||
float h_track_head = (*temp_track)[j][0].height;
|
||||
float T_track_head = (*temp_track)[j][0].T;
|
||||
double v_track_head=(*temp_track)[j][0].vr;
|
||||
double h_track_head = (*temp_track)[j][0].height;
|
||||
double T_track_head = (*temp_track)[j][0].T;
|
||||
|
||||
// if(T_track_head == 30675948)
|
||||
// {
|
||||
// qDebug() << "r_point=" << r_point
|
||||
// << "h_point=" << h_point
|
||||
// << "T_point=" << T_point
|
||||
// << "x_track_head=" << x_track_head
|
||||
// << "y_track_head=" << y_track_head
|
||||
// << "v_track_head=" << v_track_head
|
||||
// << "T_track_head=" << T_track_head;
|
||||
// }
|
||||
|
||||
// if(T_point == 30682320)
|
||||
// {
|
||||
// qDebug() <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
|
||||
// qDebug() << "r_point=" << r_point
|
||||
// << "h_point=" << h_point
|
||||
// << "T_point=" << T_point
|
||||
// << "x_track_head=" << x_track_head
|
||||
// << "y_track_head=" << y_track_head
|
||||
// << "v_track_head=" << v_track_head
|
||||
// << "T_track_head=" << T_track_head;
|
||||
// }
|
||||
|
||||
|
||||
//距离差
|
||||
float dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
|
||||
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
|
||||
|
||||
float vmax;
|
||||
double vmax;
|
||||
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
|
||||
{
|
||||
vmax = V_MAX/5;
|
||||
vmax = Work_Parameter.V_MAX;
|
||||
}
|
||||
else //中远程模式 最大速度正常用
|
||||
{
|
||||
vmax = V_MAX;
|
||||
vmax = Work_Parameter.V_MAX;
|
||||
}
|
||||
|
||||
float delta_T = (T_point - T_track_head)/1000.0;
|
||||
double delta_T = (T_point - T_track_head)/1000.0;
|
||||
|
||||
|
||||
|
||||
//满足关联条件的点航
|
||||
if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=V_MIN*delta_T/2.0) )
|
||||
&& vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E
|
||||
// if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=V_MIN*delta_T/2.0) )
|
||||
// && vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E
|
||||
if(
|
||||
( (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
|
||||
|| (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
|
||||
)
|
||||
&& vr_point*v_track_head>0 )
|
||||
{
|
||||
|
||||
// if(T_track_head == 30675948)
|
||||
// {
|
||||
// qDebug() << "r_point=" << r_point
|
||||
// << "h_point=" << h_point
|
||||
// << "T_point=" << T_point
|
||||
// << "x_track_head=" << x_track_head
|
||||
// << "y_track_head=" << y_track_head
|
||||
// << "v_track_head=" << v_track_head
|
||||
// << "T_track_head=" << T_track_head;
|
||||
// }
|
||||
|
||||
struct Asso_info asso_info_tmp;
|
||||
asso_info_tmp.point_idx = i+1;
|
||||
asso_info_tmp.track_idx = j+1;
|
||||
@@ -356,21 +462,24 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
|
||||
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.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num;
|
||||
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));
|
||||
asso_track_info_tmp.asso_flag = 0;
|
||||
asso_track_info_tmp.buff_round = 1;
|
||||
asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;;
|
||||
float Z0[2]={(*temp_track)[asso_info[i].track_idx-1][0].r*cos((*temp_track)[asso_info[i].track_idx-1][0].azi),
|
||||
// asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;;
|
||||
double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][0].r*cos((*temp_track)[asso_info[i].track_idx-1][0].azi),
|
||||
(*temp_track)[asso_info[i].track_idx-1][0].r*sin((*temp_track)[asso_info[i].track_idx-1][0].azi)};
|
||||
float Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth),
|
||||
double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth),
|
||||
point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)};
|
||||
float X[4];
|
||||
float P[4][4];
|
||||
double X[4];
|
||||
double P[4][4];
|
||||
kalman Kalman;
|
||||
float 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].T)/1000.0;
|
||||
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(float));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(float));
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
|
||||
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
|
||||
|
||||
}
|
||||
@@ -398,7 +507,7 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
|
||||
|
||||
|
||||
////////////////////////////////////////////计算三点间的夹角////////////////////////////////////////////////
|
||||
double Track_Init::alpha_cal_track_init(float x0,float y0,float x1,float y1,float x2,float y2)
|
||||
double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2)
|
||||
{
|
||||
double D12[2]={x1-x0,y1-y0};
|
||||
double D23[2]={x2-x1,y2-y1};
|
||||
@@ -412,156 +521,33 @@ double Track_Init::alpha_cal_track_init(float x0,float y0,float x1,float y1,floa
|
||||
|
||||
|
||||
|
||||
void Track_Init::tmp_track_to_trust_track(QVector <QVector<Trust_Track>> *trust_track,
|
||||
void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
|
||||
QVector <QVector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter)
|
||||
{
|
||||
|
||||
//temp_track中的临时航迹转为可靠航迹
|
||||
QVector <QVector<Temp_track>> Track_to_start;
|
||||
|
||||
// 1. 将temp_track中满足条件的航迹取出, 放到Track_to_start中
|
||||
QVector <QVector<Temp_track>>::iterator Iter;
|
||||
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
|
||||
{
|
||||
int L=(*Iter).size();
|
||||
float range=(*Iter)[L-1].r;
|
||||
float azi=(*Iter)[L-1].azi/PI*180;
|
||||
double range=(*Iter)[L-1].r;
|
||||
double azi=(*Iter)[L-1].azi/PI*180;
|
||||
if( ( L==Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0) //按长度查找TRUST_TRACK_POINT
|
||||
{
|
||||
if(trust_track->size()<MAX_TRACK_INDEX)
|
||||
//加入Track_to_start中
|
||||
Track_to_start.push_back(QVector <Temp_track> ());
|
||||
for (int i = 0; i<L; i++)
|
||||
{
|
||||
Track_to_start[Track_to_start.size()-1].push_back((*Iter)[i]);
|
||||
|
||||
int index = track_index_mangement.track_ind_get(trust_track);
|
||||
if(index>0)
|
||||
{
|
||||
|
||||
//加入本地航迹文件
|
||||
float Z0[2]={((*Iter)[L-3].r)*cos((*Iter)[L-3].azi),((*Iter)[L-3].r)*sin((*Iter)[L-3].azi) };
|
||||
float Z1[2]={((*Iter)[L-2].r)*cos((*Iter)[L-2].azi),((*Iter)[L-2].r)*sin((*Iter)[L-2].azi) };
|
||||
float Z2[2]={((*Iter)[L-1].r)*cos((*Iter)[L-1].azi),((*Iter)[L-1].r)*sin((*Iter)[L-1].azi) };
|
||||
float X[6];
|
||||
float P[6][6];
|
||||
float T1=((*Iter)[L-2].T-(*Iter)[L-3].T)/1000.0;
|
||||
float T2=((*Iter)[L-1].T-(*Iter)[L-2].T)/1000.0;
|
||||
kalman Kalman;
|
||||
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
|
||||
|
||||
Trust_Track trust_track_tmp;
|
||||
memcpy(trust_track_tmp.X, X, 6*sizeof(float));
|
||||
memcpy(trust_track_tmp.P, P, 6*6*sizeof(float));
|
||||
|
||||
trust_track_tmp.Amplitude=(*Iter)[L-1].Amp;
|
||||
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.point_flag=1; //实点
|
||||
trust_track_tmp.Track_Mode=0; //跟踪模式 TWS 0
|
||||
trust_track_tmp.Target_Type=UNCONF_TARGET;
|
||||
trust_track_tmp.Track_Index=index;
|
||||
trust_track_tmp.Height=(*Iter)[L-1].height;
|
||||
trust_track_tmp.Extrapolate_round=0;
|
||||
trust_track_tmp.approach_flag = -1;
|
||||
trust_track_tmp.manual_delete_flag = 0;
|
||||
trust_track_tmp.associate_point_number = 0;
|
||||
trust_track_tmp.manual_tracking_flag = 0;
|
||||
trust_track_tmp.point_type = 0;
|
||||
trust_track_tmp.u[0]=0.3333;
|
||||
trust_track_tmp.u[1]=0.3333;
|
||||
trust_track_tmp.u[2]=0.3333;
|
||||
memcpy(trust_track_tmp.X1, X, 6*sizeof(float));
|
||||
memcpy(trust_track_tmp.P1, P, 6*6*sizeof(float));
|
||||
memcpy(trust_track_tmp.X2, X, 6*sizeof(float));
|
||||
memcpy(trust_track_tmp.P2, P, 6*6*sizeof(float));
|
||||
memcpy(trust_track_tmp.X3, X, 6*sizeof(float));
|
||||
memcpy(trust_track_tmp.P3, P, 6*6*sizeof(float));
|
||||
|
||||
float r, azi;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(X[0], X[3], &r, &azi);
|
||||
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_1_START && azi/PI*180<TRACK_SECTION_2_START)
|
||||
trust_track_tmp.Track_section_idx = 1;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_2_START && azi/PI*180<TRACK_SECTION_3_START)
|
||||
trust_track_tmp.Track_section_idx = 2;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_3_START && azi/PI*180<TRACK_SECTION_4_START)
|
||||
trust_track_tmp.Track_section_idx = 3;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_4_START && azi/PI*180<TRACK_SECTION_5_START)
|
||||
trust_track_tmp.Track_section_idx = 4;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_5_START && azi/PI*180<TRACK_SECTION_6_START)
|
||||
trust_track_tmp.Track_section_idx = 5;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_6_START && azi/PI*180<TRACK_SECTION_7_START)
|
||||
trust_track_tmp.Track_section_idx = 6;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_7_START && azi/PI*180<TRACK_SECTION_8_START)
|
||||
trust_track_tmp.Track_section_idx = 7;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_8_START && azi/PI*180<TRACK_SECTION_9_START)
|
||||
trust_track_tmp.Track_section_idx = 8;
|
||||
|
||||
if(azi/PI*180>=TRACK_SECTION_9_START || azi/PI*180<TRACK_SECTION_1_START)
|
||||
trust_track_tmp.Track_section_idx = 9;
|
||||
|
||||
(*trust_track)[trust_track_tmp.Track_section_idx-1].push_back(trust_track_tmp);
|
||||
|
||||
|
||||
//输出航迹更新信息
|
||||
*Trust_track_num_Output=*Trust_track_num_Output+1;
|
||||
for (int j=0;j<L;j++)
|
||||
{
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Index=index;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Point_Sum=L;
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=(*Iter)[j].r;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=(*Iter)[j].azi/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=sqrt(pow((*Iter)[j].X[1],2)+pow((*Iter)[j].X[3],2));
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].z=(*Iter)[j].height;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=(*Iter)[j].Amp;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = (*Iter)[j].snr;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = (*Iter)[j].RCS;
|
||||
float Direction_Angle;
|
||||
Direction_Angle=atan2((*Iter)[j].X[3],(*Iter)[j].X[1]);
|
||||
if(Direction_Angle<0)
|
||||
Direction_Angle=Direction_Angle+2*PI;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][L-1].Direction_Angle=Direction_Angle/PI*180;
|
||||
|
||||
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=((*Iter)[j].T)/1000.0;
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].x=(*Iter)[j].X[0];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=(*Iter)[j].X[1];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].y=(*Iter)[j].X[2];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=(*Iter)[j].X[3];
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=(*Iter)[j].r;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=(*Iter)[j].azi/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=(*Iter)[j].vr;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].point_type=0;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Flag_Point=1;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Mode=0;//跟踪模式 TWS 0
|
||||
}
|
||||
|
||||
temp_track->erase(Iter);
|
||||
Iter=temp_track->begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter++;
|
||||
}
|
||||
|
||||
temp_track->erase(Iter);
|
||||
Iter=temp_track->begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -569,6 +555,173 @@ void Track_Init::tmp_track_to_trust_track(QVector <QVector<Trust_Track>> *tru
|
||||
}
|
||||
}
|
||||
|
||||
//2.两两比较Track_to_start中的航迹信息，删除重复的航迹
|
||||
int L = Work_Parameter.track_start_point_num;
|
||||
for (int i=0; i<Track_to_start.size()-1;i++)
|
||||
{
|
||||
if(Track_to_start[i][0].asso_flag!=2)
|
||||
{
|
||||
for (int j=i+1;j<Track_to_start.size();j++)
|
||||
{
|
||||
if(Track_to_start[j][0].asso_flag!=2)
|
||||
{
|
||||
|
||||
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][L-1].d<Track_to_start[j][L-1].d)
|
||||
{
|
||||
Track_to_start[j][0].asso_flag=2;
|
||||
}
|
||||
else
|
||||
{
|
||||
Track_to_start[i][0].asso_flag=2;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
QVector <QVector<Temp_track>>::iterator Iter1;
|
||||
for (Iter1=Track_to_start.begin(); Iter1!=Track_to_start.end();)
|
||||
{
|
||||
if( (*Iter1)[0].asso_flag == 2 )
|
||||
{
|
||||
Track_to_start.erase(Iter1);
|
||||
Iter1=Track_to_start.begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter1++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//3.Track_to_start中剩余的航迹起始为可靠航迹
|
||||
for (int i=0; i<Track_to_start.size();i++)
|
||||
{
|
||||
if(trust_track->size()<MAX_TRACK_INDEX)
|
||||
{
|
||||
int index = track_index_mangement.track_ind_get(trust_track);
|
||||
if(index>0)
|
||||
{
|
||||
|
||||
//加入本地航迹文件
|
||||
double Z0[2]={(Track_to_start[i][L-3].r)*cos(Track_to_start[i][L-3].azi),(Track_to_start[i][L-3].r)*sin(Track_to_start[i][L-3].azi) };
|
||||
double Z1[2]={(Track_to_start[i][L-2].r)*cos(Track_to_start[i][L-2].azi),(Track_to_start[i][L-2].r)*sin(Track_to_start[i][L-2].azi) };
|
||||
double Z2[2]={(Track_to_start[i][L-1].r)*cos(Track_to_start[i][L-1].azi),(Track_to_start[i][L-1].r)*sin(Track_to_start[i][L-1].azi) };
|
||||
double X[6];
|
||||
double 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;
|
||||
kalman Kalman;
|
||||
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
|
||||
|
||||
Trust_Track trust_track_tmp;
|
||||
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
|
||||
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
|
||||
|
||||
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.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.point_flag=1; //实点
|
||||
trust_track_tmp.Track_Mode=0; //跟踪模式 TWS 0
|
||||
trust_track_tmp.Target_Type=UNCONF_TARGET;
|
||||
trust_track_tmp.Track_Index=index;
|
||||
trust_track_tmp.Height=Track_to_start[i][L-1].height;
|
||||
trust_track_tmp.Extrapolate_round=0;
|
||||
trust_track_tmp.approach_flag = -1;
|
||||
trust_track_tmp.manual_delete_flag = 0;
|
||||
trust_track_tmp.associate_point_number = 0;
|
||||
trust_track_tmp.manual_tracking_flag = 0;
|
||||
trust_track_tmp.point_type = 0;
|
||||
trust_track_tmp.u[0]=0.3333;
|
||||
trust_track_tmp.u[1]=0.3333;
|
||||
trust_track_tmp.u[2]=0.3333;
|
||||
memcpy(trust_track_tmp.X1, X, 6*sizeof(double));
|
||||
memcpy(trust_track_tmp.P1, P, 6*6*sizeof(double));
|
||||
memcpy(trust_track_tmp.X2, X, 6*sizeof(double));
|
||||
memcpy(trust_track_tmp.P2, P, 6*6*sizeof(double));
|
||||
memcpy(trust_track_tmp.X3, X, 6*sizeof(double));
|
||||
memcpy(trust_track_tmp.P3, P, 6*6*sizeof(double));
|
||||
|
||||
double r, azi;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(X[0], X[3], &r, &azi);
|
||||
|
||||
(*trust_track).push_back(trust_track_tmp);
|
||||
|
||||
|
||||
//输出航迹更新信息
|
||||
*Trust_track_num_Output=*Trust_track_num_Output+1;
|
||||
for (int j=0;j<L;j++)
|
||||
{
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Index=index;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Point_Sum=L;
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=Track_to_start[i][j].r;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=Track_to_start[i][j].azi/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=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=Track_to_start[i][j].height;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=Track_to_start[i][j].Amp;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = Track_to_start[i][j].snr;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = Track_to_start[i][j].RCS;
|
||||
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;
|
||||
|
||||
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim, Track_to_start[i][j].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim));
|
||||
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].range_dim, Track_to_start[i][j].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].range_dim));
|
||||
double Direction_Angle;
|
||||
Direction_Angle=atan2(Track_to_start[i][j].X[3],Track_to_start[i][j].X[1]);
|
||||
if(Direction_Angle<0)
|
||||
Direction_Angle=Direction_Angle+2*PI;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j-1].Direction_Angle=Direction_Angle/PI*180;
|
||||
|
||||
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=(Track_to_start[i][j].T)/1000.0;
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].x=Track_to_start[i][j].X[0];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=Track_to_start[i][j].X[1];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].y=Track_to_start[i][j].X[2];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=Track_to_start[i][j].X[3];
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=Track_to_start[i][j].r;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=Track_to_start[i][j].azi/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=Track_to_start[i][j].vr;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].point_type=0;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Flag_Point=1;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Mode=0;//跟踪模式 TWS 0
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
QVector <QVector<Temp_track>>().swap(Track_to_start);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -579,10 +732,11 @@ void Track_Init::tmp_track_die(QVector <QVector<Temp_track>> *temp_track)
|
||||
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
|
||||
{
|
||||
int n=(*Iter).size();
|
||||
if( (*Iter)[n-1].buff_round >1 || n>=10)
|
||||
if( (*Iter)[n-1].buff_round >2 || n>=10)
|
||||
{
|
||||
temp_track->erase(Iter);
|
||||
Iter=temp_track->begin();
|
||||
|
||||
temp_track->erase(Iter);
|
||||
Iter=temp_track->begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -594,7 +748,7 @@ void Track_Init::tmp_track_die(QVector <QVector<Temp_track>> *temp_track)
|
||||
|
||||
|
||||
|
||||
int Track_Init::track_init_prohibit(float r, float azi, struct RadarPara Work_Parameter)
|
||||
int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter)
|
||||
{
|
||||
|
||||
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
#include "parameters.h"
|
||||
#include "struct.h"
|
||||
#include <QVector>
|
||||
#include <QDebug>
|
||||
using namespace std;
|
||||
|
||||
|
||||
@@ -17,7 +18,7 @@ public:
|
||||
|
||||
//航迹起始逻辑法
|
||||
int track_init_process_logic( QVector<PointRecv> *point_recv, //输入点迹
|
||||
QVector <QVector<Trust_Track>> *trust_track, //可靠航迹
|
||||
QVector<Trust_Track> *trust_track, //可靠航迹
|
||||
QVector <QVector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
@@ -39,7 +40,7 @@ private:
|
||||
void point_track_head_asso( QVector <QVector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter); //航迹头与点迹关联
|
||||
|
||||
void tmp_track_to_trust_track( QVector <QVector<Trust_Track>> *trust_track, //临时航迹转可靠航迹
|
||||
void tmp_track_to_trust_track( QVector <Trust_Track> *trust_track, //临时航迹转可靠航迹
|
||||
QVector <QVector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
@@ -47,9 +48,9 @@ private:
|
||||
|
||||
void tmp_track_die(QVector <QVector<Temp_track>> *temp_track);//临时航迹消亡
|
||||
|
||||
double alpha_cal_track_init(float x0,float y0,float x1,float y1,float x2,float y2); //计算夹角
|
||||
double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角
|
||||
|
||||
int track_init_prohibit(float r, float azi, struct RadarPara Work_Parameter); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0
|
||||
int track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0
|
||||
|
||||
Track_Ind_Mangement track_index_mangement;
|
||||
|
||||
|
||||
@@ -0,0 +1,469 @@
|
||||
#include "track_init_direct_tracking.h"
|
||||
#include "kalman.h"
|
||||
#include "coor_trans.h"
|
||||
#include <qmath.h>
|
||||
#include "memory.h"
|
||||
#include <QVector>
|
||||
#include <iostream>
|
||||
#include<iomanip>
|
||||
using namespace std;
|
||||
|
||||
int Track_Init_Direct_Tracking::track_init_process_logic( QVector<PointRecv> *point_recv, //输入点迹
|
||||
QVector<Trust_Track> *trust_track, //可靠航迹
|
||||
QVector <QVector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
int track_ID
|
||||
)
|
||||
{
|
||||
|
||||
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Track_Init_Direct_Tracking::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter) //临时航迹与点迹关联
|
||||
{
|
||||
//关联信息
|
||||
struct Asso_info
|
||||
{
|
||||
int track_idx;
|
||||
int point_idx;
|
||||
};
|
||||
QVector <Asso_info> asso_info;
|
||||
|
||||
for ( int i=0;i<point_process.size();i++)
|
||||
{
|
||||
for ( int j=0;j<(*temp_track).size();j++)
|
||||
{
|
||||
int L = (*temp_track)[j].size();
|
||||
|
||||
if( (*temp_track)[j].size() > 1 && (*temp_track)[j][L-1].buff_round >= 2)
|
||||
{
|
||||
|
||||
//点迹信息
|
||||
// double Z[2]={ point_process[i].Range*cos(point_process[i].Azimuth),point_process[i].Range*sin(point_process[i].Azimuth)} ;
|
||||
double Z[3]={point_process[i].Range,
|
||||
point_process[i].Azimuth,
|
||||
point_process[i].Velocity};
|
||||
double vr_point=point_process[i].Velocity;
|
||||
double r_point=point_process[i].Range;
|
||||
double prt = point_process[i].PRF_index;
|
||||
double freq_ind = point_process[i].Freq_index;
|
||||
double h_point = point_process[i].Height;
|
||||
double T_point = point_process[i].CPI_Time;
|
||||
|
||||
//航迹信息
|
||||
double X[4];
|
||||
double P[4][4];
|
||||
memcpy(X,(*temp_track)[j][L-1].X,4*sizeof(double));
|
||||
memcpy(P,(*temp_track)[j][L-1].P,4*4*sizeof(double));
|
||||
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 = (*temp_track)[j][L-1].T;
|
||||
|
||||
|
||||
double delta_T = (T_point - T_track_head)/1000.0;
|
||||
|
||||
if( fabs(r_point-r_temp_track)>=5 )
|
||||
{
|
||||
//计算d
|
||||
kalman Kalman;
|
||||
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
|
||||
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
|
||||
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
|
||||
//计算夹角
|
||||
double x0=(*temp_track)[j][L-2].X[0];
|
||||
double y0=(*temp_track)[j][L-2].X[2];
|
||||
double x1=(*temp_track)[j][L-1].X[0];
|
||||
double y1=(*temp_track)[j][L-1].X[2];
|
||||
double x2=Z[0]*cos(Z[1]);
|
||||
double y2=Z[0]*sin(Z[1]);
|
||||
double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2);
|
||||
|
||||
|
||||
if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha<ALPHA_START && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
|
||||
{
|
||||
struct Asso_info asso_info_tmp;
|
||||
asso_info_tmp.point_idx = i+1;
|
||||
asso_info_tmp.track_idx = j+1;
|
||||
asso_info.push_back(asso_info_tmp);
|
||||
(*temp_track)[j][L-1].asso_flag = 1;
|
||||
point_process[i].Use_Flag = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// temp_track中加入新关联上的临时航迹
|
||||
for (int i = 0 ; i<asso_info.size();i++ )
|
||||
{
|
||||
(*temp_track).push_back(QVector <Temp_track> ());
|
||||
|
||||
//前L个点
|
||||
int L = (*temp_track)[asso_info[i].track_idx-1].size();
|
||||
for (int j=0;j < L;j++ )
|
||||
{
|
||||
(*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][j]);
|
||||
(*temp_track)[(*temp_track).size()-1][j].asso_flag = 0;
|
||||
}
|
||||
|
||||
//关联上的点
|
||||
Temp_track asso_track_info_tmp;
|
||||
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.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.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_num;
|
||||
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));
|
||||
asso_track_info_tmp.asso_flag=0;
|
||||
asso_track_info_tmp.buff_round = 1;
|
||||
// asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;
|
||||
double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][L-1].r*cos((*temp_track)[asso_info[i].track_idx-1][L-1].azi),
|
||||
(*temp_track)[asso_info[i].track_idx-1][L-1].r*sin((*temp_track)[asso_info[i].track_idx-1][L-1].azi)};
|
||||
double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth),
|
||||
point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)};
|
||||
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;
|
||||
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
|
||||
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
|
||||
}
|
||||
|
||||
|
||||
//temp_track中删除关联上的临时航迹
|
||||
QVector <QVector<Temp_track>>::iterator Iter;
|
||||
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
|
||||
{
|
||||
if((*Iter).size()>=2)
|
||||
{
|
||||
if((*Iter)[(*Iter).size()-1].asso_flag==1)
|
||||
{
|
||||
temp_track->erase(Iter);
|
||||
Iter=temp_track->begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Track_Init_Direct_Tracking::point_track_head_asso( QVector <QVector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter) //航迹头与点迹关联
|
||||
{
|
||||
//关联上的信息
|
||||
struct Asso_info
|
||||
{
|
||||
int track_idx;
|
||||
int point_idx;
|
||||
};
|
||||
QVector <Asso_info> asso_info;
|
||||
|
||||
|
||||
//关联
|
||||
for ( int i=0;i<point_process.size();i++)
|
||||
{
|
||||
|
||||
for ( int j=0;j<temp_track->size();j++)
|
||||
{
|
||||
|
||||
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
|
||||
{
|
||||
|
||||
|
||||
//点迹信息
|
||||
double x_point, y_point, vr_point;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.polar2cart(&x_point,&y_point,point_process[i].Range,point_process[i].Azimuth);
|
||||
vr_point=point_process[i].Velocity;
|
||||
double r_point=point_process[i].Range;
|
||||
double h_point = point_process[i].Height;
|
||||
double T_point = point_process[i].CPI_Time;
|
||||
|
||||
//航迹信息
|
||||
double x_track_head, y_track_head;
|
||||
x_track_head=(*temp_track)[j][0].X[0];
|
||||
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 = (*temp_track)[j][0].T;
|
||||
|
||||
|
||||
//距离差
|
||||
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
|
||||
|
||||
double vmax;
|
||||
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
|
||||
{
|
||||
//vmax = V_MAX/5;
|
||||
vmax = Work_Parameter.V_MAX;
|
||||
|
||||
}
|
||||
else //中远程模式 最大速度正常用
|
||||
{
|
||||
// vmax = V_MAX;
|
||||
vmax = Work_Parameter.V_MAX;
|
||||
|
||||
}
|
||||
|
||||
double delta_T = (T_point - T_track_head)/1000.0;
|
||||
|
||||
|
||||
|
||||
//满足关联条件的点航
|
||||
if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )
|
||||
&& vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E
|
||||
{
|
||||
struct Asso_info asso_info_tmp;
|
||||
asso_info_tmp.point_idx = i+1;
|
||||
asso_info_tmp.track_idx = j+1;
|
||||
asso_info.push_back(asso_info_tmp);
|
||||
(*temp_track)[j][0].asso_flag = 1;
|
||||
point_process[i].Use_Flag = 1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// temp_track中加入新关联上的临时航迹
|
||||
for (int i = 0 ; i<asso_info.size();i++ )
|
||||
{
|
||||
(*temp_track).push_back(QVector <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;
|
||||
|
||||
//第二个点
|
||||
struct Temp_track asso_track_info_tmp;
|
||||
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.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.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.pitch_num = point_process[asso_info[i].point_idx-1].pitch_num;
|
||||
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));
|
||||
asso_track_info_tmp.asso_flag = 0;
|
||||
asso_track_info_tmp.buff_round = 1;
|
||||
// asso_track_info_tmp.Temp_track_section_idx = floor(point_process[asso_info[i].point_idx-1].beam_index/BEAM_NUM_DOT_SECTION)+1;;
|
||||
double Z0[2]={(*temp_track)[asso_info[i].track_idx-1][0].r*cos((*temp_track)[asso_info[i].track_idx-1][0].azi),
|
||||
(*temp_track)[asso_info[i].track_idx-1][0].r*sin((*temp_track)[asso_info[i].track_idx-1][0].azi)};
|
||||
double Z1[2]={point_process[asso_info[i].point_idx-1].Range*cos(point_process[asso_info[i].point_idx-1].Azimuth),
|
||||
point_process[asso_info[i].point_idx-1].Range*sin(point_process[asso_info[i].point_idx-1].Azimuth)};
|
||||
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;
|
||||
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
|
||||
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
//temp_track中删除关联上的航迹头
|
||||
QVector <QVector<Temp_track>>::iterator Iter;
|
||||
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
|
||||
{
|
||||
if((*Iter)[0].asso_flag==1)
|
||||
{
|
||||
temp_track->erase(Iter);
|
||||
Iter=temp_track->begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector<Trust_Track> *trust_track, //临时航迹转可靠航迹
|
||||
QVector <QVector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
int track_ID)
|
||||
{
|
||||
//temp_track中的临时航迹转为可靠航迹
|
||||
QVector <QVector<Temp_track>>::iterator Iter;
|
||||
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
|
||||
{
|
||||
int L=(*Iter).size();
|
||||
double range=(*Iter)[L-1].r;
|
||||
double azi=(*Iter)[L-1].azi/PI*180;
|
||||
if( L==Work_Parameter.track_start_point_num) //按长度查找TRUST_TRACK_POINT
|
||||
{
|
||||
|
||||
//加入本地航迹文件
|
||||
double Z0[2]={((*Iter)[L-3].r)*cos((*Iter)[L-3].azi),((*Iter)[L-3].r)*sin((*Iter)[L-3].azi) };
|
||||
double Z1[2]={((*Iter)[L-2].r)*cos((*Iter)[L-2].azi),((*Iter)[L-2].r)*sin((*Iter)[L-2].azi) };
|
||||
double Z2[2]={((*Iter)[L-1].r)*cos((*Iter)[L-1].azi),((*Iter)[L-1].r)*sin((*Iter)[L-1].azi) };
|
||||
double X[6];
|
||||
double 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;
|
||||
kalman Kalman;
|
||||
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
|
||||
Trust_Track trust_track_tmp;
|
||||
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
|
||||
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
|
||||
|
||||
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.RCS = (*Iter)[L-1].RCS;
|
||||
trust_track_tmp.T_track = (*Iter)[L-1].T;
|
||||
trust_track_tmp.point_flag=1; //实点
|
||||
trust_track_tmp.Track_Mode=1; //跟踪模式 TWS 0
|
||||
trust_track_tmp.Target_Type=UNCONF_TARGET;
|
||||
trust_track_tmp.Height=(*Iter)[L-1].height;
|
||||
trust_track_tmp.Extrapolate_round=0;
|
||||
trust_track_tmp.approach_flag = -1;
|
||||
trust_track_tmp.manual_delete_flag = 0;
|
||||
trust_track_tmp.associate_point_number = 0;
|
||||
trust_track_tmp.manual_tracking_flag = 0;
|
||||
trust_track_tmp.point_type = 0;
|
||||
trust_track_tmp.u[0]=0.3333;
|
||||
trust_track_tmp.u[1]=0.3333;
|
||||
trust_track_tmp.u[2]=0.3333;
|
||||
memcpy(trust_track_tmp.X1, X, 6*sizeof(double));
|
||||
memcpy(trust_track_tmp.P1, P, 6*6*sizeof(double));
|
||||
memcpy(trust_track_tmp.X2, X, 6*sizeof(double));
|
||||
memcpy(trust_track_tmp.P2, P, 6*6*sizeof(double));
|
||||
memcpy(trust_track_tmp.X3, X, 6*sizeof(double));
|
||||
memcpy(trust_track_tmp.P3, P, 6*6*sizeof(double));
|
||||
(*trust_track).push_back(trust_track_tmp);
|
||||
|
||||
trust_track_tmp.pitch_num = (*Iter)[L-1].pitch_num;
|
||||
|
||||
std::memcpy(trust_track_tmp.speed_dim, (*Iter)[L-1].speed_dim, sizeof(trust_track_tmp.speed_dim));
|
||||
std::memcpy(trust_track_tmp.range_dim, (*Iter)[L-1].range_dim, sizeof(trust_track_tmp.range_dim));
|
||||
|
||||
//输出航迹更新信息
|
||||
*Trust_track_num_Output=*Trust_track_num_Output+1;
|
||||
for (int j=0;j<L;j++)
|
||||
{
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Index=track_ID;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Point_Sum=L;
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=(*Iter)[j].r;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=(*Iter)[j].azi/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=sqrt(pow((*Iter)[j].X[1],2)+pow((*Iter)[j].X[3],2));
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].z=(*Iter)[j].height;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=(*Iter)[j].Amp;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = (*Iter)[j].snr;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = (*Iter)[j].RCS;
|
||||
double Direction_Angle;
|
||||
Direction_Angle=atan2((*Iter)[j].X[3],(*Iter)[j].X[1]);
|
||||
if(Direction_Angle<0)
|
||||
Direction_Angle=Direction_Angle+2*PI;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=Direction_Angle/PI*180;
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=((*Iter)[j].T)/1000.0;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].x=(*Iter)[j].X[0];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=(*Iter)[j].X[1];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].y=(*Iter)[j].X[2];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=(*Iter)[j].X[3];
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=(*Iter)[j].r;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=(*Iter)[j].azi/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=(*Iter)[j].vr;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].point_type=0;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Flag_Point=1;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Mode=1;//跟踪模式 TWS 0
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = (*Iter)[j].pitch_num;
|
||||
|
||||
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim, (*Iter)[j].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim));
|
||||
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].range_dim, (*Iter)[j].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].range_dim));
|
||||
}
|
||||
|
||||
temp_track->erase(Iter);
|
||||
Iter=temp_track->begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Track_Init_Direct_Tracking::tmp_track_die(QVector <QVector<Temp_track>> *temp_track)
|
||||
{
|
||||
|
||||
QVector <QVector<Temp_track>>::iterator Iter;
|
||||
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
|
||||
{
|
||||
int n=(*Iter).size();
|
||||
if( (*Iter)[n-1].buff_round >1 || n>=10)
|
||||
{
|
||||
temp_track->erase(Iter);
|
||||
Iter=temp_track->begin();
|
||||
}
|
||||
else
|
||||
{
|
||||
Iter++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
double Track_Init_Direct_Tracking::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2)
|
||||
{
|
||||
double D12[2]={x1-x0,y1-y0};
|
||||
double D23[2]={x2-x1,y2-y1};
|
||||
double alpha=acos((D12[0]*D23[0]+D12[1]*D23[1])/(sqrt(D12[0]*D12[0]+D12[1]*D12[1])*sqrt(D23[0]*D23[0]+D23[1]*D23[1])))/PI*180;
|
||||
|
||||
return alpha;
|
||||
|
||||
|
||||
}
|
||||
|
||||
Track_Init_Direct_Tracking::Track_Init_Direct_Tracking()
|
||||
{
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
#ifndef TRACK_INIT_DIRECT_TRACKING_H
|
||||
#define TRACK_INIT_DIRECT_TRACKING_H
|
||||
#include "data_process_class_dll.h"
|
||||
#include "parameters.h"
|
||||
#include "struct.h"
|
||||
#include <QVector>
|
||||
|
||||
class Track_Init_Direct_Tracking
|
||||
{
|
||||
public:
|
||||
|
||||
//航迹起始逻辑法
|
||||
int track_init_process_logic( QVector<PointRecv> *point_recv, //输入点迹
|
||||
QVector<Trust_Track> *trust_track, //可靠航迹
|
||||
QVector <QVector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
int track_ID
|
||||
);
|
||||
|
||||
Track_Init_Direct_Tracking();
|
||||
|
||||
|
||||
private:
|
||||
|
||||
|
||||
QVector<PointRecv> point_process; //要处理的点迹
|
||||
|
||||
|
||||
void point_temp_track_asso(QVector <QVector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter); //临时航迹与点迹关联
|
||||
|
||||
|
||||
void point_track_head_asso( QVector <QVector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter); //航迹头与点迹关联
|
||||
|
||||
void tmp_track_to_trust_track( QVector<Trust_Track> *trust_track, //临时航迹转可靠航迹
|
||||
QVector <QVector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
int track_ID
|
||||
);
|
||||
|
||||
void tmp_track_die(QVector <QVector<Temp_track>> *temp_track);//临时航迹消亡
|
||||
|
||||
double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // TRACK_INIT_DIRECT_TRACKING_H
|
||||
Binary file not shown.
Reference in New Issue
Block a user