AP_NavEKF: fixed indexing errors