Simultaneous calibration, localization, and mapping
Covariance recovery from a square root information matrix for data association