Dual hypothesis filter for robust INS/camera fusion

This paper presents a sensor fusion methodology that enhances the robustness (here defined as resistance to divergence) of filters used to mechanize camera-aided inertial navigation systems (INS). Maintaining stability of the filter that fuses camera and INS information can be challenging when low quality (consumer/automotive grade) inertial sensors are used. This is because the optimal fusion strategy between camera pixel measurements and INS relies on a non-linear measurement equation which is not well-behaved in some unfavorable landmark geometries. In these cases, the filter estimates can diverge. There are camera-INS fusion strategies which are less divergence-prone but less flexible and accurate. In view of above, it may be beneficial to design a filter that can switch between optimal and suboptimal strategies "on the fly" depending on the geometry of the landmarks being tracked and the quality of the inertial sensor. This paper proposes such a strategy based on dual hypothesis testing approach. The proposed approach has the advantages of enhancing the robustness while maintaining the estimation accuracy. The filter performance is examined and validated using simulated UAV flight data.