Summary:
This document walks you through how to fuse IMU data with wheel encoder data of a Rover Pro using the robot_localization ROS package. This is useful to make the /odom to /base_link transform that move_base uses more reliable, especially while turning.
This walk-through assumes you have IMU data and wheel encoder data publishing to ROS topics.
Step 1 - Make the odom_ekf.launch file using launch file code below
Create a new launch file using the launch file code given at the bottom of this tutorial. Be sure to change the bolded rosparams to your wheel odometry topic and imu data topic.
Step 2 - Verify output of EKF using one data source at a time
Run the ekf node and pipe in only one of the 2 sources. One way to do this is to start only the wheel odom or imu nodes at a time. Verify the output from the EKF node is reasonable in that the output appears to be just a filtered version of the input and there’s no funny twitches, jerking, or quick sliding. When checking the wheel odometry, I’d recommend using RVIZ to visualize the wheel encoder input and EKF output at the same time. They should appear to be the same. When testing the EKF output with just IMU input, verify the ekf output is turning in the correct direction and no quick sliding or quick rotations are happening when the robot is stationary.
Notes
The magnetic fields produced from the Rover’s motors will interfere with magnetometer readings so it is highly recommended to disable magnetometers and/or magnetometer fusing in your IMU when on the Rover. Even when the robot is stationary there is enough interference that magnetometer readings become worthless.
Sources and Documents
[ 1 ] Rover Robotics Docs Page
[ 2 ] rr_openrover_driver ROS Wiki
[ 3 ] robot_localization ROS wiki
[ 4 ] robot_localization documentation
[ 5 ] Highly Recommended to Watch This Video by Tom Moore from ROScon 2015
[ 6 ] Rover Robotics Tutorial 1 - Wheel Odometry
[ 7 ] Rover Robotics Tutorial 2 - IMU Odometry
ROS Packages Required
rr_openrover_stack
robot_localization
Example node setup in Launch File for robot_localization package
<!-- Odom node (Encoders + IMU) -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom_node" output="screen" >
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="true"/>
<remap from="odometry/filtered" to="odom/ekf/enc_imu"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/rr_openrover_driver/odom_encoder"/>
<param name="odom0_differential" value="false" />
<param name="odom0_relative" value="false" />
<param name="odom0_queue_size" value="10" />
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]</rosparam>
<param name="imu0" value="/imu/data"/>
<param name="imu0_differential" value="false" />
<param name="imu0_relative" value="true" />
<param name="imu0_queue_size" value="10" />
<param name="imu0_remove_gravitational_acceleration" value="true" />
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
true , true, true,
true, true, true]</rosparam>
<param name="print_diagnostics" value="true" />
<param name="debug" value="false" />
<param name="debug_out_file" value="debug_odom_ekf.txt" />
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025,0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025,0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.005]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1 , 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1 , 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 , 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>