Observability-Aware Trajectory Optimization for Self-Calibration with Application to UAVs

TL;DR

This paper introduces an observability-aware trajectory optimization framework for nonlinear systems, enhancing self-calibration in UAVs by generating trajectories that improve state estimation convergence and outperform existing methods in speed and accuracy.

Contribution

The authors develop a novel trajectory-optimization method that considers observability quality, significantly improving self-calibration and state estimation in UAVs over traditional approaches.

Findings

80x faster than covariance-based approach

2x improvement in global position RMSE

4x improvement in GPS-IMU transformation RMSE

Abstract

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.