AP_NavEKF : IMU2 velocity state indexing bugfix