feat(pose_instability_detector): change validation algorithm (#7042)

* Create define_static_threshold() Signed-off-by: TaikiYamada4 <[email protected]> * Revised dead reckoning methodology Signed-off-by: TaikiYamada4 <[email protected]> * style(pre-commit): autofix * Change threshold calculation to use the online time difference Signed-off-by: TaikiYamada4 <[email protected]> * Simplify threshold calculation. Rewrite json schema. Refactor some variables. Signed-off-by: TaikiYamada4 <[email protected]> * style(pre-commit): autofix * Rewrite lateral_threshold and vertical threshold Signed-off-by: TaikiYamada4 <[email protected]> * Consider dead reckoning noise, update README.md. Signed-off-by: TaikiYamada4 <[email protected]> * Added sentences to README.md Signed-off-by: TaikiYamada4 <[email protected]> * Filled README.md Revert lateral threshold calculation. Signed-off-by: TaikiYamada4 <[email protected]> * style(pre-commit): autofix * Add #include <algorithm> Signed-off-by: TaikiYamada4 <[email protected]> * style(pre-commit): autofix * Fixed pose_instability_detector.schema.json Signed-off-by: TaikiYamada4 <[email protected]> * Revised calculation of the process noise of dead reckoning. Signed-off-by: TaikiYamada4 <[email protected]> * style(pre-commit): autofix * Fixed typo and lack of information Signed-off-by: TaikiYamada4 <[email protected]> * Revised redundant time substitutions Signed-off-by: TaikiYamada4 <[email protected]> * Revised dead reckoning algorithm for orientation. Signed-off-by: TaikiYamada4 <[email protected]> * style(pre-commit): autofix * Added information about lateral thresholld calculation in README.md Signed-off-by: TaikiYamada4 <[email protected]> * Removed all dead reckoning related process noise stuff Signed-off-by: TaikiYamada4 <[email protected]> * Removed parameters and desciprtion of dead reckoning process noise stuff Signed-off-by: TaikiYamada4 <[email protected]> * style(pre-commit): autofix * Fixed integration logic for angular twist Signed-off-by: TaikiYamada4 <[email protected]> * Let the hpp file be exportable, and follow the guidelines when exporting hpp files Signed-off-by: TaikiYamada4 <[email protected]> * style(pre-commit): autofix * Fix typo Signed-off-by: TaikiYamada4 <[email protected]> * Delete include from package.xml Signed-off-by: TaikiYamada4 <[email protected]> * Make test codes work. Create a threshold structure so that other packages can use the methods in pose_instability_detector Signed-off-by: TaikiYamada4 <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: TaikiYamada4 <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jun 7, 2024 · 2b7c0db · 2b7c0db
1 parent 7d6c2fd
commit 2b7c0db
Show file tree

Hide file tree

Showing 11 changed files with 858 additions and 122 deletions.
diff --git a/localization/pose_instability_detector/README.md b/localization/pose_instability_detector/README.md
@@ -1,20 +1,111 @@
 # pose_instability_detector
 
-The `pose_instability_detector` package includes a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF).
+The `pose_instability_detector` is a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF).
 
 This node triggers periodic timer callbacks to compare two poses:
 
-- The pose obtained by integrating the twist values from the last received message on `/localization/kinematic_state` over a duration specified by `interval_sec`.
+- The pose calculated by dead reckoning starting from the pose of `/localization/kinematic_state` obtained `timer_period` seconds ago.
 - The latest pose from `/localization/kinematic_state`.
 
 The results of this comparison are then output to the `/diagnostics` topic.
 
+![overview](./media/pose_instability_detector_overview.png)
+
+![rqt_runtime_monitor](./media/rqt_runtime_monitor.png)
+
 If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values.
+In other words, WARN outputs indicate that the vehicle has moved to a place outside the expected range based on the twist values.
 This discrepancy suggests that there may be an issue with either the estimated pose or the input twist.
 
