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