measurement_range_variance = 0.3 * 0.3 measurement_angle_variance = 5.0 * math.pi / 180.0 * 5.0 * math.pi / 180.0 ekf = EKF(start_x, start_y, start_yaw) ekf.add_landmark(2.0, 2.0) ekf.add_landmark(4.0, -4.0) ekf.add_landmark(-2.0, -2.0) ekf.add_landmark(-4.0, 4.0) ekf.set_odom_noises(5.0, 2.0, 2.0, 8.0) ekf.set_measurement_variances(measurement_range_variance, measurement_angle_variance) ekf.set_min_trace(0.001) ekf.set_plot_sizes(5.0, 5.0) while True: # simulate robot behaviors robot_sim.update_pose(0.2, -0.2) delta_dist = robot_sim.sim_v * robot_sim.sim_time_step delta_yaw = robot_sim.sim_w * robot_sim.sim_time_step measurements = robot_sim.get_sensor_measurements() # ekf ekf.predict(delta_dist, delta_yaw) ekf.update(measurements) ekf.print_estimated_pose() ekf.print_estimated_covariance() ekf.plot_ekf_world(measurements) # sleep time.sleep(robot_sim.sim_time_step)