Equality Constrained Robust Measurement Fusion for Adaptive Kalman-Filter-Based Heterogeneous Multi-Sensor Navigation

Constraints can provide additional aids to a multi-sensor integrated navigation system and hence can increase accuracy of the solution and enhance reliability of the system. To integrate the constraints with the data from the sensors, the traditional integration Kalman filter (IKF) needs to be reconstructed. A new algorithm, the so-called constrained adaptive robust integration Kalman filter (CARIKF) is presented, which implements adaptive integration upon the robust direct fusion solution. In the algorithm the raw observations from all heterogeneous sensors are corrected by the pseudoobservations derived from state equality constraint. The posterior covariances of the corrected observations are subsequently estimated upon the robust maximum-likelihood-type estimation (M-estimation) theory. The fusion state and its covariance are solved for all sensors further in the least squares (LS) sense. The pseudoobservations are constructed according to the estimated state and its covariance. They are further combined with the dynamic model of the host platform in an adaptive Kalman filter (AKF), from which a reliable and accurate navigation solution can be then obtained. A state constraint model is proposed upon Newton's forward differential extrapolation numerical method. To demonstrate performance of the CARIKF algorithm, simulations have been conducted in different dynamic and observation scenarios. Several algorithms are compared to evaluate the validity and efficiency of the CARIKF. The results show that the CARIKF is superior to other algorithms and can significantly improve the precision and reliability of the integrated solution.