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()