Use derived SBs

src/sensor/sensor_compass.cpp

@@ -20,7 +20,7 @@
 //
 //--------LICENSE_END--------
 #include "imu/sensor/sensor_compass.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_quaternion.h"
 
 namespace wolf
@@ -29,10 +29,10 @@ namespace wolf
 SensorCompass::SensorCompass(const Eigen::VectorXd& _extrinsics,
                              ParamsSensorCompassPtr _params) :
         SensorBase("SensorCompass",
-                   std::make_shared<StateBlock>(_extrinsics.head(3), true),
-                   std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
-                   std::make_shared<StateBlock>(_params->bias_prior_state,
-                                                _params->bias_prior_mode == "fix"),
+                   std::make_shared<StatePoint3d>(_extrinsics.head(3), true, false),
+                   std::make_shared<StateQuaternion>(_extrinsics.tail(4), true, false),
+                   std::make_shared<StateParams3>(_params->bias_prior_state,
+                                                  _params->bias_prior_mode == "fix"),
                    3,
                    false, // P static
                    false, // O static
@@ -46,8 +46,8 @@ SensorCompass::SensorCompass(const Eigen::VectorXd& _extrinsics,
 
     // Global Magnetic Field 'F' state block
     addStateBlock('F',
-                  std::make_shared<StateBlock>(params_compass_->field_prior_state,
-                                               params_compass_->field_prior_mode == "fix"));
+                  std::make_shared<StateParams3>(params_compass_->field_prior_state,
+                                                 params_compass_->field_prior_mode == "fix"));
 
     if (params_compass_->field_prior_mode == "factor")
         addPriorParameter('F',