diff --git a/src/sensor_GPS_fix.cpp b/src/sensor_GPS_fix.cpp
index b97e43c49a8ff74befcd77024e54a37100e64c21..b259eecc002e0e71233e80644f0ccc1e9b546d4f 100644
--- a/src/sensor_GPS_fix.cpp
+++ b/src/sensor_GPS_fix.cpp
@@ -13,7 +13,8 @@ SensorGPSFix::SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const dou
 SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) :
         SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3)), std::make_shared<StateQuaternion>(_extrinsics.tail(4)), nullptr, _intrinsics.noise_std)
 {
-    assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
+    assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
+           && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
 }
 
 SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) : SensorGPSFix(_extrinsics, *_intrinsics_ptr)