Skip to content

Commit

Permalink
feat(ekf_localizer): allow multi sensor inputs in ekf_localizer (tier…
Browse files Browse the repository at this point in the history 
…4#1027)

* first commit

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* updated

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* deque to queue

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* queue debugged

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* deque to queue

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* queue didn't support q.clear()...

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* for debug, and must be ignored later

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* removed dummy variables

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* run pre-commit

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* update readme

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* update readme

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* reflected some review comments

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* reflected some review comments

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* added smoothing_steps param in pose_info and twist_info

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* use withcovariance in PoseInfo & TwistInfo, now build works

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* (not verified yet) update z, roll, pitch using 1D filter

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* added TODO comments

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* update initialization of simple1DFilter

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* fixed a bug (=NDT did not converge when launching logging_simulator)

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* debug

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* change gnss covariance, may have to be removed from PR

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* removed unnecessary comments

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* added known issue

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* change the default gnss covariance to the previous one

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* pre-commit

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@kminoda @pre-commit-ci @boyali
2 people authored and boyali committed Oct 3, 2022
1 parent b41e2c8 commit 86d66c9
Show file tree
Hide file tree
Showing 5 changed files with 213 additions and 97 deletions.
2 changes: 1 addition & 1 deletion ...4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
<include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
<arg name="enable_yaw_bias_estimation" value="False"/>
<arg name="tf_rate" value="50.0"/>
<arg name="twist_rate" value="25.0"/>
<arg name="twist_smoothing_steps" value="2"/>
<arg name="input_initial_pose_name" value="/initialpose3d"/>

<arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
Expand Down
74 changes: 40 additions & 34 deletions localization/ekf_localizer/README.md
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
# 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.
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.

## Flowchart

Expand All @@ -14,10 +14,10 @@ The overall flowchart of the ekf_localizer is described below.

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

<p align="center">
<img src="./media/ekf_delay_comp.png" width="800">
Expand All @@ -43,15 +43,15 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi

- measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped)

Input pose source with measurement covariance matrix.
Input pose source with the measurement covariance matrix.

- measured_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped)

Input twist source with measurement covariance matrix.
Input twist source with the 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.
Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published.

### Published Topics

Expand Down Expand Up @@ -81,7 +81,7 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi

- ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped)

Estimated twist with covariance.
The estimated twist with covariance.

### Published TF

Expand All @@ -93,13 +93,13 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi

### 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.
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.

### 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.
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.

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

Expand All @@ -117,20 +117,20 @@ The parameters are set in `launch/ekf_localizer.launch` .

### 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 |
| 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_smoothing_steps | int | A value for smoothing steps | 5 |
| 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 |
| Name | Type | Description | Default value |
| :--------------------- | :----- | :-------------------------------------------------------- | :------------ |
| twist_additional_delay | double | Additional delay time for twist [s] | 0.0 |
| twist_smoothing_steps | int | A value for smoothing steps | 2 |
| twist_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 |

### For process noise

Expand All @@ -141,28 +141,30 @@ The parameters are set in `launch/ekf_localizer.launch` .
| 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.
note: process noise for positions x & y are calculated automatically from nonlinear dynamics.

## How to turn EKF parameters
## How to tune 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.
- 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.
- 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.

### 1. Set sensor parameters
### 1. Tune 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.
Set standard deviation for each sensor. The `pose_measure_uncertainty_time` is for the uncertainty of the header timestamp data.
You can also tune a number of steps for smoothing for each observed sensor data by tuning `*_smoothing_steps`.
Increasing the number will improve the smoothness of the estimation, but may have an adverse effect on the estimation performance.

- `pose_measure_uncertainty_time`
- `pose_rate`
- `twist_rate`
- `pose_smoothing_steps`
- `twist_smoothing_steps`

### 2. Set process model parameters
### 2. Tune 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_c` : This parameter describes the correlation between the yaw and yawrate. A large value means the change in yaw does not correlate to the estimated yawrate. If this is set to 0, it means the change in estimated yaw is equal to yawrate. 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
Expand All @@ -171,22 +173,26 @@ Set sensor-rate and standard-deviation from the basic information of the sensor.

<img src="./media/ekf_dynamics.png" width="320">

where `b_k` is the yaw-bias.
where `b_k` is the yawbias.

### time delay model