-The following diagram provides an overview of what the timeline of this process looks like:
+The following diagram provides an overview of how the procedure looks like:
+
+![procedure](./media/pose_instabilty_detector_procedure.svg)
+
+## Dead reckoning algorithm
+
+Dead reckoning is a method of estimating the position of a vehicle based on its previous position and velocity.
+The procedure for dead reckoning is as follows:
+
+1. Capture the necessary twist values from the `/input/twist` topic.
+2. Integrate the twist values to calculate the pose transition.
+3. Apply the pose transition to the previous pose to obtain the current pose.
+
+### Collecting twist values
+
+The `pose_instability_detector` node collects the twist values from the `~/input/twist` topic to perform dead reckoning.
+Ideally, `pose_instability_detector` needs the twist values between the previous pose and the current pose.
+Therefore, `pose_instability_detector` snips the twist buffer and apply interpolations and extrapolations to obtain the twist values at the desired time.
+
+![how_to_snip_necessary_twist](./media/how_to_snip_twist.png)
+
+### Linear transition and angular transition
+
+After the twist values are collected, the node calculates the linear transition and angular transition based on the twist values and add them to the previous pose.
+
+## Threshold definition
+
+The `pose_instability_detector` node compares the pose calculated by dead reckoning with the latest pose from the EKF output.
+These two pose are ideally the same, but in reality, they are not due to the error in the twist values the pose observation.
+If these two poses are significantly different so that the absolute value exceeds the threshold, the node outputs a WARN message to the `/diagnostics` topic.
+There are six thresholds (x, y, z, roll, pitch, and yaw) to determine whether the poses are significantly different, and these thresholds are determined by the following subsections.
+
+### `diff_position_x`
+
+This threshold examines the difference in the longitudinal axis between the two poses, and check whether the vehicle goes beyond the expected error.
+This threshold is a sum of "maximum longitudinal error due to velocity scale factor error" and "pose estimation error tolerance".
+
+$$
+\tau_x = v_{\rm max}\frac{\beta_v}{100} \Delta t + \epsilon_x\\
+$$
 
-![timeline](./media/timeline.drawio.svg)
+| Symbol        | Description                                                                      | Unit  |
+| ------------- | -------------------------------------------------------------------------------- | ----- |
+| $\tau_x$      | Threshold for the difference in the longitudinal axis                            | $m$   |
+| $v_{\rm max}$ | Maximum velocity                                                                 | $m/s$ |
+| $\beta_v$     | Scale factor tolerance for the maximum velocity                                  | $\%$  |
+| $\Delta t$    | Time interval                                                                    | $s$   |
+| $\epsilon_x$  | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the longitudinal axis | $m$   |
+
+### `diff_position_y` and `diff_position_z`
+
+These thresholds examine the difference in the lateral and vertical axes between the two poses, and check whether the vehicle goes beyond the expected error.
+The `pose_instability_detector` calculates the possible range where the vehicle goes, and get the maximum difference between the nominal dead reckoning pose and the maximum limit pose.
+
+![lateral_threshold_calculation](./media/lateral_threshold_calculation.png)
+
+Addition to this, the `pose_instability_detector` node considers the pose estimation error tolerance to determine the threshold.
+
+$$
+\tau_y = l + \epsilon_y
+$$
+
+| Symbol       | Description                                                                                     | Unit |
+| ------------ | ----------------------------------------------------------------------------------------------- | ---- |
+| $\tau_y$     | Threshold for the difference in the lateral axis                                                | $m$  |
+| $l$          | Maximum lateral distance described in the image above (See the appendix how this is calculated) | $m$  |
+| $\epsilon_y$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the lateral axis                     | $m$  |
+
+Note that `pose_instability_detector` sets the threshold for the vertical axis as the same as the lateral axis. Only the pose estimator error tolerance is different.
+
+### `diff_angle_x`, `diff_angle_y`, and `diff_angle_z`
+
+These thresholds examine the difference in the roll, pitch, and yaw angles between the two poses.
+This threshold is a sum of "maximum angular error due to velocity scale factor error and bias error" and "pose estimation error tolerance".
+
+$$
+\tau_\phi = \tau_\theta = \tau_\psi = \left(\omega_{\rm max}\frac{\beta_\omega}{100} + b \right) \Delta t + \epsilon_\psi
+$$
+
+| Symbol             | Description                                                              | Unit          |
+| ------------------ | ------------------------------------------------------------------------ | ------------- |
+| $\tau_\phi$        | Threshold for the difference in the roll angle                           | ${\rm rad}$   |
+| $\tau_\theta$      | Threshold for the difference in the pitch angle                          | ${\rm rad}$   |
+| $\tau_\psi$        | Threshold for the difference in the yaw angle                            | ${\rm rad}$   |
+| $\omega_{\rm max}$ | Maximum angular velocity                                                 | ${\rm rad}/s$ |
+| $\beta_\omega$     | Scale factor tolerance for the maximum angular velocity                  | $\%$          |
+| $b$                | Bias tolerance of the angular velocity                                   | ${\rm rad}/s$ |
+| $\Delta t$         | Time interval                                                            | $s$           |
+| $\epsilon_\psi$    | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the yaw angle | ${\rm rad}$   |
 
 ## Parameters
 
