First real working version! Estimation in the y-direction looks a bit crummier than the x-direction. Fixed bounds checking bug in MotorController