Invariant filtering for simultaneous localization and mapping

This paper presents an algorithm for simultaneous localization and map building for a mobile robot moving in an unknown environment. The robot can measure only the bearings to identifiable targets and its own relative motion. The approach is to recursively estimate features of the environment which are invariant to the robot pose in order to decouple the pose error from the map error. The highly nonlinear nature of this problem requires more explicit reasoning about the spatial relationships between landmarks and between the robot and landmarks than those used in previous methods