Copter: Improve surface tracking dynamics

Warning, not tested.
This little fix will allow much higher sonar_gain without (I hope) the
oscillating effect it used to bring.