bugfix: fix crash when launching SLAM with Kalman + GPS.
bugfix: fix crash when launching SLAM with Kalman + GPS.

* This was caused by a bad index when computing the error vector in the Kalman filter

Kitware/VeloView,Kitware/VeloView,Kitware/VeloView,Kitware/VeloView,Kitware/VeloView