  We study the nonlinear observability of a systems states in view of how well
they are observable and what control inputs would improve the convergence of
their estimates. We use these insights to develop an observability-aware
trajectory-optimization framework for nonlinear systems that produces
trajectories well suited for self-calibration. Common trajectory-planning
algorithms tend to generate motions that lead to an unobservable subspace of
the system state, causing suboptimal state estimation. We address this problem
with a method that reasons about the quality of observability while respecting
system dynamics and motion constraints to yield the optimal trajectory for
rapid convergence of the self-calibration states (or other user-chosen states).
Experiments performed on a simulated quadrotor system with a GPS-IMU sensor
suite demonstrate the benefits of the optimized observability-aware
trajectories when compared to a covariance-based approach and multiple
heuristic approaches. Our method is approx. 80x faster than the
covariance-based approach and achieves better results than any other approach
in the self-calibration task. We applied our method to a waypoint navigation
task and achieved a approx. 2x improvement in the integrated RMSE of the global
position estimates and approx. 4x improvement in the integrated RMSE of the
GPS-IMU transformation estimates compared to a minimal-energy trajectory
planner.
