Converging when the IMU covariance matrix is diagonal with variances of 1e-7