The task of Unmanned Aerial Vehicle (UAV) situational awareness is to use airborne sensors to identify and guide targets in unknown environments. To solve the relative navigation problem between UAV and non-cooperative target at medium and long distances, a relative state estimation algorithm based on angle and distance measurements is proposed. On the basis of existing filtering algorithms, in order to improve the accuracy and stability, this paper improves the Iterative Extended Kalman Filter (IEKF) algorithm by using the Levenberg-Marquardt (LM) optimization. This paper proposes a Levenberg-Marquardt Iterative Extended Kalman Filter (LM-IEKF) algorithm, deducing the state updating equation and the recursive formula of covariance matrix in the iteration process of the LM-IEKF algorithm. The multiplicative noise introduced by the distance sensor due to the signal correlation characteristics makes the existing additive noise model difficult to adapt. Therefore, a Modified LM-IEKF method based on adaptive correction of measurement noise statistics is proposed. The filtering accuracy is improved by updating the noise matrix online in real time. Meanwhile, the results of fading memory index smoothing estimation are set up. Verification results of the algorithm show that compared with the existing EKF (Extended Kalman Filter) and IEKF algorithms, the LM-IEKF algorithm has better performance in additive noise only situation. In the case of containing multiplicative noise, Modified LM-IEKF can effectively estimate the measurement noise. Compared with the widely used EKF algorithm, the accuracy of integrated relative position and relative velocity improved by 10% and 23%.
[1] STACEY G, MAHONY R. A passivity-based approach to formation control using partial measurements of relative position[J]. IEEE Transactions on Automatic Control, 2016, 61(2):538-543.
[2] VETRELLA A, FASANO G, ACCARDO D, et al. Differential GNSS and vision-based tracking to improve navigation performance in cooperative multi-UAV systems[J]. Sensors, 2016, 16(12):2164-2172.
[3] XU Y, LUO D, XIAN N, et al. Pose estimation for UAV aerial refueling with serious turbulences based on extended Kalman filter[J]. Optik-International Journal for Light and Electron Optics, 2014, 125(13):3102-3106.
[4] 王小刚, 郭继峰, 崔乃刚. 一种鲁棒Sigma-point滤波算法及其在相对导航中的应用[J]. 航空学报, 2010, 31(5):1024-1029. WANG X G, GUO J F, CUI N G. Robust sigma-point filtering and its application to relative navigation[J]. Acta Aeronautica et Astronautica Sinica, 2010, 31(5):1024-1029(in Chinese).
[5] 于浛, 魏喜庆, 宋申民. 基于自适应容积卡尔曼滤波的非合作航天器相对运动估计[J]. 航空学报, 2014, 35(8):2251-2260. YU H, WEI X Q, SONG S M. Relative motion estimation of non-cooperative spacecraft based on adative CKF[J]. Aata Aeronautica et Astronautica Sinica, 2014, 35(8):2251-2260(in Chinese).
[6] LI J, ZHAO Y J, LI D H, et al. Accurate single-observer passive coherent location estimation based on TDOA and DOA[J]. Chinese Journal of Aeronautics, 2014, 27(4):913-923.
[7] 王洪, 刘昌忠, 汪学刚, 等. 一种多点定位的目标位置精确解算方法[J]. 航空学报, 2011, 32(7):1269-1274. WANG H, LIU C Z, WANG X G, et al. An accurate target localization method for multilateration[J]. Acta Aeronautica et Astronautica Sinica, 2011, 32(7):1269-1274(in Chinese).
[8] CHEN X, WANG D, LIU R, et al. Structural total least squares algorithm for locating multiple disjoint sources based on AOA/TOA/FOA in the presence of system error[J]. Frontiers of Information Technology & Electronic Engineering, 2018, 19(7):917-936.
[9] HU D, ZHEN H, ZHANG S, et al. Joint TDOA, FDOA and differential doppler rate estimation:method and its performance analysis[J]. Chinese Journal of Aeronautics, 2018, 31(1), 137-147.
[10] 吴铁洲, 罗蒙, 张琪, 等. 基于迭代扩展卡尔曼滤波算法的电池SOC估算[J]. 电力电子技术, 2016, 50(5):95-98. WU T Z, LUO M, ZHANG Q, et al. Battery SOC estimation based on iterative extended Kalman filter algorithm[J]. Power Electronics, 2016, 50(5):95-98(in Chinese).
[11] 杨勇. 基于IEKF的目标、外辐射源联合跟踪滤波[J]. 现代雷达, 2013, 35(2):31-34. YANG Y. An united tracking algorithm based on IEKF for objects and external emitter[J]. Modern Radar, 2013, 35(2):31-34(in Chinese).
[12] 常国宾, 许江宁, 李安, 等. 基于组合牛顿迭代法的改进IEKF及其在UNGM中的应用[J]. 海军工程大学学报, 2012, 24(1):15-19. CHANG G B, XU J N, LI A, et al. Modified iterated extended Kalman filter based on Gauss-Newton iteration and its application in UNGM[J]. Journal of Naval University of Engineering, 2012, 24(1):15-19(in Chinese).
[13] 张俊根, 姬红兵. 基于修正IEKF的IRST系统多站融合跟踪[J]. 系统工程与电子技术, 2010, 32(3):504-507. ZHANG J G, JI H B. Modified iterated extended Kalman filter based multi-observer fusion tracking for IRST[J].Systems Engineering and electronics, 2010, 32(3):504-507(in Chinese).
[14] HU F Q, XU B G, HU Y H. Generalised Kalman filter tracking with multiplicative measurement noise in a wireless sensor network[J]. IET Signal Processing, 2014, 8(5):467-474.
[15] 吴骁航, 宋申民. 具有乘性噪声和随机量测时滞的目标跟踪算法[J]. 中国惯性技术学报, 2017, 25(1):128-135. WU X H, SONG S M. Target tracking algorithm for system with multiplicative noises and randomly delayed measurements[J]. Journal of Chinese Inertial Technology, 2017, 25(1):128-135(in Chinese).
[16] 王炯琦, 矫媛媛, 周海银, 等.适合处理乘性噪声估计卫星姿态的非线性迭代滤波算法[J]. 电子学报, 2011, 39(6):1417-1422. WANG J Q, JIAO Y Y, ZHOU H Y, et al. An iterative filter for nonlinear satellite attitude determination system with multiplicative stochastic matrix[J]. Acta Electronica Sinica, 2011, 39(6):1417-1422(in Chinese).
[17] 杨宏, 李亚安, 李国辉. 一种改进扩展卡尔曼滤波新方法[J]. 计算机工程与应用, 2010, 46(19):18-20. YANG H, LI Y A, LI G H. New method of improved extended Kalman filter[J]. Computer Engineering and Applications, 2010, 46(19):18-20(in Chinese).
[18] WILAMOWSKI B, YU H. Improved computation for Levenberg-Marquardt training[J]. IEEE Transactions on Neural Networks, 2010, 21(6):930-7.
[19] NADJIASNGAR R, INGGS M. Gauss-Newton filtering incorporating Levenberg-Marquardt methods for tracking[J]. Digital Signal Processing, 2013, 23(5):1662-1667.
[20] DUC-HUNG L, CONG-KHA P, TRANG N, et al. Parameter extraction and optimization using Levenberg-Marquardt algorithm[C]//Fourth International Conference on Communications & Electronics. Piscataway, NJ:IEEE Press, 2012.
[21] 石勇, 韩崇昭. 自适应UKF算法在目标跟踪中的应用[J]. 自动化学报, 2011, 37(6):755-759. SHI Y, HAN C Z. Adaptive UKF method with applications to target tracking[J]. Acta Automation Sinica, 2011. 37(6):755-759(in Chinese).
CJA