Fixed altitude calculation bug

Tweaked AHRS variables to converge faster