Skip to content

bugfix: imu linear acceleration gravity removal with rotated IMU #946

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: rolling-devel
Choose a base branch
from
Open

bugfix: imu linear acceleration gravity removal with rotated IMU #946

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b026af8
bugfix: gravity vector rotated to target_grame (was IMU frame) prior …
HaoguangYang Aug 3, 2024
803154e
reformat ros_filter.cpp
ottojo Sep 11, 2025
a5b8ebd
update docs for imuN_remove_gravitational_acceleration
ottojo Sep 11, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion doc/state_estimation_nodes.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ If this parameter is set to ``true``, then any measurements from this sensor wil
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
If fusing accelerometer data from IMUs, this parameter determines whether or not acceleration due to gravity is removed from the acceleration measurement before fusing it.

.. note:: This assumes that the IMU that is providing the acceleration data is also producing an absolute orientation. The orientation data is required to correctly remove gravitational acceleration.
.. note:: This uses the absolute orientation estimate from the IMU message, if available. If the IMU does not provide an orientation estimate, the first element of the orientation covariance must be set to -1, to indicate this, as specified in the IMU message definition. The orientation of the robot state is then used for gravity removal.

~gravitational_acceleration
^^^^^^^^^^^^^^^^^^^^^^^^^^^
Expand Down
19 changes: 12 additions & 7 deletions src/ros_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2766,9 +2766,8 @@ bool RosFilter<T>::prepareAcceleration(
state(StateMemberPitch),
state(StateMemberYaw));

// transform state orientation to IMU frame
trans.setBasis(stateTmp * target_frame_trans.getBasis());
rotNorm = trans.getBasis().inverse() * normAcc;
// transform state orientation to target frame
rotNorm = stateTmp.inverse() * normAcc;
} else {
tf2::Quaternion curAttitude;
tf2::fromMsg(msg->orientation, curAttitude);
Expand All @@ -2781,14 +2780,20 @@ bool RosFilter<T>::prepareAcceleration(
trans.setRotation(curAttitude);
if (!relative) {
// curAttitude is the true world-frame attitude of the sensor
rotNorm = trans.getBasis().inverse() * normAcc;
rotNorm = target_frame_trans.getBasis() *
(trans.getBasis().inverse() * normAcc);
} else {
// curAttitude is relative to the initial pose of the sensor.
// Assumption: IMU sensor is rigidly attached to the base_link
// (but a static rotation is possible).
rotNorm = target_frame_trans.getBasis().inverse() * trans.getBasis().inverse() * normAcc;
// Assumption 1: IMU sensor is rigidly attached to the
// target_frame (base_link) (but a static rotation is possible).
// Assumption 2: the initial pose of target_frame (base_link)
// is upright.
rotNorm = target_frame_trans.getBasis() *
(trans.getBasis().inverse() *
(target_frame_trans.getBasis().inverse() * normAcc));
}
}
// Note that acc_tmp and rotNorm are both in target_frame
acc_tmp.setX(acc_tmp.getX() - rotNorm.getX());
acc_tmp.setY(acc_tmp.getY() - rotNorm.getY());
acc_tmp.setZ(acc_tmp.getZ() - rotNorm.getZ());
Expand Down