Ros2 v0.8.0 ekf localizer (autowarefoundation#270)

* restore file name for v0.8.0 update

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix typos in localization (autowarefoundation#890)

* move kalman filter to lib package (autowarefoundation#1141)

* move kalman filter to lib package

* add kalman_filter dir

* Revert "restore file name for v0.8.0 update"

This reverts commit 485111da0aba91eeddda77e1e3b6b3f517373163.

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

localization/ekf_localizer/CMakeLists.txt

@@ -21,9 +21,8 @@ find_package(Eigen3 REQUIRED)
 ament_auto_add_executable(ekf_localizer
   src/ekf_localizer_node.cpp
   src/ekf_localizer.cpp
-  src/kalman_filter/kalman_filter.cpp
-  src/kalman_filter/time_delay_kalman_filter.cpp)
-ament_target_dependencies(ekf_localizer Eigen3)
+)
+ament_target_dependencies(ekf_localizer kalman_filter)
 
 # if(BUILD_TESTING)
 #   find_package(ament_cmake_gtest REQUIRED)

localization/ekf_localizer/README.md

@@ -128,7 +128,7 @@ The parameters are set in `launch/ekf_localizer.launch` .
 |:---|:---|:---|:---|
 |show_debug_info|bool|Flag to display debug info|false|
 |predict_frequency|double|Frequency for filtering and publishing [Hz]|50.0|
-|tf_rate|double|Frqcuency for tf broadcasting [Hz]|10.0|
+|tf_rate|double|Frequency for tf broadcasting [Hz]|10.0|
 |extend_state_step|int|Max delay step which can be dealt with in EKF. Large number increases computational cost. |50|
 |enable_yaw_bias_estimation| bool |Flag to enable yaw bias estimation|true|
 
@@ -140,7 +140,7 @@ The parameters are set in `launch/ekf_localizer.launch` .
 |pose_measure_uncertainty_time|double|Measured time uncertainty used for covariance calculation [s]|0.01|
 |pose_rate|double|Approximated input pose rate used for covariance calculation [Hz]|10.0|
 |pose_gate_dist|double|Limit of Mahalanobis distance used for outliers detection|10000.0|
-|use_pose_with_covariance|bool|Flag to use covariance in pose_with_covarianve message|false|
+|use_pose_with_covariance|bool|Flag to use covariance in pose_with_covariance message|false|
 |pose_stddev_x|double|Standard deviation for pose position x [m] (used when use_pose_with_covariance is false)|0.05|
 |pose_stddev_y|double|Standard deviation for pose position y [m] (used when use_pose_with_covariance is false)|0.05|
 |pose_stddev_yaw|double|Standard deviation for pose yaw angle [rad] (used when use_pose_with_covariance is false)|0.025|
@@ -160,7 +160,7 @@ The parameters are set in `launch/ekf_localizer.launch` .
 |:---|:---|:---|:---|
 |proc_stddev_vx_c|double|Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0|2.0|
 |proc_stddev_wz_c|double|Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0|0.2|
-|proc_stddev_yaw_c|double|Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omege |0.005|
+|proc_stddev_yaw_c|double|Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega |0.005|
 |proc_stddev_yaw_bias_c|double|Standard deviation of process noise in time differentiation expression of yaw_bias, noise for d_yaw_bias = 0|0.001|
 
 note: process noise for position x & y are calculated automatically from nonlinear dynamics.
@@ -169,7 +169,7 @@ note: process noise for position x & y are calculated automatically from nonline
 
 **0. Preliminaries**
  - Check header time in pose and twist message is set to sensor time appropriately, because time delay is calculated from this value. If it is difficult to set appropriate time due to timer synchronization problem, use `twist_additional_delay` and `pose_additional_delay` to correct the time.
- - Check the relation between measurement pose and twist is appropriate (whether the derivative of pose has similar value to twist). This discrepancy is caused mainly by unit error (such as comfusing radian/degree) or bias noise, and it causes large estimation errors.
+ - Check the relation between measurement pose and twist is appropriate (whether the derivative of pose has similar value to twist). This discrepancy is caused mainly by unit error (such as confusing radian/degree) or bias noise, and it causes large estimation errors.
 
 
 **1. Set sensor parameters** 
@@ -190,7 +190,7 @@ Set sensor-rate and standard-deviation from the basic information of the sensor.
 
  - `proc_stddev_vx_c` : set to maximum linear acceleration
  - `proc_stddev_wz_c` : set to maximum angular acceleration
- - `proc_stddev_yaw_c` : This parameter describes the correlation between the yaw and yaw-rate. Large value means the change in yaw does not correlate to the estiamted yaw-rate. If this is set to 0, it means the change in estimate yaw is equal to yaw-rate. Usually this should be set to 0.
+ - `proc_stddev_yaw_c` : This parameter describes the correlation between the yaw and yaw-rate. Large value means the change in yaw does not correlate to the estimated yaw-rate. If this is set to 0, it means the change in estimate yaw is equal to yaw-rate. Usually this should be set to 0.
  - `proc_stddev_yaw_bias_c` : This parameter is the standard deviation for the rate of change in yaw bias. In most cases, yaw bias is constant, so it can be very small, but must be non-zero. 
 
  **3. Tune sensor standard deviation parameters with rosbag simulation.**

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -105,20 +105,20 @@ class EKFLocalizer : public rclcpp::Node
                              //!< (pose.header.stamp - now) + additional_delay [s]
   double pose_measure_uncertainty_time_;  //!< @brief  added for measurement covariance
   double pose_rate_;  //!< @brief  pose rate [s], used for covariance calculation
-  //!< @brief  the maharanobis distance threshold to ignore pose measurement
+  //!< @brief  the mahalanobis distance threshold to ignore pose measurement
   double pose_gate_dist_;
   double pose_stddev_x_;  //!< @brief  standard deviation for pose position x [m]
   double pose_stddev_y_;  //!< @brief  standard deviation for pose position y [m]
   double pose_stddev_yaw_;          //!< @brief  standard deviation for pose position yaw [rad]
-  bool use_pose_with_covariance_;   //!< @brief  use covariance in pose_with_covarianve message
-  bool use_twist_with_covariance_;  //!< @brief  use covariance in twist_with_covarianve message
+  bool use_pose_with_covariance_;   //!< @brief  use covariance in pose_with_covariance message
+  bool use_twist_with_covariance_;  //!< @brief  use covariance in twist_with_covariance message
 
   /* twist */
   double
     twist_additional_delay_;  //!< @brief  compensated delay = (twist.header.stamp - now)
                               //!< + additional_delay [s]
   double twist_rate_;  //!< @brief  rate [s], used for covariance calculation
-  //!< @brief  measurement is ignored if the maharanobis distance is larger than this value.
+  //!< @brief  measurement is ignored if the mahalanobis distance is larger than this value.
   double twist_gate_dist_;
   double twist_stddev_vx_;  //!< @brief  standard deviation for linear vx
   double twist_stddev_wz_;  //!< @brief  standard deviation for angular wx

localization/ekf_localizer/package.xml

@@ -12,6 +12,7 @@
   <build_depend>eigen</build_depend>
 
   <depend>autoware_debug_msgs</depend>
+  <depend>kalman_filter</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>