Abstract— We implemented an odometry estimation, a PID controller, Simultaneously Localization and Mapping, A* planning and an exploration algorithm. We also used VX world to visualize and test how well our methods were working. This whole task is to build up a system to explore a map, find a key to the treasure chest and then sneak out the map.