用于实时弹道滤波的Sage-Husa改进算法

为了解决实时弹道测量数据滤波过程中量测噪声统计特性未知且时变的实际问题,对Sage-Husa算法进行了多种改进,提出了改进的Sage-Husa自适应卡尔曼滤波(improved Sage-Husa adaptive Kalman filter,ISHAKF)算法。该算法将量测噪声协方差估计矩阵变换为半正定矩阵和正定矩阵之和的形式,保证了量测噪声协方差估计矩阵的正定性,消除了量测噪声协方差估计矩阵非正定导致滤波异常的缺陷。设计了一种自适应遗忘因子,提升了滤波收敛速度,解决了量测噪声统计特性突变时Sage-Husa算法收敛较慢的问题。对卡尔曼增益矩阵进行了抗差改进,增强了算法的鲁棒性,削弱了野值对滤波效果的影响。分别对正定性改进、遗忘因子改进和抗差改进进行了对比仿真实验,对比结果验证了Sage-Husa算法改进的正确性和有效性。通过ISHAKF算法的实例应用,证明了该算法在实时弹道滤波上,具有更高的实时性、自适应性和抗差性,滤波效果提升明显。

Improved Sage-Husa Algorithm for Real-time Trajectory Filtering

In order to solve the practical problems of unknown and time-varying measurement noise statistics in real-time trajectory filtering,the algorithm of improved Sage-Husa adaptive Kalman filter(ISHAKF)was proposed based on the improvements of Sage-Husa Kalman filter algorithm. By converting the covariance estimation matrix of measurement noise into sum of a positive semi-definite matrix and a positive definite matrix,the algorithm ensures the positive definiteness of the measurement noise covariance estimation matrix. Thus,the defect of abnormal filtering caused by non-positive definite covariance estimation matrix of measurement noise can be eliminated. An adaptive forgetting factor was designed,which improved the filter convergence speed and overcomed the problem of the slow convergence speed of Sage-Husa algorithm when measurement noise statistics were abrupt. The robustness of Kalman gain matrix was improved to increase the robust performance of the algorithm and weaken the influence of outliers on the filtering effect. Then,comparative simulation experiments were carried out on the improvements of positive definiteness,forgetting factor and robustness separately. The comparison results verify the correctness and effectiveness of improved Sage-Husa algorithm. Through the example application of ISHAKF algorithm,it is proved that the real-time performance,adaptability and robustness of ISHAKF algorithm are better in real-time trajectory filtering. Also,the filtering effect of the algorithm is obviously promoted.