diff --git a/src/sensor_gps.cpp b/src/sensor_gps.cpp
index 28a9a40ddf2d53655304ffd95b8c03ede79e84dc..0321f21e30eb53f9425d3136294c9f95bf01ae6c 100644
--- a/src/sensor_gps.cpp
+++ b/src/sensor_gps.cpp
@@ -13,6 +13,7 @@ SensorGPS::SensorGPS(StateBlockPtr _p_ptr, //GPS sensor position with respect to
         :
         SensorBase("GPS", _p_ptr, _o_ptr, _bias_ptr, 0)
 {
+    getStateBlockVec().resize(5);
     setStateBlockPtr(3, _map_p_ptr); // Map position
     setStateBlockPtr(4, _map_o_ptr); // Map orientation
     //