Observability-Aware Trajectory Optimization for Self-Calibration with Application to UAVs
This work addresses the challenge of suboptimal state estimation due to unobservable trajectories in autonomous systems like UAVs, offering a practical solution for improved self-calibration, though it is incremental as it builds on existing trajectory optimization methods.
The paper tackles the problem of generating trajectories for self-calibration in nonlinear systems, such as UAVs, by developing an observability-aware optimization framework that avoids unobservable states, resulting in approximately 80x faster computation than a covariance-based method and significant improvements in estimation accuracy, including a 2x reduction in position error and 4x reduction in transformation error compared to a minimal-energy planner.
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.