Ask Your Question
3

Fusing absolute position information and IMU-data using the ekf_localization_node

asked 2015-12-22 08:32:07 -0500

Febert gravatar image

updated 2016-01-06 10:49:46 -0500

Hello,

I have a robot car with a laser-scanner and an imu and would like to fuse the position information of the two sensors.

For laser based localization I am using the hector_mapping node which produces a /poseupdate topic. Additionally I am using an imu producing the /imu/data topic.

Those two shall be fused to provide a more accurate position estimate at a higher rate.

After incorporating the suggestions from Tom Moore my launch files look like this:

Edit 1

I am getting better behaviour now. What I did is based on the following thoughts: The odom->baselink transformation according to Rep105 should only be computed by odometry sources (by def. relativ/differential information). However before I was using differential set to false in the /imu0 data of the odom->baselink ekf, so the odometry got the absolute heading information from the imu. Could this have caused the orientation errors?

As I understood Rep105 the map->odom transform is computed indirectly by using absolute sensory information. So I thought it needs the absolute information from all source. That is why I set, differential to false for the imu topic of the map->odom ekf.

Here are my updated launchfiles (the mess with hector frames reverted):

The odom-baselink ekf-instance:

    <launch>
  <!-- Start Navigation Stack -->

  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
    <param name="map_frame" value="map"/>   
    <param name="odom_frame" value="odom"/>  

    <param name="map_size" value="2048"/>
    <param name="pub_map_odom_transform" value="false"/>
    <param name="pub_map_scanmatch_transform" value="false"/>
  </node>


  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" output="screen">
  <param name="print_diagnostics" value="true"/>
  <param name="frequency" value="100"/>
  <param name="two_d_mode" value="true"/>

  <param name="map_frame" value="map"/>  
  <param name="odom_frame" value="odom"/>
  <param name="world_frame" value="odom"/>   
  <param name="base_link_frame" value="base_link"/>

  <param name="pose0" value="/poseupdate"/>
  <rosparam param="pose0_config">  
    [true, true,  false,    <!-- You have two_d_mode on, so making Z true does nothing -->
    false, false, true,  
    false, false, false,
    false, false, false,
    false, false, false]
  </rosparam>
  <param name="pose0_differential" value="true"/>

  <param name="imu0" value="/imu"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>    

  <rosparam param="imu0_config">  
    [false, false, false,
     false,  false,  true,     <!-- 2D mode, so roll and pitch do nothing -->
     false, false, false,
     false,  false,  true,     <!-- As above -->
     true,  true,  false]    <!-- Start with these turned off, verify that everything works, then turn X and Y back on (Z does nothing in 2D mode) -->
  </rosparam>
  <param name="imu0_relative" value="true"/>
  <param name="imu0_differential" value="true"/> 

  </node>

The map-odom ekf:

