[skip-ci] add a argument to parameter to set IMU biases static/dynamic

demos/sensor_imu.yaml

@@ -7,3 +7,5 @@ motion_variances:
     wb_initial_stdev:       0.350     # rad/sec - initial bias 
     ab_rate_stdev:          0.1       # m/s2/sqrt(s)           
     wb_rate_stdev:          0.0400    # rad/s/sqrt(s)
+
+dynamic_imu_bias: true
\ No newline at end of file

include/imu/sensor/sensor_imu.h

@@ -47,6 +47,9 @@ struct ParamsSensorImu : public ParamsSensorBase
     double ab_rate_stdev = 0.00001;
     double wb_rate_stdev = 0.00001;
 
+    // TODO: NOT necessary after the big refactor
+    bool dynamic_imu_bias = true;
+
     ~ParamsSensorImu() override = default;
     ParamsSensorImu()
     {
@@ -61,6 +64,7 @@ struct ParamsSensorImu : public ParamsSensorBase
         wb_initial_stdev    = _server.getParam<double>(prefix + _unique_name + "/wb_initial_stdev");
         ab_rate_stdev       = _server.getParam<double>(prefix + _unique_name + "/ab_rate_stdev");
         wb_rate_stdev       = _server.getParam<double>(prefix + _unique_name + "/wb_rate_stdev");
+        dynamic_imu_bias    = _server.getParam<bool>  (prefix + _unique_name + "/dynamic_imu_bias");
     }
     std::string print() const override
     {
@@ -70,7 +74,8 @@ struct ParamsSensorImu : public ParamsSensorBase
             + "ab_initial_stdev: "  + std::to_string(ab_initial_stdev)  + "\n"
             + "wb_initial_stdev: "  + std::to_string(wb_initial_stdev)  + "\n"
             + "ab_rate_stdev: "     + std::to_string(ab_rate_stdev)     + "\n"
-            + "wb_rate_stdev: "     + std::to_string(wb_rate_stdev)     + "\n";
+            + "wb_rate_stdev: "     + std::to_string(wb_rate_stdev)     + "\n"
+            + "dynamic_imu_bias: "  + std::to_string(wb_rate_stdev)     + "\n";
     }
 };
 

src/sensor/sensor_imu.cpp

@@ -38,7 +38,7 @@ SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu&
                    std::make_shared<StateQuaternion>(_extrinsics.tail(4), true, false),
                    std::make_shared<StateParams6>(Vector6d::Zero(), false),
                    (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(),
-                   false, false, true),
+                   false, false, _params.dynamic_imu_bias),
         a_noise(_params.a_noise),
         w_noise(_params.w_noise),
         ab_initial_stdev(_params.ab_initial_stdev),

src/yaml/sensor_imu_yaml.cpp

@@ -59,6 +59,8 @@ static ParamsSensorBasePtr createParamsSensorImu(const std::string & _filename_d
         params->ab_rate_stdev       = variances["ab_rate_stdev"]    .as<double>();
         params->wb_rate_stdev       = variances["wb_rate_stdev"]    .as<double>();
 
+        params->dynamic_imu_bias    = config["dynamic_imu_bias"]    .as<bool>();
+
         return params;
     }
 

test/yaml/sensor_imu.yaml

@@ -7,3 +7,5 @@ motion_variances:
     wb_initial_stdev:       0.350     # rad/sec - initial bias 
     ab_rate_stdev:          0.1       # m/s2/sqrt(s)           
     wb_rate_stdev:          0.0400    # rad/s/sqrt(s)
+
+dynamic_imu_bias: true
\ No newline at end of file

test/yaml/sensor_imu_static_init.yaml

@@ -7,3 +7,5 @@ motion_variances:
     wb_initial_stdev:       0.001     # rad/sec - initial bias 
     ab_rate_stdev:          0.001       # m/s2/sqrt(s)           
     wb_rate_stdev:          0.001    # rad/s/sqrt(s)
+
+dynamic_imu_bias: false
\ No newline at end of file