Localization and Mapping

This milestone will allow the robot to build a map and localize itself to that map. It will be impossible to know our environment completely and so the map should be probabilistic (something like SLAM). This will also encompass sensor fusion and will use something like an extended kalman filter to ensure that we are accurately calculating our position using all the data we have at our disposal.

This will likely rely heavily on the Sensing milestone but test data could be generated as a proof of concept.