Simulation

To begin this lab, I tested the provided simulated localization code to ensure that everything is setup properly. Below is a screenshot of the odom, ground truth, and belief indicating that the localization code is working.

As shown above, the code is working properly and matches the results from my own implementation in lab 9.

Code

In order to implement localization in reality, I needed to implement the preform_observation_loop function in the RealRobot class. To do this, I defined an array called dist and a notification handler that populates dist with sensor measurements from the robot.

Then, I defined a helper function called get_data which sends commands to start the TOF, IMU, and orientation controller on the robot. The function then pauses for 30 seconds to allow the robot to execute its observation loop. After that, the robot disables the TOF, IMU, and orientation controller and pauses for 3 seconds while the dist array is populated with values. Finally, that array is returned.

Finally, in the actual preform_observation_loop function, I simply call get_data and save the results to a file in case I need them for debugging. I then transform the dist array into a numpy column vector and return it along with an empty orientation array. (orientation data are not needed for localization)

The code on the Ble side is identical to that of lab 9.

Results

Below are the results from localizing at each grid point. The green point corresponds to the ground truth while the blue point corresponds to the robot’s belief. I uploaded two distinct plots when the dots were too close together.

(5,-3)

As shown from the plots above, the belief is perfectly accurate compared to the ground truth. I was pleasantly suprised by this as the map is more open in this area. However, it is important to note that the localization uses grid cells so there is still some rounding going on. Nevertheless, the localization worked really well here.

(-3,-2)

Again the filter is dead on. This makes sense as there are nearby walls on each side except one making localization easier compared to (5,-3). Therefore, this strong preformance is expected.

(0,3)

These readings were again dead on. Similar to (5,-3), I thought that this localization would be worse because of the more open nature. However it did a great job exactly localizing the robot. This really shows the power of the Bayes filter.

(5,3)

Suprisingly, this localization was worse than the other three. The localization got the x coordinate dead on while the y coordinate is a litte off. I noticed that for some reason on this square, the robot seems to drift a little more than on the other squares while scanning. I think that this could be a potential cause of the discrepency. In addition, the the somewhat openness of this map location could have further exascerbated this discrepency.

Conclusion

Overall, I really enjoyed this lab. Dispite a somewhat noisy TOF sensor, my robot was still able to localize itself exactly in most locations. I think that this demonstrates the power of probabalistic algorithms in robotics.