diff --git a/doc/state_estimation_nodes.rst b/doc/state_estimation_nodes.rst index 42094587..8bc6c579 100644 --- a/doc/state_estimation_nodes.rst +++ b/doc/state_estimation_nodes.rst @@ -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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 607d56af..d3bdf741 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -2766,9 +2766,8 @@ bool RosFilter::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); @@ -2781,14 +2780,20 @@ bool RosFilter::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());