Ask Your Question
0

IMU and GPS fusion without odom - robot_localization

asked 2016-06-09 13:06:27 -0500

torugobeck gravatar image

updated 2016-10-24 08:33:17 -0500

ngrennan gravatar image

I'm using ros Indigo, and trying to fuse IMU and GPS, I don't have any odom source. Reading forum and the roscon presentation about the robot_localization package, I tried the following configuration:

   <launch>
  <!-- gps and imu fusion -->
    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
    <param name="magnetic_declination_radians" value="0.36878808"/>
    <param name="zero_altitude" value="false"/>
    <param name="publish_filtered_gps" value="true"/>
    <param name="broadcast_utm_transform" value="true"/>
    <param name="wait_for_datum" value="true"/>

    <remap from="/gps/fix" to="/gps"/>
    <remap from="/imu/data" to="/imu"/>
    <remap from="/odometry/filtered" to="/odometry/map"/>
  </node>

 <!-- map frame -->
  <node pkg="robot_localization" type="ekf_localization_node"  name="map_transform" clear_params="true">

    <param name="odom0" value="odometry/gps"/>
    <param name="imu0" value="/imu"/>

    <param name="frequency" value="30"/>
    <param name="sensor_timeout" value="2"/>
    <param name="two_d_mode" value="true"/>

    <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="map"/>

    <rosparam param="imu0_config">[false, false, false,
                                   false, false, true,
                                   false, false, false,
                                   false, false, true,
                                   true, false, false]</rosparam>

    <param name="imu0_differential" value="false"/>
    <param name="imu0_remove_gravitational_acceleration" value="true"/>

    <rosparam param="odom0_config">[true, true, false,
                                    false, false, false,
                                    false, false, false,
                                    false, false, false,
                                    false, false, false]</rosparam>

      <param name="odom0_differential" value="false"/>

    <param name="print_diagnostics" value="true"/>
    <param name="debug"           value="false"/>
    <param name="debug_out_file"  value="$(env HOME)/adroit_files/debug_ekf_localization.txt"/>

    <remap from="odometry/filtered" to="/odometry/map"/>

  </node>



<!-- odom frame -->
  <node pkg="robot_localization" type="ekf_localization_node"  name="odom_transform" clear_params="true">

    <param name="imu0" value="/imu"/>
    <param name="odom0" value="/odometry/map"/>

    <param name="frequency" value="30"/>
    <param name="sensor_timeout" value="2"/>
    <param name="two_d_mode" value="true"/>

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

    <rosparam param="imu0_config">[false, false, false,
                                   false, false, true,
                                   false, false, false,
                                   false, false, true,
                                   true, false, false]</rosparam>

    <rosparam param="odom0_config">[true, true, false,
                                    false, false, false,
                                    false, false, false,
                                    false, false, false,
                                    true, false, false]</rosparam>

   <param name="odom0_differential" value="false"/>


    <param name="print_diagnostics" value="true"/>
    <param name="debug"           value="false"/>
    <param name="debug_out_file"  value="$(env HOME)/debug_ekf_localization.txt"/>

</node>

</launch>

as result, I'm getting a drift that increases in time, and the vector is always perpendicular to robot.

This warning appear several times too

[ WARN] [1465502165.359329185, 20.510000000]: Could not obtain base_link-> transform. Will not remove offset of navsat device from robot's origin.
[ WARN] [1465502167.550982133, 21.310000000]: Could not obtain transform from  to base_link. Error was Invalid argument passed to lookupTransform argument source_frame in tf2 frame_ids cannot be empty

I'm not using a real robot yet, for now I'm on gazebo, using hector_gazebo plugins for IMU and GPS.

IMU sample message:

  header: 
  seq: 10464
  stamp: 
    secs: 10
    nsecs: 918000000
  frame_id: base_link
orientation: 
  x: 0.019510418191
  y: -0.00138183083974
  z: -0.00971101353096
  w: 0.999761536739
