Skip to content

Commit 4f3aad2

Browse files
authored
revert: revert "feat(autoware_ekf_localizer)!: porting from universe to core (#9978)" (#10004)
This reverts commit 037c315. Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>
1 parent aa70067 commit 4f3aad2

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

60 files changed

+4385
-2
lines changed

codecov.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -122,6 +122,7 @@ component_management:
122122
- component_id: localization-tier-iv-maintained-packages
123123
name: Localization TIER IV Maintained Packages
124124
paths:
125+
- localization/autoware_ekf_localizer/**
125126
- localization/autoware_gyro_odometer/**
126127
- localization/autoware_localization_error_monitor/**
127128
- localization/autoware_localization_util/**
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2+
Changelog for package autoware_ekf_localizer
3+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
5+
0.40.0 (2024-12-12)
6+
-------------------
7+
* Merge branch 'main' into release-0.40.0
8+
* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 <https://github.com/autowarefoundation/autoware.universe/issues/9587>`_)"
9+
This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5.
10+
* fix: fix ticket links in CHANGELOG.rst (`#9588 <https://github.com/autowarefoundation/autoware.universe/issues/9588>`_)
11+
* chore(package.xml): bump version to 0.39.0 (`#9587 <https://github.com/autowarefoundation/autoware.universe/issues/9587>`_)
12+
* chore(package.xml): bump version to 0.39.0
13+
* fix: fix ticket links in CHANGELOG.rst
14+
* fix: remove unnecessary diff
15+
---------
16+
Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
17+
* fix: fix ticket links in CHANGELOG.rst (`#9588 <https://github.com/autowarefoundation/autoware.universe/issues/9588>`_)
18+
* fix(cpplint): include what you use - localization (`#9567 <https://github.com/autowarefoundation/autoware.universe/issues/9567>`_)
19+
* fix(autoware_ekf_localizer): publish `processing_time_ms` (`#9443 <https://github.com/autowarefoundation/autoware.universe/issues/9443>`_)
20+
Fixed to publish processing_time_ms
21+
* 0.39.0
22+
* update changelog
23+
* Merge commit '6a1ddbd08bd' into release-0.39.0
24+
* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 <https://github.com/autowarefoundation/autoware.universe/issues/9304>`_)
25+
* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 <https://github.com/autowarefoundation/autoware.universe/issues/9304>`_)
26+
* chore(package.xml): bump version to 0.38.0 (`#9266 <https://github.com/autowarefoundation/autoware.universe/issues/9266>`_) (`#9284 <https://github.com/autowarefoundation/autoware.universe/issues/9284>`_)
27+
* unify package.xml version to 0.37.0
28+
* remove system_monitor/CHANGELOG.rst
29+
* add changelog
30+
* 0.38.0
31+
---------
32+
* fix(autoware_ekf_localizer): remove `timer_tf\_` (`#9244 <https://github.com/autowarefoundation/autoware.universe/issues/9244>`_)
33+
Removed timer_tf\_
34+
* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, SakodaShintaro, Yutaka Kondo
35+
36+
0.39.0 (2024-11-25)
37+
-------------------
38+
* Merge commit '6a1ddbd08bd' into release-0.39.0
39+
* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 <https://github.com/autowarefoundation/autoware.universe/issues/9304>`_)
40+
* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 <https://github.com/autowarefoundation/autoware.universe/issues/9304>`_)
41+
* chore(package.xml): bump version to 0.38.0 (`#9266 <https://github.com/autowarefoundation/autoware.universe/issues/9266>`_) (`#9284 <https://github.com/autowarefoundation/autoware.universe/issues/9284>`_)
42+
* unify package.xml version to 0.37.0
43+
* remove system_monitor/CHANGELOG.rst
44+
* add changelog
45+
* 0.38.0
46+
---------
47+
* fix(autoware_ekf_localizer): remove `timer_tf\_` (`#9244 <https://github.com/autowarefoundation/autoware.universe/issues/9244>`_)
48+
Removed timer_tf\_
49+
* Contributors: Esteve Fernandez, SakodaShintaro, Yutaka Kondo
50+
51+
0.38.0 (2024-11-08)
52+
-------------------
53+
* unify package.xml version to 0.37.0
54+
* refactor(localization_util)!: prefix package and namespace with autoware (`#8922 <https://github.com/autowarefoundation/autoware.universe/issues/8922>`_)
55+
add autoware prefix to localization_util
56+
* refactor(ekf_localizer)!: prefix package and namespace with autoware (`#8888 <https://github.com/autowarefoundation/autoware.universe/issues/8888>`_)
57+
* import lanelet2_map_preprocessor
58+
* move headers to include/autoware/efk_localier
59+
---------
60+
* Contributors: Masaki Baba, Yutaka Kondo
61+
62+
0.26.0 (2024-04-03)
63+
-------------------
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_ekf_localizer)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(Eigen3 REQUIRED)
8+
9+
include_directories(
10+
SYSTEM
11+
${EIGEN3_INCLUDE_DIR}
12+
)
13+
14+
ament_auto_find_build_dependencies()
15+
16+
ament_auto_add_library(${PROJECT_NAME} SHARED
17+
src/ekf_localizer.cpp
18+
src/covariance.cpp
19+
src/diagnostics.cpp
20+
src/mahalanobis.cpp
21+
src/measurement.cpp
22+
src/state_transition.cpp
23+
src/warning_message.cpp
24+
src/ekf_module.cpp
25+
)
26+
27+
rclcpp_components_register_node(${PROJECT_NAME}
28+
PLUGIN "autoware::ekf_localizer::EKFLocalizer"
29+
EXECUTABLE ${PROJECT_NAME}_node
30+
EXECUTOR SingleThreadedExecutor
31+
)
32+
33+
target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)
34+
35+
function(add_testcase filepath)
36+
get_filename_component(filename ${filepath} NAME)
37+
string(REGEX REPLACE ".cpp" "" test_name ${filename})
38+
39+
ament_add_gtest(${test_name} ${filepath})
40+
target_link_libraries("${test_name}" ${PROJECT_NAME})
41+
ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
42+
endfunction()
43+
44+
if(BUILD_TESTING)
45+
add_launch_test(
46+
test/test_ekf_localizer_launch.py
47+
TIMEOUT "30"
48+
)
49+
add_launch_test(
50+
test/test_ekf_localizer_mahalanobis.py
51+
TIMEOUT "30"
52+
)
53+
find_package(ament_cmake_gtest REQUIRED)
54+
55+
file(GLOB_RECURSE TEST_FILES test/*.cpp)
56+
57+
foreach(filepath ${TEST_FILES})
58+
add_testcase(${filepath})
59+
endforeach()
60+
endif()
61+
62+
63+
# if(BUILD_TESTING)
64+
# find_package(ament_cmake_ros REQUIRED)
65+
# ament_add_ros_isolated_gtest(ekf_localizer-test test/test_ekf_localizer.test
66+
# test/src/test_ekf_localizer.cpp
67+
# src/ekf_localizer.cpp
68+
# src/kalman_filter/kalman_filter.cpp
69+
# src/kalman_filter/time_delay_kalman_filter.cpp
70+
# )
71+
# target_include_directories(ekf_localizer-test
72+
# PRIVATE
73+
# include
74+
# )
75+
# ament_target_dependencies(ekf_localizer-test geometry_msgs rclcpp tf2 tf2_ros)
76+
# endif()
77+
78+
ament_auto_package(
79+
INSTALL_TO_SHARE
80+
config
81+
launch
82+
)
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,211 @@
1+
# Overview
2+
3+
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 robots such as autonomous driving systems.
4+
5+
## Flowchart
6+
7+
The overall flowchart of the autoware_ekf_localizer is described below.
8+
9+
<p align="center">
10+
<img src="./media/ekf_flowchart.png" width="800">
11+
</p>
12+
13+
## Features
14+
15+
This package includes the following features:
16+
17+
- **Time delay compensation** for input messages, which enables proper integration of input information with varying time delays. This is important especially for high-speed moving robots, such as autonomous driving vehicles. (see the following figure).
18+
- **Automatic estimation of yaw bias** prevents modeling errors caused by sensor mounting angle errors, which can improve estimation accuracy.
19+
- **Mahalanobis distance gate** enables probabilistic outlier detection to determine which inputs should be used or ignored.
20+
- **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).
21+
- **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).
22+
23+
<p align="center">
24+
<img src="./media/ekf_delay_comp.png" width="800">
25+
</p>
26+
27+
<p align="center">
28+
<img src="./media/ekf_smooth_update.png" width="800">
29+
</p>
30+
31+
<p align="center">
32+
<img src="./media/calculation_delta_from_pitch.png" width="800">
33+
</p>
34+
35+
## Node
36+
37+
### Subscribed Topics
38+
39+
| Name | Type | Description |
40+
| -------------------------------- | ------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------- |
41+
| `measured_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose source with the measurement covariance matrix. |
42+
| `measured_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Input twist source with the measurement covariance matrix. |
43+
| `initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. |
44+
45+
### Published Topics
46+
47+
| Name | Type | Description |
48+
| --------------------------------- | --------------------------------------------------- | ----------------------------------------------------- |
49+
| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. |
50+
| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. |
51+
| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. |
52+
| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias |
53+
| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias |
54+
| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. |
55+
| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. |
56+
| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. |
57+
| `debug/processing_time_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | The processing time [ms]. |
58+
59+
### Published TF
60+
61+
- base_link
62+
TF from `map` coordinate to estimated pose.
63+
64+
## Functions
65+
66+
### Predict
67+
68+
The current robot state is predicted from previously estimated data using a given prediction model. This calculation is called at a constant interval (`predict_frequency [Hz]`). The prediction equation is described at the end of this page.
69+
70+
### Measurement Update
71+
72+
Before the 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.
73+
74+
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.
75+
76+
## Parameter description
77+
78+
The parameters are set in `launch/ekf_localizer.launch` .
79+
80+
### For Node
81+
82+
{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json") }}
83+
84+
### For pose measurement
85+
86+
{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }}
87+
88+
### For twist measurement
89+
90+
{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }}
91+
92+
### For process noise
93+
94+
{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json") }}
95+
96+
note: process noise for positions x & y are calculated automatically from nonlinear dynamics.
97+
98+
### Simple 1D Filter Parameters
99+
100+
{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }}
101+
102+
### For diagnostics
103+
104+
{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/diagnostics.sub_schema.json") }}
105+
106+
### Misc
107+
108+
{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/misc.sub_schema.json") }}
109+
110+
## How to tune EKF parameters
111+
112+
### 0. Preliminaries
113+
114+
- 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 an appropriate time due to the timer synchronization problem, use `twist_additional_delay` and `pose_additional_delay` to correct the time.
115+
- Check if the relation between measurement pose and twist is appropriate (whether the derivative of the pose has a 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.
116+
117+
### 1. Tune sensor parameters
118+
119+
Set standard deviation for each sensor. The `pose_measure_uncertainty_time` is for the uncertainty of the header timestamp data.
120+
You can also tune a number of steps for smoothing for each observed sensor data by tuning `*_smoothing_steps`.
121+
Increasing the number will improve the smoothness of the estimation, but may have an adverse effect on the estimation performance.
122+
123+
- `pose_measure_uncertainty_time`
124+
- `pose_smoothing_steps`
125+
- `twist_smoothing_steps`
126+
127+
### 2. Tune process model parameters
128+
129+
- `proc_stddev_vx_c` : set to maximum linear acceleration
130+
- `proc_stddev_wz_c` : set to maximum angular acceleration
131+
- `proc_stddev_yaw_c` : This parameter describes the correlation between the yaw and yaw rate. A 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 estimated yaw is equal to yaw rate. Usually, this should be set to 0.
132+
- `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.
133+
134+
### 3. Tune gate parameters
135+
136+
EKF performs gating using Mahalanobis distance before updating by observation. The gate size is determined by the `pose_gate_dist` parameter and the `twist_gate_dist`. If the Mahalanobis distance is larger than this value, the observation is ignored.
137+
138+
This gating process is based on a statistical test using the chi-square distribution. As modeled, we assume that the Mahalanobis distance follows a chi-square distribution with 3 degrees of freedom for pose and 2 degrees of freedom for twist.
139+
140+
Currently, the accuracy of covariance estimation itself is not very good, so it is recommended to set the significance level to a very small value to reduce rejection due to false positives.
141+
142+
| Significance level | Threshold for 2 dof | Threshold for 3 dof |
143+
| ------------------ | ------------------- | ------------------- |
144+
| $10 ^ {-2}$ | 9.21 | 11.3 |
145+
| $10 ^ {-3}$ | 13.8 | 16.3 |
146+
| $10 ^ {-4}$ | 18.4 | 21.1 |
147+
| $10 ^ {-5}$ | 23.0 | 25.9 |
148+
| $10 ^ {-6}$ | 27.6 | 30.7 |
149+
| $10 ^ {-7}$ | 32.2 | 35.4 |
150+
| $10 ^ {-8}$ | 36.8 | 40.1 |
151+
| $10 ^ {-9}$ | 41.4 | 44.8 |
152+
| $10 ^ {-10}$ | 46.1 | 49.5 |
153+
154+
## Kalman Filter Model
155+
156+
### kinematics model in update function
157+
158+
<img src="./media/ekf_dynamics.png" width="320">
159+
160+
where, $\theta_k$ represents the vehicle's heading angle, including the mounting angle bias.
161+
$b_k$ is a correction term for the yaw bias, and it is modeled so that $(\theta_k+b_k)$ becomes the heading angle of the base_link.
162+
The pose_estimator is expected to publish the base_link in the map coordinate system. However, the yaw angle may be offset due to calibration errors. This model compensates this error and improves estimation accuracy.
163+
164+
### time delay model
165+
166+
The measurement time delay is handled by an augmented state [1] (See, Section 7.3 FIXED-LAG SMOOTHING).
167+
168+
<img src="./media/delay_model_eq.png" width="320">
169+
170+
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.
171+
172+
## Test Result with Autoware NDT
173+
174+
<p align="center">
175+
<img src="./media/ekf_autoware_res.png" width="600">
176+
</p>
177+
178+
## Diagnostics
179+
180+
<p align="center">
181+
<img src="./media/ekf_diagnostics.png" width="320">
182+
</p>
183+
184+
<p align="center">
185+
<img src="./media/ekf_diagnostics_callback_pose.png" width="320">
186+
</p>
187+
<p align="center">
188+
<img src="./media/ekf_diagnostics_callback_twist.png" width="320">
189+
</p>
190+
191+
### The conditions that result in a WARN state
192+
193+
- The node is not in the activate state.
194+
- The initial pose is not set.
195+
- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`.
196+
- The timestamp of the Pose/Twist topic is beyond the delay compensation range.
197+
- The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation.
198+
- The covariance ellipse is bigger than threshold `warn_ellipse_size` for long axis or `warn_ellipse_size_lateral_direction` for lateral_direction.
199+
200+
### The conditions that result in an ERROR state
201+
202+
- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_error`/`twist_no_update_count_threshold_error`.
203+
- The covariance ellipse is bigger than threshold `error_ellipse_size` for long axis or `error_ellipse_size_lateral_direction` for lateral_direction.
204+
205+
## Known issues
206+
207+
- If multiple pose_estimators are used, the input to the EKF will include multiple yaw biases corresponding to each source. However, the current EKF assumes the existence of only one yaw bias. Therefore, yaw bias `b_k` in the current EKF state would not make any sense and cannot correctly handle these multiple yaw biases. Thus, future work includes introducing yaw bias for each sensor with yaw estimation.
208+
209+
## reference
210+
211+
[1] Anderson, B. D. O., & Moore, J. B. (1979). Optimal filtering. Englewood Cliffs, NJ: Prentice-Hall.

0 commit comments

Comments
 (0)