Towards Online Observability-Aware Trajectory Optimization for Landmark-based Estimators

NOTE: The authors have identified fundamental errors in the article. We are working on a corrected version, so check back soon. As autonomous systems rely increasingly on onboard sensing for localization and perception, the parallel tasks of motion planning and state estimation become increasingly coupled. This coupling is well-captured by augmenting the planning objective with a posterior covariance penalty -- however, online evaluation and optimization can be challenging, particularly for observation models with latent environmental dependencies (e.g., opportunistic landmarks). This paper addresses a number of fundamental challenges in efficient prediction and minimization of the posterior covariance particular to landmark- and SLAM-based estimators. First, we provide a measurement bundling approximation that enables high-rate sensors such as cameras to be approximated with fewer, low-rate updates. This also allows for landmark marginalization, for which we provide a novel recipe for computing the gradients necessary for optimization. Finally, we identify a class of measurement models for which the contributions from each landmark can be directly combined, making evaluation of the information gained at each timestep (nearly) independent of the number of landmarks. This opens the door for generalization to landmark distributions, foregoing the need for fully-specified linearization points. Taken together, these contributions allow SLAM estimators to be accurately and efficiently approximated, paving the way for online, observability-aware trajectory optimization.