orientation_covariance: [2.6030820491461885e-07, 0.0, 0.0, 0.0, 2.6030820491461885e-07, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00310991562662
  y: -0.00912178750215
  z ...
(more)
edit retag flag offensive close merge delete

Comments

How exactly did you get the gps plugin to work..?

rajnunes gravatar imagerajnunes ( 2016-06-11 05:55:22 -0500 )edit

added the plugin configuration at the topic, to big to post here

torugobeck gravatar imagetorugobeck ( 2016-06-13 09:59:18 -0500 )edit

Hmmm...this may be an issue. Can you post a bag?

Tom Moore gravatar imageTom Moore ( 2016-06-14 04:07:29 -0500 )edit

I generated without any node open, just the gazebo with the plugins https://www.dropbox.com/s/11tcvbdze5a...

torugobeck gravatar imagetorugobeck ( 2016-06-14 11:52:00 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-06-15 04:24:42 -0500

Tom Moore gravatar image

So there are a lot of issues here:

  1. You appear to be feeding the output of your map_transform instance of ekf_localization_node into your odom_transform instance. That is probably not going to work well at all.
  2. You have wait_for_datum set to true for navsat_transform_node, but your other settings indicate that you don't actually want this set to true. I just realized that I need to document that parameter on the wiki, so that's my fault. In any case, it's not relevant for your use case.
  3. If all you have is IMU and GPS data, then you don't need to run two EKFs, as it won't really help you.

I did find a bug with the node crashinig when publish_filtered_gps was set to true. I have fixed it and it is in the upstream repository. Please pull down that version until I can get a new release out.

Anyway, here are the settings that worked for me. Please note that I added a static transform publisher for the base_link->gps transform:

<launch>

  <node name="bl_gps" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 1 base_link gps_link" />

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
  <param name="magnetic_declination_radians" value="0.36878808"/>
  <param name="zero_altitude" value="false"/>
  <param name="publish_filtered_gps" value="true"/>
  <param name="broadcast_utm_transform" value="true"/>
  <param name="wait_for_datum" value="false"/>

  <remap from="/gps/fix" to="/gps"/>
  <remap from="/imu/data" to="/imu"/>
</node>

<node pkg="robot_localization" type="ekf_localization_node"  name="ekf_odom" clear_params="true">

  <param name="odom0" value="odometry/gps"/>
  <param name="imu0" value="/imu"/>

  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="2"/>
  <param name="two_d_mode" value="true"/>

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

  <rosparam param="imu0_config">[false, false, false,
                                 false, false, true,
                                 false, false, false,
                                 false, false, true,
                                 true, false, false]</rosparam>

  <param name="imu0_differential" value="false"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>

  <rosparam param="odom0_config">[true, true, false,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <param name="odom0_differential" value="false"/>

  <param name="print_diagnostics" value="true"/>
  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="$(env HOME)/adroit_files/debug_ekf_localization.txt"/>

</node>

</launch>

edit flag offensive delete link more

Comments

Hello Tom, thanks for your help. Actually I'm using Datum, I have a python script that set it. I'm using since I have a map that is streamed through map_server, and I want the origin of map to be the gps coordinate set in datum. I'll try this configuration with a static tf from map to odom.

torugobeck gravatar imagetorugobeck ( 2016-06-15 10:15:41 -0500 )edit

I just tried this configuration that you posted, with wait_for_datum and publish_filtered_gps set to false, but i'm still getting the drift in the position, without any other odom source, this continuous drift is expected?

torugobeck gravatar imagetorugobeck ( 2016-06-15 11:15:01 -0500 )edit

Is your GPS drifting?

Tom Moore gravatar imageTom Moore ( 2016-06-15 11:33:44 -0500 )edit

I ploted some data in matlab, and it look pretty stable, as you can see here: https://www.dropbox.com/s/1rauxv3ajbk...

torugobeck gravatar imagetorugobeck ( 2016-06-15 14:07:40 -0500 )edit

Try removing the linear acceleration from your state estimate (odom0 input).

Tom Moore gravatar imageTom Moore ( 2016-06-15 15:15:48 -0500 )edit

I'm using the same launch file that you posted, it already have this configuration. https://www.dropbox.com/s/9fbjdn9l1ru... But Im' getting this output as result. Can you see any problem that may cause this?

torugobeck gravatar imagetorugobeck ( 2016-06-15 15:37:20 -0500 )edit

No, the file I posted has X linear acceleration enabled (the last "true" value in imu0_config). I am suggesting that you set it to false instead.

Tom Moore gravatar imageTom Moore ( 2016-06-16 03:19:54 -0500 )edit

Sorry I thought that you were talking about the odom0 parameter, it worked well. I have one more doubt, the odom output is getting perpendicular to the robot, do you have any idea what may cause this? Thanks for the Help!!

torugobeck gravatar imagetorugobeck ( 2016-06-16 11:20:19 -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

3 followers

Stats

Asked: 2016-06-09 13:06:27 -0500

Seen: 1,956 times

Last updated: Jun 15 '16