feat: add pose_twist_fusion_filter packages (autowarefoundation#56)

* release v0.4.0

* Avoid setting CMAKE_BUILD_TYPE=Release in each CMakeLists.txt (autowarefoundation#720)

* remove set CMAKE_BUILD_TYPE Release in each CMakeLists.txt

* remove set CMAKE_BUILD_TYPE Release in ndt_pcl_modified

* set compile options for debug in ndt_omp

* Fix indent

* add warning if -DCMAKE_BUILD_TYPE=Release is not set in ndt_omp

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit f29c914.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Rename launch files to launch.xml (autowarefoundation#28)

* Port ekf_localizer (autowarefoundation#16)

* Ported ekf_localizer to ROS 2

* Ported ekf_localizer to ROS 2, but disabled them

* Update launch file

* Use ::SharedPtr where available

* Replace deprecated Float64 with Float64Stamped from autoware_debug_msgs

* Install launch file

* Added stamps

* fix duration unit for RCLCPP_*_THROTTLE (autowarefoundation#75)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Rename h files to hpp (autowarefoundation#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151)

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* [ekf_localizer] [pose_initializer] fix topic message type (autowarefoundation#176)

Co-authored-by: Autoware <autoware@tier4.jp>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* adding linters to ekf_localizer (autowarefoundation#194)

* add initialization for roll&pitch, remove condition in launch (autowarefoundation#209)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix units of time objects (autowarefoundation#195)

* fix units of time objects

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix tests

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix test

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* apply env_var to  use_sim_time (autowarefoundation#222)

* 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>

* Unify Apache-2.0 license name (autowarefoundation#1242)

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

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

* update ekf readme (add reference) (autowarefoundation#1382) (autowarefoundation#1383)

* update ekf readme (add reference) (autowarefoundation#1382)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Fix typo

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

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Add pre-commit (autowarefoundation#1560)

* add pre-commit

* add pre-commit-config

* add additional settings for private repository

* use default pre-commit-config

* update pre-commit setting

* Ignore whitespace for line breaks in markdown

* Update .github/workflows/pre-commit.yml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* exclude svg

* remove pretty-format-json

* add double-quote-string-fixer

* consider COLCON_IGNORE file when seaching modified package

* format file

* pre-commit fixes

* Update pre-commit.yml

* Update .pre-commit-config.yaml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Add markdownlint and prettier (autowarefoundation#1661)

* Add markdownlint and prettier

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* fix some typos (autowarefoundation#1941)

* fix some typos

* fix typo

* Fix typo

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Feature/add stop filter ros2 (autowarefoundation#1575)

* add sort-package-xml hook in pre-commit (autowarefoundation#1881)

* add sort xml hook in pre-commit

* change retval to exit_status

* rename

* add prettier plugin-xml

* use early return

* add license note

* add tier4 license

* restore prettier

* change license order

* move local hooks to public repo

* move prettier-xml to pre-commit-hooks-ros

* update version for bug-fix

* apply pre-commit

* add createQuaternionFromYaw (autowarefoundation#2120)

* add createQuaternionFromYaw

* add test

* change return value type of createQuaternionFromRPY from tf2::quat to geomety_msgs::msg::quat

* use geometry_msgs::msg::Quaternion in createQuaternionFromRPY in application

* [ekf_localizer] use autoware utils (autowarefoundation#2314)

* Change formatter to clang-format and black (autowarefoundation#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* add nav_msgs::msg::Odometry and publish odometry topic to ekf_localizer (autowarefoundation#501)

* publish odometry

* remove COLCON IGNORE

* fixed typo

* fixed order in package.xml

* add odom name

* fixed frame id

* pre commit

* subscribe odometry msg in stop filter package (autowarefoundation#520)

* remove COLCON IGNORE

* use nav_msgs::msg::Odometry

* publish stop filter odometry

* Create README.md for stop_filter (autowarefoundation#583)

* add odom topic to README (autowarefoundation#582)

* [stop filter] remove twist publisher (autowarefoundation#586)

* remove twist publisher

* remove twist publisher in README

* remove topic remap from launch file (autowarefoundation#703)

* [ekf_localizer] remove input pose and twist topic (autowarefoundation#707)

* remove input pose and twist topic

* fixed README

* pre-commit

* remove unused parameter

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Daichi Murakami <harihitode@gmail.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Esteve Fernandez <esteve@apache.org>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Autoware <autoware@tier4.jp>
Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: RyuYamamoto <ryu.yamamoto@tier4.jp>
Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com>
Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>

localization/ekf_localizer/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.5)
+project(ekf_localizer)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(Eigen3 REQUIRED)
+
+ament_auto_add_executable(ekf_localizer
+  src/ekf_localizer_node.cpp
+  src/ekf_localizer.cpp
+)
+ament_target_dependencies(ekf_localizer kalman_filter)
+
+# if(BUILD_TESTING)
+#   find_package(ament_cmake_gtest REQUIRED)
+#   ament_add_gtest(ekf_localizer-test test/test_ekf_localizer.test
+#     test/src/test_ekf_localizer.cpp
+#     src/ekf_localizer.cpp
+#     src/kalman_filter/kalman_filter.cpp
+#     src/kalman_filter/time_delay_kalman_filter.cpp
+#   )
+#   target_include_directories(ekf_localizer-test
+#     PRIVATE
+#     include
+#   )
+#   ament_target_dependencies(ekf_localizer-test geometry_msgs rclcpp tf2 tf2_ros)
+# endif()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

localization/ekf_localizer/README.md

@@ -0,0 +1,192 @@
+# Overview
+
+The **Extend Kalman Filter Localizer** estimates robust and less noisy robot pose and twist by integrating the 2D vehicle dynamics model with input ego-pose and ego-twist messages. The algorithm is designed especially for fast moving robot such as autonomous driving system.
+
+## Flowchart
+
+The overall flowchart of the ekf_localizer is described below.
+
+<p align="center">
+  <img src="./media/ekf_flowchart.png" width="800">
+</p>
+
+## Features
+
+This package includes the following features:
+
+- **Time delay compensation** for input messages, which enables proper integration of input information with varying time delay. This is important especially for high speed moving robot, such as autonomous driving vehicle. (see following figure).
+- **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 following figure).
+
+<p align="center">
+<img src="./media/ekf_delay_comp.png" width="800">
+</p>
+
+<p align="center">
+  <img src="./media/ekf_smooth_update.png" width="800">
+</p>
+
+## Launch
+
+The `ekf_localizer` starts with the default parameters with the following command.
+
+```sh
+roslaunch ekf_localizer ekf_localizer.launch
+```
+
+The parameters and input topic names can be set in the `ekf_localizer.launch` file.
+
+## Node
+
+### Subscribed Topics
+
+- measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped)
+
+  Input pose source with measurement covariance matrix.
+
+- measured_twist_with_covariance (geometry_msgs/PoseWithCovarianceStamped)
+
+  Input twist source with measurement covariance matrix.
+
+- initialpose (geometry_msgs/PoseWithCovarianceStamped)
+
+  Initial pose for EKF. The estimated pose is initialized with zeros at start. It is initialized with this message whenever published.
+
+### Published Topics
+
+- ekf_odom (nav_msgs/Odometry)
+
+  Estimated odometry.
+
+- ekf_pose (geometry_msgs/PoseStamped)
+
+  Estimated pose.
+
+- ekf_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped)
+
+  Estimated pose with covariance.
+
+- ekf_pose_with_covariance (geometry_msgs/PoseStamped)
+
+  Estimated pose without yawbias effect.
+
+- ekf_pose_with_covariance_without_yawbias (geometry_msgs/PoseWithCovarianceStamped)
+
+  Estimated pose with covariance without yawbias effect.
+
+- ekf_twist (geometry_msgs/TwistStamped)
+
+  Estimated twist.
+
+- ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped)
+
+  Estimated twist with covariance.
+
+### Published TF
+
+- base_link
+
+  TF from "map" coordinate to estimated pose.
+
+## Functions
+
+### Predict
+
+The current robot state is predicted from previously estimated data using a given prediction model. This calculation is called at constant interval (`predict_frequency [Hz]`). The prediction equation is described at the end of this page.
+
+### Measurement Update
+
+Before update, the Mahalanobis distance is calculated between the measured input and the predicted state, the measurement update is not performed for inputs where the Mahalanobis distance exceeds the given threshold.
+
+The predicted state is updated with the latest measured inputs, measured_pose and measured_twist. The updates are performed with the same frequency as prediction, usually at a high frequency, in order to enable smooth state estimation.
+
+## Parameter description
+
+The parameters are set in `launch/ekf_localizer.launch` .
+
+### For Node
+
+| Name                       | Type   | Description                                                                               | Default value |
+| :------------------------- | :----- | :---------------------------------------------------------------------------------------- | :------------ |
+| show_debug_info            | bool   | Flag to display debug info                                                                | false         |
+| predict_frequency          | double | Frequency for filtering and publishing [Hz]                                               | 50.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          |
+
+### For pose measurement
+
+| Name                          | Type   | Description                                                       | Default value |
+| :---------------------------- | :----- | :---------------------------------------------------------------- | :------------ |
+| pose_additional_delay         | double | Additional delay time for pose measurement [s]                    | 0.0           |
+| 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       |
+
+### For twist measurement
+
+| Name                   | Type   | Description                                                        | Default value |
+| :--------------------- | :----- | :----------------------------------------------------------------- | :------------ |
+| twist_additional_delay | double | Additional delay time for twist [s]                                | 0.0           |
+| twist_rate             | double | Approximated input twist rate used for covariance calculation [Hz] | 10.0          |
+| twist_gate_dist        | double | Limit of Mahalanobis distance used for outliers detection          | 10000.0       |
+
+### For process noise
+
+| Name                   | Type   | Description                                                                                                      | Default value |
+| :--------------------- | :----- | :--------------------------------------------------------------------------------------------------------------- | :------------ |
+| 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 = 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.
+
+## How to turn EKF parameters
+
+### 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 confusing radian/degree) or bias noise, and it causes large estimation errors.
+
+### 1. Set sensor parameters
+
+Set sensor-rate and standard-deviation from the basic information of the sensor. The `pose_measure_uncertainty_time` is for uncertainty of the header timestamp data.
+
+- `pose_measure_uncertainty_time`
+- `pose_rate`
+- `twist_rate`
+
+### 2. Set process model parameters
+
+- `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 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.
+
+## Kalman Filter Model
+
+### kinematics model in update function
+
+<img src="./media/ekf_dynamics.png" width="320">
+
+where `b_k` is the yaw-bias.
+
+### time delay model
+
+The measurement time delay is handled by an augmented states [1] (See, Section 7.3 FIXED-LAG SMOOTHING).
+
+<img src="./media/delay_model_eq.png" width="320">
+
+Note that, although the dimension gets larger, since the analytical expansion can be applied based on the specific structures of the augmented states, the computational complexity does not significantly change.
+
+## Test Result with Autoware NDT
+
+<p align="center">
+<img src="./media/ekf_autoware_res.png" width="600">
+</p>
+
+## reference
+
+[1] Anderson, B. D. O., & Moore, J. B. (1979). Optimal filtering. Englewood Cliffs, NJ: Prentice-Hall.