PID for sonar altitude hold and low-pass filter for accelerometer