面向三维目标跟踪的状态转换卡尔曼滤波方法

doi:10.12382/bgxb.2024.0182

摘要/Abstract

摘要：

雷达及声呐系统采集的三维球坐标量测与运动目标的笛卡尔坐标状态呈非线性,导致跟踪精度受限,而具有强非线性的多普勒量测更难以高效利用。针对上述问题,构造由距离、俯仰角、方位角及其微分组成的状态向量,使测量方程线性化;在距离、俯仰角构成二维时变极坐标系中对常微分动力学方程离散化,再基于投影关系引入方位角,建立球坐标系中的典型三维常速度和常加速度运动模型;结合标准卡尔曼滤波在线性高斯框架下,实现跟踪以避免非线性误差。通过仿真验证了在包括多普勒雷达等若干三维跟踪场景下提出方法的有效性与性能优势。

关键词: 多普勒雷达, 三维目标跟踪, 非线性滤波, 状态转换卡尔曼滤波

Abstract:

The three-dimensional spherical coordinate measurements collected by the radar and sonar system are nonlinear with the Cartesian coordinate state of the moving target, which limits the tracking accuracy, and. it is more difficult to use the Doppler measurement with strong nonlinearityefficiently. Aiming at the above problems, a state vector composed of distance, pitch angle, azimuth angle and their derivatives is constructed to linearize the measurement equation,and the ordinary differential dynamics equation is discretized in a two-dimensional time-varying polar coordinate system composed of distance and pitch angle.Then the azimuth angle is introduced based on the projection relationship, and a typical three-dimensional constant velocity and constant acceleration motion model in the spherical coordinate system are established. Combined with the standard Kalman filter, the tracking is realized to avoid the nonlinear errorsunder the linear Gaussian framework. The effectiveness and performance advantagesof the proposed method in several three-dimensional tracking scenarios are verified through simulation.

Key words: Doppler radar, three-dimensional target tracking, nonlinear filtering, converted state equation Kalman filter

中图分类号:

TN953

刘增力, 张文, 曹奇宏, 赵宣植, 刘康, 曾赛. 面向三维目标跟踪的状态转换卡尔曼滤波方法[J]. 兵工学报, 2024, 45(11): 3998-4010.

LIU Zengli, ZHANG Wen, CAO Qihong, ZHAO Xuanzhi, LIU Kang, ZENG Sai. Converted State Equation Kalman Filter for Three-dimensional Target Tracking[J]. Acta Armamentarii, 2024, 45(11): 3998-4010.

图/表 22

图1 量测在观测坐标系中的分布

Fig.1 Distribution of measurements in the observation coordinate system

图2 量测在笛卡尔坐标系中的分布

Fig.2 Distribution of measurements in Cartesian coordinate system

图3 量测在笛卡尔坐标系中的分布与拟和高斯分布

Fig.3 True distribution and fitted Gaussian distribution of measurements in Cartesian coordinate system

图4 三维量测空间运动分解示意图

Fig.4 Decomposition diagram of target motion in three-dimensional measurement space

图5 时变二维极坐标运动分解示意图

Fig.5 Decomposition diagram of time-varying two-dimensional polar coordinate motion

表1 仿真图中的雷达观测参数

Table 1 Parameters of radar measurements in the simulation diagram

雷达参数	三坐标雷达	多普勒雷达
σ_r/m	50	50
σ_ϕ(°)	0.4	0.4
σ_θ(°)	0.5	0.5
$σ r ·$ /(m·s^-1)		0.5
相关系数ρ		0.6

表1 仿真图中的雷达观测参数

Table 1 Parameters of radar measurements in the simulation diagram

雷达参数	三坐标雷达	多普勒雷达
σ_r/m	50	50
σ_ϕ(°)	0.4	0.4
σ_θ(°)	0.5	0.5
$σ r ·$ /(m·s^-1)		0.5
相关系数ρ		0.6

图6 CV模型位置RMSE

Fig.6 Position RMSE under the CV model

图7 CV模型速度RMSE

Fig.7 Velocity RMSE under the CV model

图8 CA模型位置RMSE

Fig.8 Position RMSE under the CA model

图9 CA模型速度RMSE

Fig.9 Velocity RMSE under the CA model

图10 CV模型ANEES

Fig.10 ANEES under the CV model

图11 CA模型ANEES

Fig.11 ANEES under the CA model

图12 多普勒雷达CV模型位置RMSE

Fig.12 Position RMSE in the CV model of Doppler radar

图13 多普勒雷达CV模型速度RMSE

Fig.13 Velocity RMSE under the CV model of Doppler radar

图14 多普勒雷达CA模型位置RMSE

