Fusing IMU + Encoders with ROS Robot Localization


Fusing IMU + Encoders with ROS Robot Localization

Summary:

This document walks you through the initial setup and testing of the fusing of IMU + Wheel odometry of a Rover using the robot_localization3,4,5 ROS package. This walkthrough assumes you have went through the Rover Robotics Tutorial 1 - Wheel Odometry6 and Rover Robotics Tutorial 2 - IMU Odometry7 have an IMU publishing to the topic /imu/data and wheel encoder data publishing to the /rr_openrover_basic/odom_encoder topic.

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 odom 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. If you find a work around so you can use a magnetometer on the Rover, we really appreciate it if you would contact us and explain how!


Sources and Documents

[ 1 ] Rover Robotics Docs Page

[ 2 ] rr_openrover_basic 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_basic

rr_control_input_manager

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_basic/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>