updating beliefDynamics in slam.h