Small change for a slight efficiency increase

Also handled potential aliasing in EKF correction