Fusing Wheel Odometry, IMU Data, and GPS Data Using robot_localization in ROS

A post on fusing the wheel odometry and IMU data using robot_localization package in ROS can be found here:

Now we are going to add GPS data to the wheel odometry and IMU data. These three measurements are going to be fused by using robot_localization package. GPS provides the position of a robot with respect to the Earth frame. This is an advantage. However, it has some shortcomings including:

• Large error. Nevertheless, special types of GPS such as DGPS and RTK have far less error but they are expensive.
• Only position is provided. GPS alone does not provide any information about orientation

When fused with wheel odometry and IMU data, the large error of the GPS can be overcome.

Below are some steps to fuse the GPS data with wheel odometry and IMU data:

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

This will create the new package having the following structure:

Step 2: Create a blank map which consists of two files. Let’s call them: “my_map_empty.pgm” and “my_map_empty.yaml”. Place these two files inside the “maps” folder.

Afterwards, create a launch file to run the “map_server” and load the map. Let’s name it “start_map_server.launch”:

Step 3: Create a launch file to run the “navsat_transform” node. Let’s call it “start_navsat_transform.launch”:

Step 4: Create a launch file to run the robot_localization node. Let’s call it “ekf_localization.launch”:

Step 5: So far, we have three launch files, namely:

• start_map_server.launch
• start_navsat_transform.launch
• ekf_localization.launch

Now, let’s have another launch file to launch all the launch files above. Let’s call it “gps_localization.launch”:

Step 6: Now 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. This is not our case because we also use GPS data which is not continuous.
• If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then set your “world_frame” to the value of your “map_frame”. Also make sure that something else is generating the odom->base_link transform. This can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data. This is our case because we use GPS data.

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

If you fuse the output of the “navsat_transform” node with any of the state estimation nodes in robot_localization, you should make sure that the odomN_differential setting (where N is a number starting from 0) is false for that input.

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 7: To run the localization node, launch the launch file you created in Step 5:

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

Summary

The package folder looks like this:

Official documentation:

• http://docs.ros.org/en/melodic/api/robot_localization/html/state_estimation_nodes.html
• http://docs.ros.org/en/melodic/api/robot_localization/html/navsat_transform_node.html