Showcase · Systems

Sensor Fusion Playground

A quaternion error-state Kalman filter (ESKF) fuses a simulated strapdown IMU with up to nine aiding sensors — GPS position and Doppler velocity, a barometer, a magnetometer, a downward LiDAR altimeter, UWB radio ranging, optical flow, a Doppler velocity log, and a direct attitude fix (nine measurement models in all) — rendering the estimate as a quadrotor, the ground truth as a faint ghost, and the filter’s live position-uncertainty ellipsoid, in 3D. Pick a scenario preset or toggle any sensor: drop GPS and the ellipsoid balloons to metres, then turn on UWB ranging and it snaps back, localised with no GPS at all. A second Analytics tab lays the filter bare in numbers — each error-state block’s estimate, its 1σ from √diag(P) and its live error, the 15×15 covariance as a correlation heatmap, scrolling consistency charts, and every sensor’s innovation and NIS gate. The interesting part is doing the quaternion filter right — a minimal body-frame error state, exact finite-difference-checked Jacobians for every sensor, and a covariance that actually matches the error, verified by a Monte-Carlo NEES consistency gate (in aggregate and over time), not just eyeballed.
Drag to orbit, scroll to zoom. The blue quadrotor is the estimate and the faint green one is ground truth; the orange shell is the 95% position-uncertainty ellipsoid, and the purple markers are the UWB beacons. Toggle sensors in the panel and watch the error hold inside its 3σ envelope on the consistency plot. The native harness (cargo run -p eskf-cli -- check) is what proves the covariance honest.

Paper & references

The full derivation — the error-state transition, the six measurement Jacobians, the injection and covariance-reset steps, and the NEES consistency methodology — together with the simulation results is written up as a short technical paper.

Read the paper (PDF)
  1. [1]R. E. Kalman, “A new approach to linear filtering and prediction problems,” J. Basic Eng., 1960. Google Scholar
  2. [2]J. Solà, “Quaternion kinematics for the error-state Kalman filter,” 2017. arXiv:1711.02508
  3. [3]N. Trawny and S. I. Roumeliotis, “Indirect Kalman filter for 3D attitude estimation,” Univ. of Minnesota, Tech. Rep. 2005-002, 2005. Google Scholar
  4. [4]E. J. Lefferts, F. L. Markley, and M. D. Shuster, “Kalman filtering for spacecraft attitude estimation,” J. Guidance, Control, and Dynamics, 1982. Google Scholar
  5. [5]F. L. Markley, “Attitude error representations for Kalman filtering,” J. Guidance, Control, and Dynamics, 2003. Google Scholar
  6. [6]J. L. Crassidis, F. L. Markley, and Y. Cheng, “Survey of nonlinear attitude estimation methods,” J. Guidance, Control, and Dynamics, 2007. Google Scholar
  7. [7]M. D. Shuster, “A survey of attitude representations,” J. Astronautical Sciences, 1993. Google Scholar
  8. [8]Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation, Wiley, 2001. Google Scholar
  9. [9]P. S. Maybeck, Stochastic Models, Estimation, and Control, Vol. 1, Academic Press, 1979. Google Scholar
  10. [10]A. Gelb (ed.), Applied Optimal Estimation, MIT Press, 1974. Google Scholar
  11. [11]C. F. Van Loan, “Computing integrals involving the matrix exponential,” IEEE Trans. Automatic Control, 1978. Google Scholar
  12. [12]J. A. Farrell, Aided Navigation: GPS with High Rate Sensors, McGraw-Hill, 2008. Google Scholar
  13. [13]P. D. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd ed., Artech House, 2013. Google Scholar
  14. [14]D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation Technology, 2nd ed., IET, 2004. Google Scholar
  15. [15]M. Kok, J. D. Hol, and T. B. Schön, “Using inertial sensors for position and orientation estimation,” Foundations and Trends in Signal Processing, 2017. Google Scholar
  16. [16]A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” IEEE ICRA, 2007. Google Scholar
  17. [17]T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robotics, 2018. Google Scholar
  18. [18]A. Barrau and S. Bonnabel, “The invariant extended Kalman filter as a stable observer,” IEEE Trans. Automatic Control, 2017. Google Scholar
  19. [19]M. Kaess et al., “iSAM2: Incremental smoothing and mapping using the Bayes tree,” Int. J. Robotics Research, 2012. Google Scholar
  20. [20]C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Trans. Robotics, 2017. Google Scholar
  21. [21]G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis, “Observability-based rules for designing consistent EKF SLAM estimators,” Int. J. Robotics Research, 2010. Google Scholar
  22. [22]J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis, “Consistency analysis and improvement of vision-aided inertial navigation,” IEEE Trans. Robotics, 2014. Google Scholar
  23. [23]A. Martinelli, “Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination,” IEEE Trans. Robotics, 2012. Google Scholar
  24. [24]S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,” Proc. IEEE, 2004. Google Scholar
  25. [25]M. W. Mueller, M. Hamer, and R. D’Andrea, “Fusing ultra-wideband range measurements with accelerometers and rate gyroscopes for quadrocopter state estimation,” IEEE ICRA, 2015. Google Scholar
  26. [26]D. Honegger, L. Meier, P. Tanskanen, and M. Pollefeys, “An open source and open hardware embedded metric optical flow CMOS camera,” IEEE ICRA, 2013. Google Scholar
  27. [27]M. Burri et al., “The EuRoC micro aerial vehicle datasets,” Int. J. Robotics Research, 2016. Google Scholar
  28. [28]D. Schubert et al., “The TUM VI benchmark for evaluating visual-inertial odometry,” IEEE/RSJ IROS, 2018. Google Scholar
  29. [29]J. Delmerico et al., “Are we ready for autonomous drone racing? The UZH-FPV drone racing dataset,” IEEE ICRA, 2019. Google Scholar
  30. [30]A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision meets robotics: The KITTI dataset,” Int. J. Robotics Research, 2013. Google Scholar