Trajectory Optimization for Self-Calibration and Navigation

Trajectory generation approaches for mobile robots generally aim to optimize with respect to a cost function such as energy, execution time, or other mission-relevant parameters within the constraints of vehicle dynamics and obstacles in the environment. We propose to add the cost of state observability to the trajectory optimization in order to ensure fast and accurate state estimation throughout the mission while still respecting the constraints of vehicle dynamics and the environment. Our approach finds a dynamically feasible estimation-optimized trajectory in a sequence of connected convex polytopes representing free space in the environment. In addition, we show a statistical procedure that enables observability-aware trajectory optimization for heterogeneous states in the system both in magnitude and units, which was not supported in previous formulations. We validate our approach with extensive simulations of a visual-inertial state estimator on an aerial platform as a specific realization of our general method. We show that the optimized trajectories lead to more accurate navigation while eliminating the need for a separate calibration procedure.