Fixes and Added Testing Class

Recalculates everything each time a point is added
RandomMappingThread causes the simulation or robot to move randomly
collecting data