AP_NavEKF : Improved stability of Z accel bias