Robust bias compensation Kalman filter algorithm for three-dimensional AoA-ToA target tracking

In the three-dimensional target tracking with angle of arrival (AoA) and time of arrival (ToA) measurements, the outliers lead to significant performance degradation or even divergence of nonlinear filter. Aiming at this problem, an M-estimation-based robust bias compensation Kalman filter (MR-BCKF) algorithm is proposed in this paper. The algorithm pseudo-linearizes the nonlinear measurement equations through the equivalent geometric relationship between AoA and ToA, and then derives the robust pseudo-linear Kalman filter by exploiting the M estimation criterion, followed by the bias compensation to improve the tracking accuracy. Moreover, the MR-BCKF uses Mahalanobis distance to distinguish outliers, which does not depend on the noise statistics, and enhances the robustness by constructing an improved three-segment weight function. Simulation results shows that compared with other robust Kalman filter, the MR-BCKF can not only improve the suppression effect of isolated outliers, but also achieve higher tracking accuracy in the case of continuous outliers.