Lab 11

Localization and Bayes Filtering

For this lab I needed to download the localization modules and the needed jupyter notebooks and moved them into the correct directories.

For the first task, I tested the provided localization and Baye's filter code to see if it would give simular results to the one from lab 10. Below is an iages of the final result. The red line is the odometry, the green line is the groun truth, and the blue line is the belief. I noticed this simulation result compared to other students the odemetry did not go crazy and kept within the map boundaries surprisingly.

Arduino Code

After getting the simulation results, I started implementing localization to work on the real world. To ensure I could get the data, I needed to slightly change my mapping command. Since in lab 9, I had already performed mapping to get the robot to rotate 360 degrees, I just changed my increment value from 24 (lab 9) to 20 for lab 11 to signify it would take 18 sensor readings at 20 degree intervals. I also needed the car to rotate in a counter clockwise direction rather than clockwise like it did in lab 9. To achieve this I just changed my increment from 20 degrees to negative 20 degrees.

Then I completed the function perform_observation_loop(). I had this function call my START_MAP command that would have a notification handler that would collect the sent over TOF and yaw sensor data. It stores these data points into two array called sensor_ranges and sensor_bearings. The function initializes the arrays then it calls the notification handler then send the START_MAP command. I used asyncio.run(asyncio.sleep(20)) to ensure that all the data would be collected before using the raw sensor readigns and formatting the data as a NumPy column vector. Finally I returned these processed arrays so that it could be used later for the Bayes filter algorithm.

Localization

After setting up the arduino and the python code for the bayes filter algorithm I started to do localization on the given map in the lab at different points.

(-3, -2)

For this point, it was slightly off from the ground truth value. This was probably because the walls are not that defined which could result in an error with the localization. The belief was 0.3044 meters below the ground truth value.

(0, 3)

Surprisingly for this graph, the algorithm did a great job and the belief was accurate each run.

(5, -3)

Unfortunately for this graph, the belief was pretty far off from the ground truth state. This was probably because of the same reasons as (-3, -2). The belief was 0.6819 meters from the ground truth state.

(5, 3)

This time I was able to get closer results but still not as accurate as (0. 3). I think this might have been because the smaller box was causing some confusion. The belief was 0.3044 meters from the ground truth.

After performing one run, I decided to do another run. Below is a table of all the Beliefs for each marked position and run.

	Beliefs Run 1	Beliefs Run 2
(-3, -2)	(-0.914, -0.914, -10.000)	(-0.890, -0.930, -10.000)
(0, 3)	(0.000, 0.914, -30.000)	(0.000, 0.910, -30.000)
(5, -3)	(0.914, -1.219, -50.000)	(0.945, -1.200, -50.000)
(5, 3)	(1.829, 0.610, -30.000)	(1.810, 0.590, -30.000)