Lab 11

Setup

Before starting, I first checked the functionality of the localization starter code by running the lab11_sim notebook. The results are shown below is closely follow the results from lab 10.

In order to implement a Bayes filter using the real robot, I first implemented the perform_observation_loop in the RealRobot class, which is called when the update loop needs to collect observations about the robot’s surroundings. I first send a MAP command which I implemented in lab 9 to collect 18 ToF readings which are seperated by 20 degrees. I also made this function async so that I could wait for 25 seconds for the localization spin to be complete before sending the GET_MAP_DATA command also from lab 9 to get the ToF measurement data. Since the localization code expects the measurements to be in counter-clockwise order by yaw instead of clockwise, I reverse the order of the list.

async def perform_observation_loop(self, rot_vel=120):
    global time_arr, mapping_arr, dist_arr, yaw_arr, measure_X, measure_Y
    time_arr, mapping_arr, dist_arr, yaw_arr = [], [], [], []

    def on_receive(uuid, bytearr):
        data = ble.bytearray_to_string(bytearr).split(",")
        time_arr.append(int(data[0]))
        mapping_arr.append(int(data[1])/1000.0) # convert from mm to m
        dist_arr.append(int(data[2]))
        yaw = float(data[3])
        yaw_arr.append(yaw if yaw >= 0 else yaw + 360)
    
    ble.send_command(CMD.MAP, "")
    await asyncio.sleep(25)
    
    ble.start_notify(ble.uuid['RX_STRING'], on_receive)
    ble.send_command(CMD.GET_MAP_DATA, "")
    await asyncio.sleep(5)
    mapping_arr.reverse()
    yaw_arr.reverse()
    mapping_arr = [mapping_arr[-1]] + mapping_arr[:-1]
    yaw_arr = [yaw_arr[-1]] + yaw_arr[:-1]

    return np.array(mapping_arr)[np.newaxis].T, np.array(yaw_arr)[np.newaxis].T

Below is a video of the robot performing a localization spin at the (-3, -2) coordinate:

(-3, -2) Scan

The robot was able to perfectly localize itself at (-3, -2), as the belief dot is drawn directly on top of the ground truth dot. I think the robot was able to localize well here because the location has many unique features since there are 3 surrounding walls and a large opening.

(0, 0) Scan

The robot was also able to perfectly localize itself at (0, 0). I think the robot was able to localize well here because the location is at the center of the map so it can see many different wall features.

(0, 3) Scan

The robot was also able to perfectly localize itself at (0, 3), but for some reason taking ToF measurements 20 degrees apart caused it to consistently localize on the completely incorrect part of the map, as shown in the image below. I fixed this by changing the code to use the lab 9 behavior of taking 24 ToF measurements 15 degrees apart. One reason this could have happened is because for a large part of the rotation ToF sensor distances maxxing out, which may cause the filter to localize to the wrong area.

(5, -3) Scan

The robot was able to perfectly localize itself at (5, -3). I think the robot was able to localize well here because the location has many unique features since there are 2 sorrounding walls and two openings.

(5, 3) Scan

The robot was mostly able to localize itself at (5, 3). One reason for this could be the ToF sensor distances maxxing out, causing the robot to think it was closer to the bottom wall than it actually was.

Acknowledgements: I referenced Aidan Derocher’s website from Spring 2025 for inspiration