@@ -34,4 +125,44 @@ The following diagram provides an overview of what the timeline of this process
 | `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped       | diff_pose   |
 | `/diagnostics`      | diagnostic_msgs::msg::DiagnosticArray | Diagnostics |
 
-![rqt_runtime_monitor](./media/rqt_runtime_monitor.png)
+## Appendix
+
+On calculating the maximum lateral distance $l$, the `pose_instability_detector` node will estimate the following poses.
+
+| Pose                            | heading velocity $v$                             | angular velocity $\omega$                                      |
+| ------------------------------- | ------------------------------------------------ | -------------------------------------------------------------- |
+| Nominal dead reckoning pose     | $v_{\rm max}$                                    | $\omega_{\rm max}$                                             |
+| Dead reckoning pose of corner A | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ |
+| Dead reckoning pose of corner B | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ |
+| Dead reckoning pose of corner C | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ |
+| Dead reckoning pose of corner D | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ |
+
+Given a heading velocity $v$ and $\omega$, the 2D theoretical variation seen from the previous pose is calculated as follows:
+
+$$
+\begin{align*}
+\left[
+    \begin{matrix}
+    \Delta x\\
+    \Delta y
+    \end{matrix}
+\right]
+&=
+\left[
+    \begin{matrix}
+    \int_{0}^{\Delta t} v \cos(\omega t) dt\\
+    \int_{0}^{\Delta t} v \sin(\omega t) dt
+    \end{matrix}
+\right]
+\\
+&=
+\left[
+    \begin{matrix}
+    \frac{v}{\omega} \sin(\omega \Delta t)\\
+    \frac{v}{\omega} \left(1 - \cos(\omega \Delta t)\right)
+    \end{matrix}
+\right]
+\end{align*}
+$$
+
+We calculate this variation for each corner and get the maximum value of the lateral distance $l$ by comparing the distance between the nominal dead reckoning pose and the corner poses.
diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml
@@ -1,9 +1,15 @@
 /**:
   ros__parameters:
-    interval_sec: 0.5  # [sec]
-    threshold_diff_position_x: 1.0  # [m]
-    threshold_diff_position_y: 1.0  # [m]
-    threshold_diff_position_z: 1.0  # [m]
-    threshold_diff_angle_x: 1.0  # [rad]
-    threshold_diff_angle_y: 1.0  # [rad]
-    threshold_diff_angle_z: 1.0  # [rad]
+    timer_period: 0.5                             # [sec]
+
+    heading_velocity_maximum: 16.667              # [m/s]
+    heading_velocity_scale_factor_tolerance: 3.0  # [%]
+
+    angular_velocity_maximum: 0.523               # [rad/s]
+    angular_velocity_scale_factor_tolerance: 0.2  # [%]
+    angular_velocity_bias_tolerance: 0.00698      # [rad/s]
+
+    pose_estimator_longitudinal_tolerance: 0.11   # [m]
+    pose_estimator_lateral_tolerance: 0.11        # [m]
+    pose_estimator_vertical_tolerance: 0.11       # [m]
+    pose_estimator_angular_tolerance: 0.0175      # [rad]
diff --git a/...etector/src/pose_instability_detector.hpp → ...ty_detector/pose_instability_detector.hpp b/...etector/src/pose_instability_detector.hpp → ...ty_detector/pose_instability_detector.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef POSE_INSTABILITY_DETECTOR_HPP_
-#define POSE_INSTABILITY_DETECTOR_HPP_
+#ifndef AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_
+#define AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -22,6 +22,8 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <deque>
+#include <tuple>
 #include <vector>
 
 class PoseInstabilityDetector : public rclcpp::Node
@@ -37,13 +39,31 @@ class PoseInstabilityDetector : public rclcpp::Node
   using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
 
 public:
+  struct ThresholdValues
+  {
+    double position_x;
+    double position_y;
+    double position_z;
+    double angle_x;
+    double angle_y;
+    double angle_z;
+  };
+
   explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  ThresholdValues calculate_threshold(double interval_sec);
+  void dead_reckon(
+    PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time,
+    const std::deque<TwistWithCovarianceStamped> & twist_deque, Pose::SharedPtr & estimated_pose);
 
 private:
   void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr);
   void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr);
   void callback_timer();
 
+  std::deque<TwistWithCovarianceStamped> clip_out_necessary_twist(
+    const std::deque<TwistWithCovarianceStamped> & twist_buffer, const rclcpp::Time & start_time,
+    const rclcpp::Time & end_time);
+
   // subscribers and timer
   rclcpp::Subscription<Odometry>::SharedPtr odometry_sub_;
   rclcpp::Subscription<TwistWithCovarianceStamped>::SharedPtr twist_sub_;
@@ -54,17 +74,26 @@ class PoseInstabilityDetector : public rclcpp::Node
   rclcpp::Publisher<DiagnosticArray>::SharedPtr diagnostics_pub_;
 
   // parameters
-  const double threshold_diff_position_x_;
-  const double threshold_diff_position_y_;
-  const double threshold_diff_position_z_;
-  const double threshold_diff_angle_x_;
-  const double threshold_diff_angle_y_;
-  const double threshold_diff_angle_z_;
+  const double timer_period_;  // [sec]
+
+  ThresholdValues threshold_values_;
+
+  const double heading_velocity_maximum_;                 // [m/s]
+  const double heading_velocity_scale_factor_tolerance_;  // [%]
+
+  const double angular_velocity_maximum_;                 // [rad/s]
+  const double angular_velocity_scale_factor_tolerance_;  // [%]
+  const double angular_velocity_bias_tolerance_;          // [rad/s]
+
+  const double pose_estimator_longitudinal_tolerance_;  // [m]
+  const double pose_estimator_lateral_tolerance_;       // [m]
+  const double pose_estimator_vertical_tolerance_;      // [m]
+  const double pose_estimator_angular_tolerance_;       // [rad]
 
   // variables
   std::optional<Odometry> latest_odometry_ = std::nullopt;
   std::optional<Odometry> prev_odometry_ = std::nullopt;
-  std::vector<TwistWithCovarianceStamped> twist_buffer_;
+  std::deque<TwistWithCovarianceStamped> twist_buffer_;
 };
 
-#endif  // POSE_INSTABILITY_DETECTOR_HPP_
+#endif  // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_
diff --git a/localization/pose_instability_detector/media/how_to_snip_twist.png b/localization/pose_instability_detector/media/how_to_snip_twist.png
diff --git a/localization/pose_instability_detector/media/lateral_threshold_calculation.png b/localization/pose_instability_detector/media/lateral_threshold_calculation.png
diff --git a/...lization/pose_instability_detector/media/pose_instability_detector_overview.png b/...lization/pose_instability_detector/media/pose_instability_detector_overview.png