New height filter using advanced sonar filtering.

Height mode working.
Sonar working. Todo: adjust PID for optical flow.