robot_sim.set_random_measurement_rate(0.05)
robot_sim.set_sim_time_step(0.1)
# ekf parameters
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)
