Improving the way sonar filtering is done.  Use a low pass float filter so that we don't have step-changes in sonar distance which can lead to spikes on the speed calculation.
Improving control loops. Added Integrator clamp.