From 1e5ae7eaf2d5e1dba1a253ebd58bce76450e6e8e Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Wed, 7 Sep 2022 16:39:22 +0200
Subject: [PATCH] [skip-ci] add a argument to parameter to set IMU biases
static/dynamic
---
demos/sensor_imu.yaml | 2 ++
include/imu/sensor/sensor_imu.h | 7 ++++++-
src/sensor/sensor_imu.cpp | 2 +-
src/yaml/sensor_imu_yaml.cpp | 2 ++
test/yaml/sensor_imu.yaml | 2 ++
test/yaml/sensor_imu_static_init.yaml | 2 ++
6 files changed, 15 insertions(+), 2 deletions(-)
diff --git a/demos/sensor_imu.yaml b/demos/sensor_imu.yaml
index 3c78a00d3..38a3df944 100644
--- a/demos/sensor_imu.yaml
+++ b/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
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 7e3e2b38b..fc6f6990e 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/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";
}
};
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
index fba6cdf5e..d5a676f66 100644
--- a/src/sensor/sensor_imu.cpp
+++ b/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),
diff --git a/src/yaml/sensor_imu_yaml.cpp b/src/yaml/sensor_imu_yaml.cpp
index e530b2445..cff3ec8a4 100644
--- a/src/yaml/sensor_imu_yaml.cpp
+++ b/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;
}
diff --git a/test/yaml/sensor_imu.yaml b/test/yaml/sensor_imu.yaml
index 3c78a00d3..38a3df944 100644
--- a/test/yaml/sensor_imu.yaml
+++ b/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
diff --git a/test/yaml/sensor_imu_static_init.yaml b/test/yaml/sensor_imu_static_init.yaml
index 700b8d676..e8df83cd8 100644
--- a/test/yaml/sensor_imu_static_init.yaml
+++ b/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
--
GitLab