Fig.14 Position RMSE under the CA model of Doppler radar

图15 多普勒雷达CA模型速度RMSE

Fig.15 Velocity RMSE under the CA model of Doppler radar

图16 多普勒雷达CV模型ANEES

Fig.16 ANEES under the CV model of Doppler radar

图17 CA模型各方法ANEES

Fig.17 ANEES under the CA model

表2 不同雷达观测参数下的性能对比

Table 2 Performance comparison of different algorithms under different radar measurement parameters

雷达参数	方法	CV位置 RMSEs	CV速度 RMSEs	CA位置 RMSEs	CA速度 RMSEs
σ_r=20m	EKF	49.5510	1.5707	50.2996	1.5158
σ_θ=0.25/(°)	UKF	26.2767	1.4255	26.8038	1.2938
σ_ϕ=0.2/(°)	DUCMKF	25.9613	1.4255	26.7194	1.3371
	3DCSKF	22.2401	1.2559	20.9610	1.0916
σ_r=80m	EKF	67.5251	2.0311	68.4591	1.8785
σ_θ=0.8/(°)	UKF	55.2284	1.9414	53.0092	1.6435
σ_ϕ=0.65/(°)	DUCMKF	55.7659	1.9151	53.2348	1.7252
	3DCSKF	43.0970	1.6944	39.3281	1.5980

表3 不同多普勒雷达观测参数下的性能对比

Table 3 Performance comparison of different algorithms with different Doppler radar measurement parameters

雷达参数	方法	CV位置 RMSEs	CV速度 RMSEs	CA位置 RMSEs	CA速度 RMSEs
σ_r=25m	EKF	25.4976	1.1758	25.8386	1.1334
σ_θ=0.25/(°)	UKF	24.7819	1.1366	25.7960	1.1424
σ_ϕ=0.2/(°)	DUCMKF	25.2530	1.1267	25.4917	1.1272
$σ r ·$ =0.2m/s	3DCSKF	19.1871	0.7401	20.5462	0.7712
ρ=0.2
σ_r=80m	EKF	55.6626	1.4727	54.5091	1.4931
σ_θ=0.8/(°)	UKF	52.8558	1.3891	52.4822	1.4327
σ_ϕ=0.65/(°)	DUCMKF	52.3959	1.3463	53.1714	1.3913
$σ r ·$ =1m/s	3DCSKF	36.4882	0.9053	37.7613	0.9677
ρ=0.9

表3 不同多普勒雷达观测参数下的性能对比

Table 3 Performance comparison of different algorithms with different Doppler radar measurement parameters

雷达参数	方法	CV位置 RMSEs	CV速度 RMSEs	CA位置 RMSEs	CA速度 RMSEs
σ_r=25m	EKF	25.4976	1.1758	25.8386	1.1334
σ_θ=0.25/(°)	UKF	24.7819	1.1366	25.7960	1.1424
σ_ϕ=0.2/(°)	DUCMKF	25.2530	1.1267	25.4917	1.1272
$σ r ·$ =0.2m/s	3DCSKF	19.1871	0.7401	20.5462	0.7712
ρ=0.2
σ_r=80m	EKF	55.6626	1.4727	54.5091	1.4931
σ_θ=0.8/(°)	UKF	52.8558	1.3891	52.4822	1.4327
σ_ϕ=0.65/(°)	DUCMKF	52.3959	1.3463	53.1714	1.3913
$σ r ·$ =1m/s	3DCSKF	36.4882	0.9053	37.7613	0.9677
ρ=0.9

图18 坐标雷达跟踪场景计算负载

Fig.18 Computational loads for 3D radar tracking scenarios

图19 多普勒雷达跟踪场景计算负载

Fig.19 Computational load for Doppler radar tracking scenarios

参考文献 36

[1]	曾雅俊, 王俊, 魏少明, 等. 分布式多传感器多目标跟踪方法综述[J]. 雷达学报, 2023, 12(1):197-213.
	ZENG Y J, WANG J, WEI S M, et al. Review of the method for distributed multi-sensor multitarget tracking[J]. Journal of Radars, 2023, 12(1): 197-213. (in Chinese)
[2]	李正杰, 陈红印, 谢军伟, 等. 一种防空服务质量模型下的集中式多输入多输出雷达三维机动跟踪功率分配方法[J]. 兵工学报, 2024, 45(4): 1321-1331. doi: 10.12382/bgxb.2022.1252
	LI Z J, CHEN H Y, XIE J W, et al. A power allocation method for three-dimensional maneuvering tracking of volocated MIMO radar based on air defense QoS model[J]. Acta Armamentarii, 2024, 45(4): 1321-1331. (in Chinese)