<launch>

  <node pkg="robot_localization" type="ekf_localization_node" name="aux_ekf_localization_node" output="screen">
   <param name="print_diagnostics" value="true"/>
  <param name="frequency" value="100"/>
  <param name="two_d_mode" value="true"/>

  <param name="map_frame" value="map"/>  
  <param name="odom_frame" value="odom"/>
  <param name="world_frame" value="map"/>   
  <param name="base_link_frame" value="base_link"/>

  <param name="pose0" value="/poseupdate"/>
  <rosparam param="pose0_config">  
    [true, true,  false,
    false, false, true,  
    false, false, false,
    false, false, false,
    false, false, false]
  </rosparam>
  <param name="pose0_differential" value="false"/>

  <param name="imu0" value="/imu"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>    

  <rosparam param="imu0_config">  
    [false, false, false,
     false,  false,  true,    
     false ...
(more)
edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
2

answered 2016-01-05 07:42:54 -0500

Tom Moore gravatar image

updated 2016-01-06 07:35:06 -0500

Can you please add a sample message from each sensor input? Also, I would advise that you try running the filter with just the pose data as a test, and then add the IMU. The issue is very likely that you are using linear accelerations from your IMU, but don't have a velocity reference.

I'm not convinced this setup really conforms to REP-105, though. Your map frame estimate should be the most globally accurate pose estimate, though it may be subject to discrete jumps, and your odom frame estimate should be continuous, but can drift (though we don't want the kind of drift you're seeing, of course). If I were you, I'd change my setup to this (note that I'd still want to see input messages from hector_mapping and the IMU to verify):

  1. Turn off pub_map_odom_transform in hector_mapping.
  2. In the launch file you posted, set pose0_differential to true.
  3. I would also turn off roll, pitch, and yaw from that instance, but let the angular velocities alone.
  4. Add a second launch file for a second instance of ekf_localization_node.
  5. In that instance, set world_frame to map.
  6. Otherwise, keep the same topics, but turn off pose0_differential.
  7. (Not sure about this step). In that instance, also turn off linear acceleration, or add a pose1 topic that is identical to pose0 (same input topic), but has differential set to true. That will give you a velocity reference to help constrain the drift.

Again, though, I'd have to see sample input data to be sure.

EDIT 1

Also, do you really need to run at 500 Hz? What is your IMU frequency? You could also bring down the frequency and up the queue_size for that sensor.

EDIT 2

Your frame configs are incorrect, and I found a few other things I'd tweak if I were you. Try these instead:

odom->base_link instance

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node_odom" output="screen">
  <param name="print_diagnostics" value="true"/>
  <param name="frequency" value="100"/>
  <param name="two_d_mode" value="true"/>

  <param name="map_frame" value="map"/>  
  <param name="odom_frame" value="odom"/>
  <param name="world_frame" value="odom"/>   
  <param name="base_link_frame" value="base_link"/>

  <param name="pose0" value="/poseupdate"/>
  <rosparam param="pose0_config">  
    [true, true,  false,    <!-- You have two_d_mode on, so making Z true does nothing -->
    false, false, true,  
    false, false, false,
    false, false, false,
    false, false, false]
  </rosparam>
  <param name="pose0_differential" value="true"/>

  <param name="imu0" value="/imu"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>    

  <rosparam param="imu0_config">  
    [false, false, false,
     false,  false,  true,     <!-- 2D mode, so roll and pitch do nothing -->
     false, false, false,
     false,  false,  true,     <!-- As above -->
     false,  false,  false]    <!-- Start with these turned off, verify that everything works, then turn X and Y back on (Z does nothing in 2D mode) -->
  </rosparam>
  <param name="imu0_relative" value="true"/>
  <param name="imu0_differential" value="false"/> 

  </node>      
</launch>

map->odom instance

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node_map" output="screen">
  <param name="print_diagnostics" value="true"/>
  <param name="frequency" value="100"/>
  <param name="two_d_mode" value="true"/>

  <param name="map_frame ...
(more)
edit flag offensive delete link more
0

answered 2016-01-05 13:58:47 -0500

Febert gravatar image

updated 2016-01-06 04:29:58 -0500

Here is a sample from the /imu data: ( It is from a gazebo simulation, I can provide the real data on Friday)

header: 
  seq: 44640
  stamp: 
    secs: 446
    nsecs: 700000000

  frame_id: base_link

orientation: 
  x: -0.0239752552915
  y: -0.000348687880234
  z: -0.997080646516
  w: -0.0724931023903
orientation_covariance: [0.0012755102040816321, 0.0, 0.0, 0.0, 0.0012755102040816321, 0.0, 0.0, 0.0, 0.0]

angular_velocity: 
  x: -0.207346366252
  y: 0.0202127792674
  z: -0.0457584426013
angular_velocity_covariance: [0.0025000000000000005, 0.0, 0.0, 0.0, 0.0025000000000000005, 0.0, 0.0, 0.0, 0.000225]

linear_acceleration: 
  x: 0.726588580842
  y: 0.193814955902
  z: 9.78251465653

linear_acceleration_covariance: [0.12249999999999998, 0.0, 0.0, 0.0, 0.12249999999999998, 0.0, 0.0, 0.0, 0.09]

and here is a sample from the /poseupdate:

---
header: 
  seq: 1976
  stamp: 
    secs: 887
    nsecs: 479000000
  frame_id: map
pose: 
  pose: 
    position: 
      x: 0.0102119445801
      y: -0.00177955627441
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.0152736048181
      w: 0.999883351695
  covariance: [31.761859893798828, -3.0734965801239014, 0.0, 0.0, 0.0, -455.1424560546875, -3.0734965801239014, 47.1007080078125, 0.0, 0.0, 0.0, 3407.331787109375, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -455.1424560546875, 3407.331787109375, 0.0, 0.0, 0.0, 846946.375]
---

Side note: The /imu data comes at 100Hz, the poseupdate at 39Hz, I changed the ekf update rate to be also 100Hz.

edit flag offensive delete link more
0

answered 2018-08-23 04:25:05 -0500

billyDong gravatar image

Did you solve you problem ? I think I've found a solution using the laser odometry of the hector slam, check this question: 301359

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-12-22 08:30:44 -0500

Seen: 1,363 times

Last updated: Aug 23 '18