We describe a trajectory optimization framework that maximizes observability of one or more user-chosen states in a nonlinear system. Our framework is based on a novel metric for quality of observability that is state-estimator agnostic and offers improved numerical stability over prior methods in some cases where the states of interest do not appear directly in the observation. We apply this metric to trajectory optimization problems for closed-loop self-calibration, maintaining observability while navigating through an environment, and rapidly modifying an already-planned trajectory for online recalibration. We include a statistical procedure to balance observability of several states with heterogeneous units and magnitudes. As an example, we apply our framework to online calibration of GPS–IMU and visual–inertial navigation systems on a quadrotor helicopter. Extensive simulations and a real-robot experiment demonstrate the effectiveness of our framework, showing better convergence of the states and the resulting higher precision in navigation.
AchtelikMWeissSChliMSiegwartR (2013) Path planning for motion dependent state estimation on micro aerial vehicles. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 3926–3932.
2.
BähnemannRBurriMGalceranESiegwartRNietoJI (2017) Sampling-based motion planning for active multirotor system identification. In: 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 3931–3938.
3.
BeinhoferMMullerJKrauseABurgardW (2013) Robust landmark selection for mobile robot navigation. In: International Conference on Intelligent Robots and Systems (IROS), pp. 3637–2643.
4.
BélisleCJPRomeijnHESmithRL (1993) Hit-and-run algorithms for generating multivariate distributions. Mathematics of Operations Research18(2): 255–266.
5.
BotevZI (2017) The normal law under linear restrictions: simulation and estimation via minimax tilting. Journal of the Royal Statistical Society: Series B (Statistical Methodology)79(1): 125–148.
6.
BrumbackBSrinathMD (1987) A chi-square test for fault-detection in Kalman filters. IEEE Transactions on Automatic Control32(6): 552–554.
7.
BryARoyN (2011) Rapidly-exploring random belief trees for motion planning under uncertainty. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 723–730.
8.
BrysonMSukkariehS (2008) Observability analysis and active control for airborne SLAM. IEEE Transactions on Aerospace and Electronic Systems44(1): 261–280.
9.
ButcherJC (1996) A history of Runge–Kutta methods. Applied Numerical Mathematics20(3): 247–260.
DeitsRTedrakeR (2015) Computing large convex regions of obstacle-free space through semidefinite programming. In: Algorithmic Foundations of Robotics XI. New York: Springer, pp. 109–124.
12.
FloresME (2008) Real-time trajectory generation for constrained nonlinear dynamical systems using non-uniform rational b-spline basis functions. Ph.D. Dissertation, California Institute of Technology.
13.
HausmanKMüllerJHariharanAAyanianNSukhatmeG (2015) Cooperative multi-robot control for target tracking with onboard sensing. The International Journal of Robotics Research34(13): 1660–1677.
14.
HausmanKPreissJASukhatmeGSWeissS (2017) Observability-aware trajectory optimization for self-calibration with application to uavs. IEEE Robotics and Automation Letters2(3): 1770–1777.
15.
HermannRKrenerA (1977) Nonlinear controllability and observability. IEEE Transactions on automatic control22(5): 728–740.
16.
HernandezJTsotsosKSoattoS (2015) Observability, identifiability and sensitivity of vision-aided inertial navigation. In: 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 2319–2325.
17.
HeschJAKottasDGBowmanSLRoumeliotisSI (2014) Camera–IMU-based localization: Observability analysis and consistency improvement. The International Journal of Robotics Research33(1): 182–201.
18.
HinsonBMorgansenK (2013) Observability optimization for the nonholonomic integrator. In: 2013 American Control Conference (ACC), pp. 4257–4262.
19.
HinsonBTBinderMKMorgansenKA (2013) Path planning to optimize observability in a planar uniform flow field. In: 2013 American Control Conference (ACC), pp. 1392–1399.
20.
HollingerGASukhatmeGS (2013) Sampling-based motion planning for robotic information gathering. In: Robotics: Science and Systems IX.
21.
IndelmanVCarloneLDellaertF (2015) Planning in the continuous domain: A generalized belief space approach for autonomous navigation in unknown environments. The International Journal of Robotics Research34(7): 849–882.
JulianBJAngermannMSchwagerMRusD (2012) Distributed robotic sensor networks: An information-theoretic approach. The International Journal of Robotics Research31(10): 1134–1154.
24.
KaramanSFrazzoliE (2011) Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research30(7): 846–894.
25.
KellyJSukhatmeGS (2011) Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration. The International Journal of Robotics Research30(1): 56–79.
KleinGMurrayD (2007) Parallel tracking and mapping for small AR workspaces. In: IEEE and ACM International Symposium on Mixed and Augmented Reality. IEEE, pp. 225–234.
28.
KrenerAIdeK (2009) Measures of unobservability. In: Proceedings of the 48th IEEE Conference on Decision and Control, pp. 6401–6406.
29.
LeffertsEJMarkleyFLShusterMD (1982) Kalman filtering for spacecraft attitude estimation. Journal of Guidance, Control, and Dynamics5(5): 417–429.
30.
LynenSAchtelikMWeissSChliMSiegwartR (2013) A robust and modular multi-sensor fusion approach applied to mav navigation. In: In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3923–3929.
31.
MartinPMurrayRMRouchonP (2003) Flat systems, equivalence and trajectory generation. Technical Report, California Institute of Technology.
32.
MartinelliASiegwartR (2006) Observability properties and optimal trajectories for on-line odometry self-calibration. In: 2006 45th IEEE Conference on Decision and Control, pp. 3065–3070.
33.
MayeJSommerHAgamennoniGSiegwartRFurgalePT (2016) Online self-calibration for robotic systems. The International Journal of Robotics Research35(4): 357–380.
34.
MellingerDKumarV (2011) Minimum snap trajectory generation and control for quadrotors. In: International Conference on Robotics and Automation (ICRA). IEEE, pp. 2520–2525.
35.
MüllerJSukhatmeG (2014) Risk-aware trajectory generation with application to safe quadrotor landing. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3642–3648.
36.
NobreFKasperMHeckmanC (2017) Drift-correcting self-calibration for visual-inertial SLAM. In: 2017 IEEE International Conference on Robotics and Automation (ICRA 2017), Singapore, 29 May–3 June 2017, pp. 6525–6532.
37.
PreissJHausmanKSukhatmeGWeissS (2017) Trajectory optimization for self-calibration and navigation. In: Proceedings of Robotics: Science and Systems, Cambridge, MA.
38.
RatliffNDZuckerMBagnellJASrinivasaSS (2009) CHOMP: gradient optimization techniques for efficient motion planning. In: IEEE International Conference on Robotics and Automation, (ICRA), pp. 489–494.
39.
RichterCBryARoyN (2013) Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments. In: Proceedings of the International Symposium on Robotics Research (ISRR), pp. 649–666.
40.
SchneiderTLiMBurriMNietoJISiegwartRGilitschenskiI (2017) Visual-inertial self-calibration on informative motion segments. In: 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 6487–6494.
41.
SunWPatilSAlterovitzR (2015) High-frequency replanning under uncertainty using parallel sampling-based motion planning. IEEE Transactions on Robotics31(1): 104–116.
42.
TangSKumarV (2016) Safe and complete trajectory generation for robot teams with higher-order dynamics. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1894–1901.
43.
TodorovELiW (2005) A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems. In: Proceedings of the American Control Conference, vol. 1, pp. 300–306.
44.
TraversMChosetH (2015) Use of the nonlinear observability rank condition for improved parametric estimation. In: 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 1029–1035.
45.
TrefethenLBauD (1997) Numerical Linear Algebra. Philadelphia, PA: Society for Industrial and Applied Mathematics.
46.
TribouMJWangDWLWaslanderSL (2015) Degenerate motions in multicamera cluster SLAM with non-overlapping fields of view. CoRR abs/1506.07597.
47.
VondrákJ (2007) Submodularity in combinatorial optimization. Doctoral Thesis, Stanford University, Stanford, CA.
48.
WeissS (2012) Vision Based Navigation for Micro Helicopters. PhD thesis, ETH Zurich.
49.
WeissSAchtelikMLynenSet al. (2013) Monocular vision for long-term micro aerial vehicle state estimation: A compendium. Journal of Field Robotics30(5): 803–831.
