A First-Estimates Jacobian EKF for Improving SLAM Consistency
Consistency of the EKF-SLAM Algorithm