diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp
index f74b3baa23383ea5265d7d288abc8d97d225a0ff..ca6130465acdac333330b3c97c656ac12a038778 100644
--- a/src/sensor/sensor_imu2d.cpp
+++ b/src/sensor/sensor_imu2d.cpp
@@ -19,8 +19,9 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-#include <imu/sensor/sensor_imu2d.h>
-#include <core/state_block/state_block.h>
+#include "imu/sensor/sensor_imu2d.h"
+
+#include <core/state_block/state_block_derived.h>
 #include <core/state_block/state_angle.h>
 
 namespace wolf {
@@ -31,20 +32,27 @@ SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPt
     //
 }
 
-SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) :
-        SensorBase("SensorImu2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), std::make_shared<StateBlock>(3, false, nullptr), (Eigen::Vector3d()<<_params.a_noise,_params.a_noise,_params.w_noise).finished(), false, false, true),
-        a_noise(_params.a_noise),
-        w_noise(_params.w_noise),
-        ab_initial_stdev(_params.ab_initial_stdev),
-        wb_initial_stdev(_params.wb_initial_stdev),
-        ab_rate_stdev(_params.ab_rate_stdev),
-        wb_rate_stdev(_params.wb_rate_stdev),
-        orthogonal_gravity(_params.orthogonal_gravity)
+SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params)
+    : SensorBase("SensorImu2d",
+                 std::make_shared<StatePoint2d>(_extrinsics.head(2), true, false),
+                 std::make_shared<StateAngle>(_extrinsics(2), true, false),
+                 std::make_shared<StateParams3>(Vector3d::Zero(3), false),
+                 (Eigen::Vector3d() << _params.a_noise, _params.a_noise, _params.w_noise).finished(),
+                 false,
+                 false,
+                 true),
+      a_noise(_params.a_noise),
+      w_noise(_params.w_noise),
+      ab_initial_stdev(_params.ab_initial_stdev),
+      wb_initial_stdev(_params.wb_initial_stdev),
+      ab_rate_stdev(_params.ab_rate_stdev),
+      wb_rate_stdev(_params.wb_rate_stdev),
+      orthogonal_gravity(_params.orthogonal_gravity)
 {
     assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
-    if(!orthogonal_gravity)
+    if (!orthogonal_gravity)
     {
-        addStateBlock('G', std::make_shared<StateBlock>(2, false), false);
+        addStateBlock('G', std::make_shared<StateVector2d>(Vector2d::Zero(), false, true), false);
     }
 }