Added one step of mono-slam alg