[3]	ZHANG J L, XIANG X B, LI W J. Advances in marine intelligent electromagnetic detection system, technology, and applications:areview[J]. IEEE Sensors Journal, 2023, 23(5):4312-4326.
[4]	ZHANG X F, FAN Z P, TAN X J, et al. Spatiotemporal adaptive attention 3D multiobject tracking for autonomous driving[J]. Knowledge Based Systems, 2023, 267: 110442.
[5]	于航, 李清玉, 戴可人, 等. 三维自适应固定时间多导弹协同制导律[J]. 兵工学报, 2024, 45(8): 2646-2657. doi: 10.12382/bgxb.2023.0640
	YU H, LI Q Y, DAI K R, et al. Three-dimensional adaptive fixed-time multi-missile cooperative guidance law[J]. Acta Armamentarii, 2024, 45(8): 2646-2657. (in Chinese) doi: 10.12382/bgxb.2023.0640
[6]	ZENG T, LI C X, LIU Q H, et al. Tracking with nonlinear measurement model by coordinate rotation transformation[J]. Science China Technological Sciences, 2014, 57(12):2396-2406.
[7]	DONALD L, YAAKOV B S. Tracking with debiased consistent converted measurements versus EKF[J]. IEEE Transactions on Aerospace & Electronic Systems, 1993, 29(3):1015-1022.
[8]	MARCO F H, FREDERIK B, UWE D H. (Semi-)analytic Gaussian mixture filter[J]. IFAC Proceedings Volumes, 2011, 44(1): 10014-10020.
[9]	TIAN X, YAAKOV B S. Coordinate conversion and tracking for very long range radars[J]. IEEE Transactions on Aerospace and Electronic Systems, 2009, 45(3):1073-1088.
[10]	WEI M, YAAKOV B S. Unbiased Kalman filter using converted measurements:revisit[C]// Proceedings of Signal and Data Processing of Small Targets 2009. San Diego, CA, US:SPIE, 2009, 7445:346-354.
[11]	STEVEN B, PETER W, YAAKOV B S. Decorrelated unbiased converted measurement Kalman filter[J]. IEEE Transactions on Aerospace and Electronic Systems, 2014, 50(2):1431-1444.
[12]	SINMON J, JEFFREY U. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3):401-422.
[13]	王泳安, 李东光, 吴浩, 等. 一种自适应滤波与干扰观测器相结合的大型舰船状态估计算法[J]. 兵工学报, 2024, 45(7): 2318-2328. doi: 10.12382/bgxb.2023.0588
	WANG Y A, LI D G, WU H, et al. An adaptive filtering-disturbance observer-based state estimation algorithm for large ships[J]. Acta Armamentarii, 2024, 45(7): 2318-2328. (in Chinese) doi: 10.12382/bgxb.2023.0588
[14]	STEVEN V B, PETER W, YAAKOV B S, et al. Converted measurement sigma point Kalman filter for bistatic sonar and radar tracking[J]. IEEE Transactions on Aerospace and Electronic Systems, 2018, 55(1):147-159.
[15]	ZHANG X, YAN Z B, CHEN Y Q. High-degree cubature Kalman filter for nonlinear state estimation with missing measurements[J]. Asian Journal of Control, 2022, 24(3):1261-1272.
[16]	ZHAO M J, ZHANG T, WANG D. A high precision indoor positioning method based on UKF[J]. Journal of Intelligent & Fuzzy Systems, 2022, 42(4):3639-3652.
[17]	ZHAO Z L, LI X R, VESSELIN P J. Best linear unbiased filtering with nonlinear measurements for target tracking[J]. IEEE Transactions on Aerospace and Electronic Systems, 2004, 40(4): 1324-1336.
[18]	盛琥, 赵温波, 张远. 基于转换量测高斯化的三坐标雷达跟踪算法[J]. 电子学报, 2018, 46(3): 562-568. doi: 10.3969/j.issn.0372-2112.2018.03.007
	SHENG H, ZHAO W B, ZHANG Y. 3D radar tracking algorithm with Gaussianized converted measurement[J]. Acta Electronica Sinica, 2018, 46(3):562-568. (in Chinese) doi: 10.3969/j.issn.0372-2112.2018.03.007
