Change wall sonar mode filter to 5 elements to reject more of the bad data.  Then apply a further lowpass filter to smooth data more.
Put tighter contraints on output of the controller so that the pilot can over-ride easily if something is wrong.