AP_NavEKF: Add divergence check to filter health