AP_NavEKF: Bug fix in covariance error correction