Altitude hold cleanups

This contains some miscellaneous cleanups to the altitude hold code.  More importantly it adds an alarm for altitude hold which sets an error if there are any NANs from the EKF which.  Also it fixes the NANs to they don't occur anymore (but still good to have the alarm).