Changes to run test_ekf_pose_slam