Nonlinear SLAM