Lab 11

Localization on the real robot

Task 1: Test Localization in Simulation

After copying the notebooks and Python files into the directories, I ran lab11_sim.ipynb and got the following plot:

Task 2: Run Bayes filter on real robot

Implement function perform_observation_loop

Below is my implementation of the function perform_observation_loop of class RealRobot:

• An empty list is first initialized to store the distance readings.
• A notification handler is created and started to receive the distance readings from the robot.
• The robot runs the LAB11_MAP command where it performs a 360 degree counter-clockwise rotation in 20 degree increments (Method 2 from Lab 9) and takes a total of 18 distance readings with the first sensor reading taken at the robot’s current heading.

• Because I am using BLE handlers to get the observation data from the real robot, I used asyncio.run(asyncio.sleep(3)) to collect the distance sensor readings. This while loop ensures that the distance list is full before running loc.update_step().
• I convert the distance list into a numpy column array and convert the readings from mm to meters. I also created a numpy column array for sensor_bearings which is 0 to 360 degrees in 20 degree increments.

Running the update step of Bayes filter for the four marked poses

For every marked position, I placed the robot facing towards the right side of the map (+X direction). I assume that the robot is collecting equidistant (in the angular space) sensor readings. I assume that the robot is making a perfect 20 degree turn for each increment through the scan for simplicity.

For each map, I plot the ground truth using cmdr.plot_gt().

(-3 ft, -2 ft, 0 deg)

sensor_ranges: [3040.0, 2060.0, 3069.0, 2326.0, 663.0, 668.0, 812.0, 981.0, 801.0, 761.0, 899.0, 1085.0, 900.0, 748.0, 794.0, 966.0, 918.0, 701.0]

Bel index: (np.int64(2), np.int64(2), np.int64(9)) with prob = 1.0

Bel_bar prob at index = 0.00051440329218107

Belief: (-0.914, -0.610, 10.000)

Ground Truth: (-0.9144, -0.6096, 0)

(0 ft, 3 ft, 0 deg)

sensor_ranges: [2358.0, 1377.0, 692.0, 493.0, 449.0, 514.0, 663.0, 790.0, 647.0, 650.0, 767.0, 2734.0, 2620.0, 2151.0, 2598.0, 1193.0, 876.0, 901.0]

Bel index: (np.int64(5), np.int64(7), np.int64(9)) with prob = 0.9999999

Bel_bar prob at index = 0.00051440329218107

Belief: (0.000, 0.914, 10.000)

Ground Truth: (0, 0.9144, 0)

(5 ft, -3 ft, 0 deg)

sensor_ranges: [394.0, 427.0, 537.0, 870.0, 2387.0, 742.0, 914.0, 3356.0, 2815.0, 1330.0, 976.0, 536.0, 411.0, 352.0, 393.0, 496.0, 420.0, 357.0]

Bel index: (np.int64(10), np.int64(1), np.int64(9)) with prob = 0.9959384

Bel_bar prob at index = 0.00051440329218107

Belief: (1.524, -0.914, 10.000)

Ground Truth: (1.524, -0.9144, 0)

(5 ft, 3 ft, 0 deg)

sensor_ranges: [406.0, 447.0, 584.0, 501.0, 427.0, 424.0, 490.0, 754.0, 1352.0, 2450.0, 2405.0, 593.0, 436.0, 537.0, 2193.0, 739.0, 492.0, 418.0]

Bel index: (np.int64(10), np.int64(6), np.int64(8)) with prob = 1.0

Bel_bar prob at index = 0.00051440329218107

Belief: (1.524, 0.610, -10.000)

Ground Truth: (1.524, 0.9144, 0)

Discussion

The localized pose was exactly on the ground truth for 3 of the 4 marked poses: (-3 ft, -2 ft, 0 deg), (0 ft, 3 ft, 0 deg), and (5 ft, -3 ft, 0 deg).

The only belief that was not the same spot as the ground truth was for the marked pose (5 ft, 3 ft, 0 deg). The difference between the belief and ground truth is 0.3044 meters, which is close to the discrete cell size defined in ‘world.yaml`. The belief is only 1 cell off in the y-direction.

Below is the transformed sensor readings plotted on the world map with walls from world.yaml:

The localized pose for (5 ft, 3 ft, 0 deg) doesn’t match the ground truth because that spot in the world is most ambiguous part of the map compared to the other three marked poses. The top-right corner of the world has geometric symmetry: a long straight wall on the right and a long straight wall on the top. This produces distance measurements that are similar across neighboring poses. Other parts of the world have more unique features that create asymmetry in distance readings.

Collaborators

I collaborated with Sam Zhen while working on this lab.