Fusing Wheel Odometry and IMU Data Using robot_localization in ROS

Accurate robot localization is very important for robot SLAM and navigation. Using only wheel odometry typically does not provide accurate localization of a mobile ground robot because of the uncertainty resulting from the wheels’ slip and drift. For this reason, it is quite common to fuse the wheel odometry data and the IMU data. In general, it can be said that the sensor data is noisy due to the sensor’s uncertainty. And this uncertainty typically increases with time and more distance from the start position. Hence, data fusion is beneficial. A ROS package called robot_localization is quite common to be used to perform this fusion to improve the localization’s accuracy.

The robot_localization package is a generic state estimator based on EKF and UKF with sensor data fusion capability. It can fuse unlimited number of sensors as long as the sensors provide any of the following messages:

• nav_msgs/Odometry
• geometry_msgs/PoseWithCovarianceStamped
• geometry_msgs/TwistWithCovarianceStamped
• sensor_msgs/Imu

The robot_localization package provides two nodes based on the estimation algorithm used:

• ekf_localization_node – Implementation of an extended Kalman filter (EKF)
• ukf_localization_node – Implementation of an unscented Kalman filter (UKF)

Here is the steps to implement robot_localication to fuse the wheel odometry and IMU data for mobile robot localization.

Step 1: Create your robot_localization package. Let’s call it “my_fused_localization”. Type on the terminal:

This will create the new package having the following structure:

Step 2: Create a launch file to run the robot_localization node. Let’s say we want to use the ekf_localization_node, then we can run this node by using the following launch file. Create the launch file inside the “launch” folder.

Step 3: Create the configuration file for the robot_localization node. As we can see in the launch file above, we need to write a configuration file for the ekf_localization_node. Let’s say it is called “ekf_localization.yaml”. This configuration file looks like this:

Description of the parameters in the configuration file

frequency: the real-valued frequency, in Hz, at which the filter produces a state estimate.

two_d_mode: If your robot is operating in a planar environment and you’re comfortable with ignoring the subtle variations in the ground (as reported by an IMU), then set this to true. It will fuse 0 values for all 3D variables (Z, roll, pitch, and their respective velocities and accelerations). This keeps the covariances for those values from exploding while ensuring that your robot’s state estimate remains affixed to the X-Y plane.

publish_tf: If true, the state estimation node will publish the transform from the frame specified by the world_frame parameter to its child. If the world_frame is the same as the map_frame it will publish the transform from the map_frame to the odom_frame and if the world_frame is the same as the odom_frame it will publish the transform from the odom_frame to the base_link_frame. Defaults to true.

Coordinate frame specification:

• The default values for “map_frame”, “odom_frame”, and “base_link_frame” are “map”, “odom”, and “base_link”, respectively. The “world_frame” parameter defaults to the value of “odom_frame”.
• If your system does not have a map_frame, just remove or comment it, and make sure “world_frame” is set to the value of “odom_frame”.
• If you are only fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set “world_frame” to the value of your “odom_frame”. This is the default behavior for the state estimation nodes in robot_localization, and the most common use for it.

[sensor]: For each sensor, you need to define this parameter based on the topic published by the sensor.

[sensor]_config: is defined by a Boolean 5×3 matrix as follows:

[sensor]_differential: With this parameter, you specify whether the pose variables should be integrated differentially. If a given value is set to true, then for a measurement at time t from the sensor in question, we first subtract the measurement at time t−1, and convert the resulting value to a velocity. This setting is especially useful if your robot has two sources of absolute pose information, e.g., yaw measurements from odometry and an IMU. In that case, if the variances on the input sources are not configured correctly, these measurements may get out of sync with one another and cause oscillations in the filter, but by integrating one or both of them differentially, we avoid this scenario.

process_noise_covariance: commonly denoted Q, is used to model uncertainty in the prediction stage of the filtering algorithms. The values on the diagonals are the variances for the state vector which include pose, then velocities, then linear acceleration. It can be difficult to tune, and has been exposed as a parameter for easier customization. This parameter can be left alone, but you will achieve superior results by tuning it. In general, the larger the value for Q relative to the variance for a given variable in an input message, the faster the filter will converge to the value in the measurement.

initial_estimate_covariance: This parameter allows to set the initial value for the state estimate covariance matrix, which will affect how quickly the filter converges. Here’s the rule you should follow: if you are measuring a variable, make the diagonal value in initial_estimate_covariance larger than that measurement’s covariance. So, for example, if your measurement’s covariance value for the variable in question is 1e-6, make the initial_estimate_covariance diagonal value 1e-3 or something like that.

Step 4: To run the localization node, launch the launch file you created in Step 1. Let’s say your launch file is called “start_filter.launch”, the launch the launch file by typing the following command:

Adding move_base for navigation

If you want to move the robot by using this localization, you can additionally run the “move_base” node in the launch file:

The “my_move_base” launch file above is a launch file which runs the “move_base” node. This node should be wrapped in a separate package called “my_move_base”.

Official documentation: http://docs.ros.org/en/melodic/api/robot_localization/html/state_estimation_nodes.html