包含乘性噪声自适应修正的非合作目标相对导航算法
|
摘要
无人机(UAV)态势感知的任务是利用机载传感器对未知环境进行目标识别和引导,针对无人机与非合作目标间中远距离的相对导航问题,提出了一种基于角度和距离量测的相对状态估计算法。在现有滤波算法的基础上,为了提高精度和稳定性,本文利用了列文伯格-马夸尔特(LM)优化的思想对迭代卡尔曼滤波(IEKF)算法进行改进,提出了一种LM-IEKF算法,并推导该算法在迭代过程中的状态更新方程及协方差阵的递推公式。在此基础上,考虑到距离传感器由于信号相关特性而引入的乘性噪声,现有的加性噪声模型难以适应,因此,进一步提出了基于量测噪声自适应修正的Modified LM-IEKF方法,通过在线实时更新噪声阵提高滤波的精度,并设置渐消记忆指数平滑估计结果。算法验证结果表明,与现有的EKF、IEKF算法相比,在仅含加性噪声的情况下,LM-IEKF算法具有更好的性能;在包含乘性噪声的情况下,Modified LM-IEKF可以有效地估计量测噪声,与目前广泛使用的EKF算法相比,在综合相对位置和相对速度精度上分别提高了10%和23%。
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 LevenbergMarquardt 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%.
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 LevenbergMarquardt 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 UAVaerial 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,FDOAand 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 IEKFfor 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:IEEEPress,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).
[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 UAVaerial 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,FDOAand 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 IEKFfor 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:IEEEPress,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).