AP_NavEKF: Reverted initial covariance calculation