[19]	BENJAMIN D, WILLIAM D B. Maximum likelihood mixture modeling for three-dimensional non-Gaussian measurements[C]// Proceedings of the2018 IEEE Radar Conference. Washington,D.C., US:IEEE, 2018: 1027-1032.
[20]	ARULAMPALAM M S, SIMON M, NEIL G, et al. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J]. IEEE Transactions on signal processing, 2002, 50(2): 174-188.
[21]	JINY Z, MU L X, FENG N, et al. Maximum correntropy-based extended particle filter for nonlinear system[J]. IEEE Transactions on Circuits and Systems II: Express Briefs, 2023, 70(7):2520-2524.
[22]	卢航, 郝顺义, 彭志颖, 等. 基于边缘采样的简化高阶CKF在非线性快速传递对准中的应用[J]. 航空学报, 2019, 40(3):189-202.
	LU H, HAO S Y, PENG Z Y, et al. Application of marginal reduced high-degree cubature Kalman filter to nonlinear rapid transfer alignment[J]. Acta Aeronautica et Astronautica Sinica, 2019, 40(3):189-202. (in Chinese)
[23]	LAI J L, JUSTIN D, DANIEL S. Variational marginal particle filters[C]// Proceedings of the 25th International Conference on Artificial Intelligence and Statistics. Valencia, Spain:IEEE, 2022:875-895.
[24]	HU B Q, CHANG L B, LI A, et al. Computationallyefficient simplex unscented Kalman filter based on numerical integration[J]. Asian Journal of Control, 2014, 16(3): 915-921.
[25]	戴宗武, 张少甫, 刘乃强, 等. 用于自主导航的SAR/多普勒雷达一体化技术设计[J]. 兵工学报, 2022, 43(12): 3093-3102.
	DAI Z W, ZHANG S F, LIU N Q, et al. SAR/Doppler radar integration design for autonomous navigation[J]. Acta Armamentarii, 2022, 43(12): 3093-3102. (in Chinese) doi: 10.12382/bgxb.2021.0678
[26]	李姝怡, 程婷. 基于量测转换IMM的多普勒雷达机动目标跟踪[J]. 电子学报, 2019, 47(3): 538-544. doi: 10.3969/j.issn.0372-2112.2019.03.004
	LI S Y, CHENG T. Interactive multiple model algorithm for a Doppler radar maneuvering target tracking based on converted measurements[J]. Acta Electronica Sinica, 2019, 47(3):538-544. (in Chinese)
[27]	王琪, 廖志忠, 燕飞. 基于概率数据关联的雷达导引头抗速度拖引干扰算法[J]. 系统工程与电子技术, 2022, 44(2):448-454. doi: 10.12305/j.issn.1001-506X.2022.02.12
	WANG Q, LIAO Z Z, YAN F. Algorithm for countering velocity gate pull-off jamming of radar seeker based on probability data association[J]. Systems Engineering and Electronics, 2022, 44(2): 448-454. (in Chinese) doi: 10.12305/j.issn.1001-506X.2022.02.12
[28]	LEI M, HAN C Z. Sequential nonlinear tracking using UKF and raw range-rate measurements[J]. IEEE Transactions on Aerospace and Electronic Systems, 2007, 43(1):239-250.
[29]	YAAKOV B S. Negative correlation and optimal tracking with Doppler measurements[J]. IEEE Transactions on Aerospace and Electronic Systems, 2001, 37(3):1117-1120.
[30]	ZHOU G J, MICHEL P, THIAGALINGAM K, et al. Statically fused converted position and Doppler measurement Kalman filters[J]. IEEE Transactions on Aerospace and Electronic Systems, 2014, 50(1) :300-316.
[31]	FU J B, SUN J P, LU S T, et al. Debiased converted position and Doppler measurement tracking with array radar measurements in direction cosine coordinates[J]. IET Radar, Sonar & Navigation, 2016, 10(1):155-165.
[32]	LIU H Q, ZHOU Z L, LEI Y, et al. Two unbiased converted measurement Kalman filtering algorithms with range rate[J]. IET Radar, Sonar & Navigation, 2018, 12(11): 1217-1224.
[33]	CHAW B C, TABACZNSKI J. Application of state estimation to target tracking[J]. IEEE Transactions on Automatic Control, 1984, 29(2): 98-109.
[34]	LI K Y, ZHOU G J, CUI N G. Motion modeling and state estimation in range-squared coordinate[J]. IEEE Transactions on Signal Processing, 2022, 70: 5279-5294.
[35]	赵艳丽, 李宏, 高向东. 不同坐标系下中段弹道目标跟踪算法研究[J]. 现代雷达, 2011, 33(5):54-59.
	ZHAO Y L, LI H, GAO X D. A study on ballistic target tracking in midcourse of various coordinate systems[J]. Modern Radar, 2011, 33(5):54-59. (in Chinese)
[36]	ZHANG W, ZHAO X Z, LIU Z L, et al. Converted state equation Kalman filter for nonlinear maneuvering target tracking[J]. Signal Processing, 2023, 202:1087-41.