Bayesian Filtering Fusion of Optimization-Based Monocular Visual Localization and Autonomous Quadcopter Navigation

Visual localization is typically implemented using optimization algorithms. Because the optimization minimizes a cost function, their estimate is sensitive to noises, and estimated trajectory is usually jerk. The estimate can be fused with other estimates using Bayesian filter (BF) and smooth trajectory can be obtained; however, the optimization-based localization does not estimate uncertainty. In this study, we present an uncertainty estimation method for the optimization-based localization using Hessian matrix. The Hessian matrix is used in Newton method to calculate the increment. The increment is obtained by multiplying the inverse of Hessian and the gradient of the cost function. We consider that the inverse of Hessian could be used to indicate variability of the optimized states, and determine the uncertainty. We determine normal distribution over the estimate by the optimization-based localization and its uncertainty, that is, covariance, is determined using Hessian. In the experiment, we validate the uncertainty estimation method under different measurement conditions, and the effectiveness of the fusion using BF. Results show that the uncertainty can be appropriately estimated according to the conditions, and smooth trajectory can be estimated while maintaining the tracking accuracy to the visual localization result. Furthermore, we conduct autonomous flight navigation experiment with a quadcopter based on the presented localization, and its navigation is successfully performed. The software used in this work is publicly available.