fix(imu_corrector): correct sign of offset in imu_corrector (autowarefoundation#1022)

* fix: change sign of offset in imu_corrector

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* add an equation in imu_corrector readme

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* update readme in imu_corrector

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* update readme in imu_corrector

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: kminoda

sensing/imu_corrector/README.md

@@ -4,8 +4,17 @@
 
 `imu_corrector_node` is a node that correct imu data.
 
-1. Correct yaw rate offset by reading the parameter.
-2. Correct yaw rate standard deviation by reading the parameter.
+1. Correct yaw rate offset $b$ by reading the parameter.
+2. Correct yaw rate standard deviation $\sigma$ by reading the parameter.
+
+Mathematically, we assume the following equation:
+
+$$
+\tilde{\omega}(t) = \omega(t) + b(t) + n(t)
+$$
+
+where $\tilde{\omega}$ denotes observed angular velocity, $\omega$ denotes true angular velocity, $b$ denotes an offset, and $n$ denotes a gaussian noise.
+We also assume that $n\sim\mathcal{N}(0, \sigma^2)$.
 
 <!-- TODO(TIER IV): Make this repository public or change the link. -->
 <!-- Use the value estimated by [deviation_estimator](https://github.com/tier4/calibration_tools/tree/main/localization/deviation_estimation_tools) as the parameters for this node. -->

sensing/imu_corrector/src/imu_corrector_core.cpp

@@ -38,9 +38,9 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
   sensor_msgs::msg::Imu imu_msg;
   imu_msg = *imu_msg_ptr;
 
-  imu_msg.angular_velocity.x += angular_velocity_offset_x_;
-  imu_msg.angular_velocity.y += angular_velocity_offset_y_;
-  imu_msg.angular_velocity.z += angular_velocity_offset_z_;
+  imu_msg.angular_velocity.x -= angular_velocity_offset_x_;
+  imu_msg.angular_velocity.y -= angular_velocity_offset_y_;
+  imu_msg.angular_velocity.z -= angular_velocity_offset_z_;
 
   imu_msg.angular_velocity_covariance[0 * 3 + 0] =
     angular_velocity_stddev_xx_ * angular_velocity_stddev_xx_;