Minor bug fix in AHRS::filter
One axis seems to have spontaneously reversed, commiting to compare
Added altitude control, pushing before switching to nested PID control