Copter: Improve surface tracking dynamics

This little fix will allow much higher sonar_gain without the
oscillating effect it used to bring.