AP_NavEKF: improved covariance matrix numerical stability protection