Ask Your Question

Fusing GPS, IMU and odom data

asked 2018-09-29 07:24:20 -0500

Nikhil_1001 gravatar image

Hi, I am trying to fuse the GPS and IMU data using robot_localization package. I have configured the nodes as mentioned here.

I am running

  1. one instance of ekf_localization_node which provides output on topic /odometry/filtered.
  2. one instance for navsat_transform_node which takes input from these 3 topics - /odometry/filtered, /imu/data and /fix.

Now at this point I should get the output on /odometry/gps but there is nothing published on this topic. I know that in order to fuse the GPS, IMU and odom data together I need to run one more instance of ekf_localization_node but since I am not getting any output from navsat_transform_node I haven't proceeded further.

For testing this out I am using the test1.bag provided with robot_localization package inside the test folder.

This is my launch file -

        <node pkg="robot_localization" type="ekf_localization_node" name="ekf_1" clear_params="true">
        <rosparam command="load" file="$(find robot_localization)/params/GPS_IMU.yaml" />
         <!--  Placeholder for output topic remapping
        <remap from="odometry/filtered" to=""/>
        <remap from="accel/filtered" to=""/>

      <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
        <rosparam command="load" file="$(find robot_localization)/params/navsat_transform_GPS_IMU.yaml" />
        <!-- Placeholders for input remapping. Set your topic names as the "to" values.
        <remap from="odometry/filtered" to="odometry/filtered_map"/> -->
        <remap from="imu/data" to="/imu/data"/>  
        <remap from="gps/fix" to="/fix"/>              


Here is the YAML file -

frequency: 30
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

odom0: /husky_velocity_controller/odom

odom0_config: [true,  true,  false,
               false, false, false,
               false, false, false,
               false, false, true,
               false, false, false]

odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #

imu0_remove_gravitational_acceleration: true

use_control: true
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

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 ...
edit retag flag offensive close merge delete


Can you add images of the topics(rqt_graph) and transforms (rosrun tf view-frames) while bag file is playing?

billy gravatar image billy  ( 2018-09-29 11:37:01 -0500 )edit

Hi Billy, here are the images rqt_grapg, view_frames

Nikhil_1001 gravatar image Nikhil_1001  ( 2018-09-29 12:42:00 -0500 )edit

I'm really not an expert but I think you need transforms between baselink and your odom, imu, and GPS. I don't see those.

billy gravatar image billy  ( 2018-09-29 17:25:02 -0500 )edit

The node actually throws error when it doesn't receive transform which is required. I don't see any error so I think I don't need any other transforms. I changed the frame id of the imu messages from "imu_link" to "base_link" and now data is getting published over /odometry/gps but its all 0.0

Nikhil_1001 gravatar image Nikhil_1001  ( 2018-10-02 12:50:40 -0500 )edit

1 Answer

Sort by » oldest newest most voted

answered 2018-10-05 09:48:00 -0500

Subodh Malgonde gravatar image

updated 2018-10-05 09:48:59 -0500

The navsat_transform node, as the name suggests, performs a transform. In this case it transforms the GPS data to robot’s world frame coordinates (i.e., the frame with its origin at the robot’s start position).

This transformed data can then be fused with another state estimate - in your case the state estimate is provided by an ekf_localization_node which fuses data from the IMU sensors. So you need 3 nodes:

  1. Kalman filter node to fuse together data from sensors like IMU, wheel encoder etc. Lets call this state_estimate_local.
  2. Navsat transform node to transform GPS data to robot's world frame coordinates.
  3. Another instance of kalman filter to fuse the transformed GPS data with state_estimate_local.

In general for a robot with an IMU and a wheel encoder (or any other source of odometry) the node graph should look like this:

image description

Right now you are missing the second kalman filter node. If you are just using the heading information from the IMU then you can skip the first kalman filter node and directly fuse the IMU data with the transformed GPS data.

edit flag offensive delete link more


Hi Subodh I know that I need another kalman filter but the output of navsat transform node is all (x,y,z)= 0.0 Before using the 2nd kalman filter I need to get proper output from navsat_transform on topic odometry/gps

Nikhil_1001 gravatar image Nikhil_1001  ( 2018-10-08 01:08:02 -0500 )edit

Hey, any updates on this?

joaocabogon gravatar image joaocabogon  ( 2019-07-18 12:13:49 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2018-09-29 07:24:20 -0500

Seen: 1,078 times

Last updated: Oct 05 '18