Initial commit. Selected the code that needs to be changed.
Added wall detection function.
Added a 3 value median filter to the motion controller.
Added functions to process heading estimates.