sigma_pose[1,1] = 0.2**2
sigma_pose[2,2] = (5*pi/180)**2
posEstimator.setInitialCovariance(sigma_pose)
posEstimator.setInitialPose(poseStart)
#posEstimator.setTimestep(T)
# Move robot and plot true and odo poses:
pos_odo = [myWorld.getTrueRobotPose()]
pos_true = [posEstimator.getPose()]
for t in range(n):
# plot covariance:
if t % 10 == 0:
PlotUtilities.plotPoseCovariance(posEstimator.getPose(), posEstimator.getCovariance(), 'b')
# move robot
motion = motionCircle[t]
print("v = ", motion[0], "omega = ", motion[1]*180/pi)
myRobot.move(motion)
# add true pose
pos_true.append(myWorld.getTrueRobotPose())
# odo pose:
posEstimator.integrateMovement(motion,myRobot.getSigmaMotion())
pos_odo.append(posEstimator.getPose())
# Gib Daten vom Distanzsensor aus:
distanceSensorData = myRobot.sense()
