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.

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

####

Launch

The ekf_localizer starts with the default parameters with the following command.

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, used when use_pose_with_covariance is true.

• measured_twist_with_covariance (geometry_msgs/PoseWithCovarianceStamped)

  Input twist source with measurement covariance matrix, used when use_twist_with_covariance is true.

• measured_pose (geometry_msgs/PoseStamped)

  Input pose source, used when use_pose_with_covariance is false.

• measured_twist (geometry_msgs/TwistStamped)

  Input twist source, used when use_twist_with_covariance is false.

• 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_pose (geometry_msgs/PoseStamped)

  Estimated pose.

• ekf_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped)

  Estimated pose with covariance.

• ekf_twist (geometry_msgs/TwistStamped)

  Estimated twist.

• ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped)

  Estimated twist with covariance.

Published TF

• ekf_pose

  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

For pose measurement

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| |use_twist_with_covariance|bool|Flag to use covariance in twist_with_covariance message|false| |twist_stddev_vx|double|Standard deviation for twist linear x [m/s] (used when use_twist_with_covariance is false) |0.2| |twist_stddev_wz|double|Standard deviation for twist angular z [rad/s] (used when use_twist_with_covariance is false) |0.03|

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 = omege |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 comfusing 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
• pose_stddev_x
• pose_stddev_y
• pose_stddev_yaw
• twist_rate
• twist_stddev_vx
• twist_stddev_wz

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

If the position measurement seems more reliable, make these parameters smaller. If the estimated position seems to be noisy due to pose measurement noise, make these values bigger.

• pose_stddev_x
• pose_stddev_y
• pose_stddev_yaw

If the twist measurement seems more reliable, make these parameters smaller. If the estimated twist seems to be noisy due to pose measurement noise, make these values bigger.

• twist_stddev_vx
• twist_stddev_wz

Kalman Filter Model

kinematics model in update function

where b_k is the yaw-bias.

time delay model

Test Result with Autoware NDT