From 4a2b06f23f68d9b6aac3a7603a10363f679ef8cc Mon Sep 17 00:00:00 2001 From: Takaaki Numai Date: Tue, 9 Jan 2024 18:57:17 +0900 Subject: [PATCH 1/3] Fix imu system --- src/hdl_localization/pose_system.cpp | 44 ++++++++++++---------------- 1 file changed, 18 insertions(+), 26 deletions(-) diff --git a/src/hdl_localization/pose_system.cpp b/src/hdl_localization/pose_system.cpp index 6eda3748..88700a2c 100644 --- a/src/hdl_localization/pose_system.cpp +++ b/src/hdl_localization/pose_system.cpp @@ -11,17 +11,13 @@ PoseSystem::PoseSystem() PoseSystem::VectorXt PoseSystem::f(const VectorXt& state) const { VectorXt state_next(16); - Vector3t pt = state.middleRows(0, 3); Vector3t vt = state.middleRows(3, 3); Quaterniont qt(state[6], state[7], state[8], state[9]); qt.normalize(); - Vector3t acc_bias = state.middleRows(10, 3); - Vector3t gyro_bias = state.middleRows(13, 3); - // position - state_next.middleRows(0, 3) = pt + vt * dt_; // + state_next.middleRows(0, 3) = pt + vt * dt_; // velocity state_next.middleRows(3, 3) = vt; @@ -39,7 +35,6 @@ PoseSystem::VectorXt PoseSystem::fImu(const VectorXt& state, const Eigen::Vector const Eigen::Vector3f& imu_gyro) const { VectorXt state_next(16); - Vector3t pt = state.middleRows(0, 3); Vector3t vt = state.middleRows(3, 3); Quaterniont qt(state[6], state[7], state[8], state[9]); @@ -48,26 +43,24 @@ PoseSystem::VectorXt PoseSystem::fImu(const VectorXt& state, const Eigen::Vector Vector3t acc_bias = state.middleRows(10, 3); Vector3t gyro_bias = state.middleRows(13, 3); - const Vector3t& raw_acc = imu_acc; - const Vector3t& raw_gyro = imu_gyro; - // position - state_next.middleRows(0, 3) = pt + vt * dt_; // + Vector3t pt_next = pt + vt * dt_; + state_next.middleRows(0, 3) = pt_next; // velocity Vector3t g(0.0f, 0.0f, 9.80665f); - Vector3t acc = qt * (raw_acc - acc_bias); - state_next.middleRows(3, 3) = vt + (acc - g) * dt_; - // state_next.middleRows(3, 3) = vt; // + (acc - g) * dt_; // acceleration didn't contribute to accuracy due to - // large noise + Vector3t acc = qt * (imu_acc - acc_bias) - g; + Vector3t vt_next = vt + acc * dt_; + state_next.middleRows(3, 3) = vt_next; // orientation - Vector3t gyro = raw_gyro - gyro_bias; - Quaterniont dq(1, gyro[0] * dt_ / 2, gyro[1] * dt_ / 2, gyro[2] * dt_ / 2); + Vector3t gyro = imu_gyro - gyro_bias; + Quaterniont dq; + dq = Eigen::AngleAxisf(gyro[0] * dt_, Vector3t::UnitX()) * Eigen::AngleAxisf(gyro[1] * dt_, Vector3t::UnitY()) * + Eigen::AngleAxisf(gyro[2] * dt_, Vector3t::UnitZ()); dq.normalize(); Quaterniont qt_next = (qt * dq).normalized(); state_next.middleRows(6, 4) << qt_next.w(), qt_next.x(), qt_next.y(), qt_next.z(); - state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration state_next.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity @@ -83,22 +76,21 @@ PoseSystem::VectorXt PoseSystem::fOdom(const VectorXt& state, const Eigen::Vecto Quaterniont qt(state[6], state[7], state[8], state[9]); qt.normalize(); - Vector3t lin_vel_raw = odom_twist_lin; - Vector3t ang_vel_raw = odom_twist_ang; - // position - state_next.middleRows(0, 3) = pt + vt * dt_; + Vector3t pt_next = pt + vt * dt_; + state_next.middleRows(0, 3) = pt_next; // velocity - Vector3t vel = qt * lin_vel_raw; - state_next.middleRows(3, 3) = vel; + Vector3t vt_next = qt * odom_twist_lin; + state_next.middleRows(3, 3) = vt_next; // orientation - Quaterniont dq(1, ang_vel_raw[0] * dt_ / 2, ang_vel_raw[1] * dt_ / 2, ang_vel_raw[2] * dt_ / 2); - dq.normalize(); + Quaterniont dq; + dq = Eigen::AngleAxisf(odom_twist_ang[0] * dt_, Vector3t::UnitX()) * + Eigen::AngleAxisf(odom_twist_ang[1] * dt_, Vector3t::UnitY()) * + Eigen::AngleAxisf(odom_twist_ang[2] * dt_, Vector3t::UnitZ()); Quaterniont qt_next = (qt * dq).normalized(); state_next.middleRows(6, 4) << qt_next.w(), qt_next.x(), qt_next.y(), qt_next.z(); - state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration state_next.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity From 8f63b7411c68109c023095639e65f0f64d477e68 Mon Sep 17 00:00:00 2001 From: Takaaki Numai Date: Wed, 10 Jan 2024 16:00:27 +0900 Subject: [PATCH 2/3] Add missing normalize() --- src/hdl_localization/pose_system.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/hdl_localization/pose_system.cpp b/src/hdl_localization/pose_system.cpp index 88700a2c..a23b4d36 100644 --- a/src/hdl_localization/pose_system.cpp +++ b/src/hdl_localization/pose_system.cpp @@ -89,6 +89,7 @@ PoseSystem::VectorXt PoseSystem::fOdom(const VectorXt& state, const Eigen::Vecto dq = Eigen::AngleAxisf(odom_twist_ang[0] * dt_, Vector3t::UnitX()) * Eigen::AngleAxisf(odom_twist_ang[1] * dt_, Vector3t::UnitY()) * Eigen::AngleAxisf(odom_twist_ang[2] * dt_, Vector3t::UnitZ()); + dq.normalize(); Quaterniont qt_next = (qt * dq).normalized(); state_next.middleRows(6, 4) << qt_next.w(), qt_next.x(), qt_next.y(), qt_next.z(); state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration From 1feaf1ee89c1fca71fe6764ccd240f3114566aeb Mon Sep 17 00:00:00 2001 From: Takaaki Numai Date: Thu, 11 Jan 2024 21:17:45 +0900 Subject: [PATCH 3/3] Use quaternion --- src/hdl_localization/pose_system.cpp | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/src/hdl_localization/pose_system.cpp b/src/hdl_localization/pose_system.cpp index a23b4d36..6fe10663 100644 --- a/src/hdl_localization/pose_system.cpp +++ b/src/hdl_localization/pose_system.cpp @@ -55,11 +55,8 @@ PoseSystem::VectorXt PoseSystem::fImu(const VectorXt& state, const Eigen::Vector // orientation Vector3t gyro = imu_gyro - gyro_bias; - Quaterniont dq; - dq = Eigen::AngleAxisf(gyro[0] * dt_, Vector3t::UnitX()) * Eigen::AngleAxisf(gyro[1] * dt_, Vector3t::UnitY()) * - Eigen::AngleAxisf(gyro[2] * dt_, Vector3t::UnitZ()); - dq.normalize(); - Quaterniont qt_next = (qt * dq).normalized(); + Quaterniont dq = Quaterniont(0, gyro[0] * dt_ / 2.0, gyro[1] * dt_ / 2.0, gyro[2] * dt_ / 2.0) * qt; + Quaterniont qt_next = Quaterniont(qt.w() + dq.w(), qt.x() + dq.x(), qt.y() + dq.y(), qt.z() + dq.z()).normalized(); state_next.middleRows(6, 4) << qt_next.w(), qt_next.x(), qt_next.y(), qt_next.z(); state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration state_next.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity @@ -85,12 +82,9 @@ PoseSystem::VectorXt PoseSystem::fOdom(const VectorXt& state, const Eigen::Vecto state_next.middleRows(3, 3) = vt_next; // orientation - Quaterniont dq; - dq = Eigen::AngleAxisf(odom_twist_ang[0] * dt_, Vector3t::UnitX()) * - Eigen::AngleAxisf(odom_twist_ang[1] * dt_, Vector3t::UnitY()) * - Eigen::AngleAxisf(odom_twist_ang[2] * dt_, Vector3t::UnitZ()); - dq.normalize(); - Quaterniont qt_next = (qt * dq).normalized(); + Quaterniont dq = + Quaterniont(0, odom_twist_ang[0] * dt_ / 2.0, odom_twist_ang[1] * dt_ / 2.0, odom_twist_ang[2] * dt_ / 2.0) * qt; + Quaterniont qt_next = Quaterniont(qt.w() + dq.w(), qt.x() + dq.x(), qt.y() + dq.y(), qt.z() + dq.z()).normalized(); state_next.middleRows(6, 4) << qt_next.w(), qt_next.x(), qt_next.y(), qt_next.z(); state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration state_next.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity