AP_NavEKF: Added improved covariance matrix error fix