feat(ekf_localizer): short periodic updates of z values based on pitch (

autowarefoundation#4554)

* feat(ekf_localizer): short periodic updates of z values based on pitch

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* delete an unnecessary space

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* add brief considering_z_ndt_delay()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* add const and reference

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* modify function about renewing z value, change name of function and variable

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* delete unnecessary lines

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* move updateZConsideringDelay() to in updateSimple1DFilters()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* change name of function

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* add explanation in timerCallback() to make it easier to understand

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* modify a mistake

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* move calculateDeltaZFromPitch(), delete counter

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* use variable t_receive_pose

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* add underscore to member variables

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* add a description about caluculation delta from pitch

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* delete pitch_from_ndt_

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* modify ekf_localizer

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* modify README about Calculation of vertical correction amount from pitch

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* adjust code to a standard

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* adjust lines to a standard

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* modify README and name of variable

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* modify README

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* modify media

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* modify media

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* delete calculateDeltaFromPitch() and write internal calculation in one liner

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* modify README (Calculation of vertical correction amount from pitch in Features)

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com>

localization/ekf_localizer/README.md

@@ -18,6 +18,7 @@ This package includes the following features:
 - **Automatic estimation of yaw bias** prevents modeling errors caused by sensor mounting angle errors, which can improve estimation accuracy.
 - **Mahalanobis distance gate** enables probabilistic outlier detection to determine which inputs should be used or ignored.
 - **Smooth update**, the Kalman Filter measurement update is typically performed when a measurement is obtained, but it can cause large changes in the estimated value, especially for low-frequency measurements. Since the algorithm can consider the measurement time, the measurement data can be divided into multiple pieces and integrated smoothly while maintaining consistency (see the following figure).
+- **Calculation of vertical correction amount from pitch** mitigates localization instability on slopes. For example, when going uphill, it behaves as if it is buried in the ground (see the left side of the "Calculate delta from pitch" figure) because EKF only considers 3DoF(x,y,yaw). Therefore, EKF corrects the z-coordinate according to the formula (see the right side of the "Calculate delta from pitch" figure).
 
 <p align="center">
 <img src="./media/ekf_delay_comp.png" width="800">
@@ -27,6 +28,10 @@ This package includes the following features:
   <img src="./media/ekf_smooth_update.png" width="800">
 </p>
 
+<p align="center">
+  <img src="./media/calculation_delta_from_pitch.png" width="800">
+</p>
+
 ## Launch
 
 The `ekf_localizer` starts with the default parameters with the following command.

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -446,7 +446,15 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
     poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps);
 
   ekf_.updateWithDelay(y, C, R, delay_step);
-  updateSimple1DFilters(pose, params_.pose_smoothing_steps);
+
+  // Considering change of z value due to measurement pose delay
+  const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);
+  const double dz_delay = current_ekf_twist_.twist.linear.x * delay_time * std::sin(-rpy.y);
+  geometry_msgs::msg::PoseWithCovarianceStamped pose_with_z_delay;
+  pose_with_z_delay = pose;
+  pose_with_z_delay.pose.pose.position.z += dz_delay;
+
+  updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps);
 
   // debug
   const Eigen::MatrixXd X_result = ekf_.getLatestX();