Use derived state blocks

src/capture/capture_gnss.cpp

@@ -20,7 +20,7 @@
 //
 //--------LICENSE_END--------
 #include "gnss/capture/capture_gnss.h"
-
+#include <core/state_block/state_block_derived.h>
 
 namespace wolf {
 
@@ -30,18 +30,18 @@ CaptureGnss::CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUt
 {
     // Clock bias
     assert(_sensor_ptr->getStateBlock('T') != nullptr and _sensor_ptr->isStateBlockDynamic('T'));
-    addStateBlock('T', std::make_shared<StateBlock>(1,true), nullptr);
+    addStateBlock('T', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
 
     // interconstellation clock bias
     assert(_sensor_ptr->getStateBlock('G') != nullptr);
     if(_sensor_ptr->isStateBlockDynamic('G'))
-        addStateBlock('G', std::make_shared<StateBlock>(1,true), nullptr);
+        addStateBlock('G', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
     assert(_sensor_ptr->getStateBlock('E') != nullptr);
     if(_sensor_ptr->isStateBlockDynamic('E'))
-        addStateBlock('E', std::make_shared<StateBlock>(1,true), nullptr);
+        addStateBlock('E', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
     assert(_sensor_ptr->getStateBlock('M') != nullptr);
     if(_sensor_ptr->isStateBlockDynamic('M'))
-        addStateBlock('M', std::make_shared<StateBlock>(1,true), nullptr);
+        addStateBlock('M', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
 
 }
 

src/sensor/sensor_gnss.cpp

@@ -20,7 +20,7 @@
 //
 //--------LICENSE_END--------
 #include "gnss/sensor/sensor_gnss.h"
-#include "core/state_block/state_block.h"
+#include <core/state_block/state_block_derived.h>
 #include "core/state_block/state_angle.h"
 #include "gnss_utils/gnss_utils.h"
 #include "gnss_utils/utils/transformations.h"
@@ -31,7 +31,7 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
                        const ParamsSensorGnssPtr& _params)
         :
         SensorBase("SensorGnss",
-                   std::make_shared<StateBlock>(_extrinsics,_params->extrinsics_fixed),
+                   std::make_shared<StatePoint3d>(_extrinsics,_params->extrinsics_fixed),
                    nullptr, // antena orientation has no sense in GNSS
                    nullptr, // SensorParams are set afterwards
                    0),
@@ -46,15 +46,15 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
 
     // STATE BLOCKS
     // ENU-MAP
-    addStateBlock('t', std::make_shared<StateBlock>(3, params_->translation_fixed), false);
+    addStateBlock('t', std::make_shared<StatePoint3d>(Vector3d::Zero(), params_->translation_fixed), false);
     addStateBlock('r', std::make_shared<StateAngle>(0.0, params_->roll_fixed), false);
     addStateBlock('p', std::make_shared<StateAngle>(0.0, params_->pitch_fixed), false);
     addStateBlock('y', std::make_shared<StateAngle>(0.0, params_->yaw_fixed), false);
     // clock bias
-    addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateBlock>(1,false), true); // receiver clock bias
-    addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
-    addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
-    addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
+    addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),false), true); // receiver clock bias
+    addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),true), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
+    addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),true), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
+    addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),true), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
 
     // ENU-ECEF
     // Mode "manual": ENU provided via params