The measurement time delay is handled by an augmented states [1] (See, Section 7.3 FIXED-LAG SMOOTHING).
The measurement time delay is handled by an augmented state [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.
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>

## Known issues

- In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation.

## reference

[1] Anderson, B. D. O., & Moore, J. B. (1979). Optimal filtering. Englewood Cliffs, NJ: Prentice-Hall.
84 changes: 78 additions & 6 deletions localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,74 @@
#include <chrono>
#include <iostream>
#include <memory>
#include <queue>
#include <string>
#include <vector>

struct PoseInfo
{
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose;
int counter;
int smoothing_steps;
};

struct TwistInfo
{
geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist;
int counter;
int smoothing_steps;
};

class Simple1DFilter
{
public:
Simple1DFilter()
{
initialized_ = false;
x_ = 0;
stddev_ = 1e9;
proc_stddev_x_c_ = 0.0;
return;
};
void init(const double init_obs, const double obs_stddev, const rclcpp::Time time)
{
x_ = init_obs;
stddev_ = obs_stddev;
latest_time_ = time;
initialized_ = true;
return;
};
void update(const double obs, const double obs_stddev, const rclcpp::Time time)
{
if (!initialized_) {
init(obs, obs_stddev, time);
return;
}

// Prediction step (current stddev_)
double dt = (time - latest_time_).seconds();
double proc_stddev_x_d = proc_stddev_x_c_ * dt;
stddev_ = std::sqrt(stddev_ * stddev_ + proc_stddev_x_d * proc_stddev_x_d);

// Update step
double kalman_gain = stddev_ * stddev_ / (stddev_ * stddev_ + obs_stddev * obs_stddev);
x_ = x_ + kalman_gain * (obs - x_);
stddev_ = std::sqrt(1 - kalman_gain) * stddev_;

latest_time_ = time;
return;
};
void set_proc_stddev(const double proc_stddev) { proc_stddev_x_c_ = proc_stddev; }
double get_x() { return x_; }

private:
bool initialized_;
double x_;
double stddev_;
double proc_stddev_x_c_;
rclcpp::Time latest_time_;
};

class EKFLocalizer : public rclcpp::Node
{
public:
Expand Down Expand Up @@ -87,6 +152,9 @@ class EKFLocalizer : public rclcpp::Node
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_br_;
//!< @brief extended kalman filter instance.
TimeDelayKalmanFilter ekf_;
Simple1DFilter z_filter_;
Simple1DFilter roll_filter_;
Simple1DFilter pitch_filter_;

/* parameters */
bool show_debug_info_;
Expand Down Expand Up @@ -138,16 +206,18 @@ class EKFLocalizer : public rclcpp::Node
};

/* for model prediction */
geometry_msgs::msg::TwistStamped::SharedPtr
current_twist_ptr_; //!< @brief current measured twist
geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr_; //!< @brief current measured pose
geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose
std::queue<TwistInfo> current_twist_info_queue_; //!< @brief current measured pose
std::queue<PoseInfo> current_pose_info_queue_; //!< @brief current measured pose
geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose
geometry_msgs::msg::PoseStamped
current_ekf_pose_no_yawbias_; //!< @brief current estimated pose w/o yaw bias
geometry_msgs::msg::TwistStamped current_ekf_twist_; //!< @brief current estimated twist
std::array<double, 36ul> current_pose_covariance_;
std::array<double, 36ul> current_twist_covariance_;

int pose_smoothing_steps_;
int twist_smoothing_steps_;

/**
* @brief computes update & prediction of EKF for each ekf_dt_[s] time
*/
Expand Down Expand Up @@ -192,13 +262,13 @@ class EKFLocalizer : public rclcpp::Node
* @brief compute EKF update with pose measurement
* @param pose measurement value
*/
void measurementUpdatePose(const geometry_msgs::msg::PoseStamped & pose);
void measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);

/**
* @brief compute EKF update with pose measurement
* @param twist measurement value
*/
void measurementUpdateTwist(const geometry_msgs::msg::TwistStamped & twist);
void measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist);

/**
* @brief check whether a measurement value falls within the mahalanobis distance threshold
Expand Down Expand Up @@ -241,6 +311,8 @@ class EKFLocalizer : public rclcpp::Node
*/
void showCurrentX();

void updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);

tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;

friend class EKFLocalizerTestSuite; // for test code
Expand Down
8 changes: 4 additions & 4 deletions localization/ekf_localizer/launch/ekf_localizer.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,13 @@
<arg name="input_pose_with_cov_name" default="in_pose_with_covariance"/>
<arg name="pose_additional_delay" default="0.0"/>
<arg name="pose_measure_uncertainty_time" default="0.01"/>
<arg name="pose_rate" default="10.0"/>
<arg name="pose_smoothing_steps" default="5"/>
<arg name="pose_gate_dist" default="10000.0"/>

<!-- for twist measurement -->
<arg name="input_twist_with_cov_name" default="in_twist_with_covariance"/>
<arg name="twist_additional_delay" default="0.0"/>
<arg name="twist_rate" default="10.0"/>
<arg name="twist_smoothing_steps" default="2"/>
<arg name="twist_gate_dist" default="10000.0"/>

<!-- for process model -->
Expand Down Expand Up @@ -53,11 +53,11 @@

<param name="pose_additional_delay" value="$(var pose_additional_delay)"/>
<param name="pose_measure_uncertainty_time" value="$(var pose_measure_uncertainty_time)"/>
<param name="pose_rate" value="$(var pose_rate)"/>
<param name="pose_smoothing_steps" value="$(var pose_smoothing_steps)"/>
<param name="pose_gate_dist" value="$(var pose_gate_dist)"/>

<param name="twist_additional_delay" value="$(var twist_additional_delay)"/>
<param name="twist_rate" value="$(var twist_rate)"/>
<param name="twist_smoothing_steps" value="$(var twist_smoothing_steps)"/>
<param name="twist_gate_dist" value="$(var twist_gate_dist)"/>

<param name="proc_stddev_yaw_c" value="$(var proc_stddev_yaw_c)"/>
Expand Down
Loading

0 comments on commit 86d66c9

Please sign in to comment.