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()
print("Dist Sensor: ", distanceSensorData)
# Simulation schliessen:
myWorld.close()
# Plot poses:
PlotUtilities.plotPositions(pos_true, 'k')
PlotUtilities.plotPositions(pos_odo, 'r')
PlotUtilities.plotShow()
