Heng Wang, Shoudong Huang, Guanghong Yang, Gamini Dissanayake, Comparison of two different objective functions in 2D point feature SLAM, Automatica,
Volume 97, 2018, Pages 172-181, DOI: 10.1016/j.automatica.2018.08.009.
This paper compares two different objective functions in 2D point feature Simultaneous Localization and Mapping (SLAM). It is shown that the objective function can have a significant impact on the convergence of the iterative optimization techniques used in SLAM. When Frobenius norm is adopted for the error term of the orientation part of odometry, the SLAM problem has much better convergence properties, as compared with that using the angle difference as the error term. For one-step case, we have proved that there is one and only one minimum to the SLAM problem, and strong duality always holds. For two-step case, strong duality always holds except when three very special conditions hold simultaneously (which happens with probability zero), thus the global optimal solution to primal SLAM problem can be obtained by solving the corresponding Lagrangian dual problem in most cases. Further, for arbitrary m-step cases, we also show using examples that much better convergence results can be obtained. Simulation examples are given to demonstrate the different convergence properties using two different objective functions.