Fusing IMU + GPS with robot_location package

asked 2016-08-07 03:41:02 -0500

yrj
3 ●1 ●2 ●3

updated 2016-08-07 06:08:15 -0500

Currently, I am trying to realize the localization by using the robot_localization package based on a GPS and an IMU. THe realization principle is based on the setting in the following link: http://answers.ros.org/question/20007... . I jut copied it here:

ekf_localization_node

Inputs
    IMU (type: sensor_msgs/Imu; topic: /imu; frame_id: base_imu_link)
    Transformed GPS data as an odometry message (navsat_transform_node output: topic: /odometry/gps)

Outputs
    Odometry message (this is what you want to use as your state estimate for your robot; topic: /odometry/filtered)

navsat_transform_node

Inputs
    IMU (type: sensor_msgs/Imu; topic: /imu; frame_id: base_imu_link)
    Raw GPS (type: NavSatFix; topic: /fix; frame_id: gps_reddot)
    Odometry (output of ekf_localization_node: topic: /odometry/filtered)

Outputs
    Transformed GPS data as odometry message (topic: /odometry/gps)

The image of my hardware setting is quite simple: https://drive.google.com/file/d/0BwCt...

My current launch file is:

<launch>
  <include file="$(find gps_ublox)/launch/gps.launch"/>
  <include file="$(find imu_ftdi)/launch/imu.launch"/>
  <!--node name="location_prediction_node" pkg="location_prediction" type="location_prediction_node" output="screen"/-->


  <!-- Parameters setting of the node: ekf_localization_node -->
  <node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 base_link base_imu_link 20"/>
  <node pkg="tf" type="static_transform_publisher" name="gps_tf" args="0 0 0 0 0 0 1 base_link gps_reddot 20"/>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
      <param name="output_frame" value="odom"/>
      <param name="frequency" value="20"/>
      <param name="odom_used" value="true"/>
      <param name="imu_used" value="true"/>
      <param name="vo_used" value="false"/>  
      <param name="sensor_timeout" value="0.1"/>  
      <param name="two_d_mode" value="false"/>

      <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="odom0" value="/odometry/gps"/>
      <param name="imu0"  value="/imu"/> 

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

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

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

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

      <param name="odom0_relative" value="false"/>
      <param name="imu0_relative"  value="false"/>

      <param name="print_diagnostics" value="true"/>

       <!-- ======== ADVANCED PARAMETERS ======== -->

      <param name="odom0_queue_size" value="2"/>
      <param name="imu0_queue_size" value="10"/>

      <rosparam param="process_noise_covariance">[0.05, 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.05, 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.06, 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.03, 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.03, 0.0, 0.0, 0 ...

(more)

edit retag flag offensive close merge delete

add a comment

answered 2016-08-30 02:41:01 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

(1) Both of those are correct, thought I'm surprised the node generates any output when you turn off the base_link->gps_reddot transform. Regardless, you need that transform to be defined, so that's not a problem per se. Also, the node only generates pose data, not twist data. I'm not differentiating pose to get velocity. The real issue is that the output of the node should have been a PoseWithCovarianceStamped, but for legacy reasons, I left it as-is.

(2) This is also what I expect. First, you only have an IMU and GPS, so when you aren't getting GPS signals, your robot's pose is dictated solely by integrating IMU data. Since the only linear (as opposed to rotational) quantity measured by the IMU is acceleration, you're probably going to see a fair amount of drift when you aren't getting GPS measurements. This will cause the filter's error (covariance) to increase, and when you next receive a GPS measurement, its covariance will likely be much lower than the filter's, so the filter will accept that GPS measurement as more or less a measure of ground truth, and will "jump" to that pose.

Bottom line: integrating an IMU with a GPS isn't going to produce fantastic results without a velocity measurement, though it may improve if I ever find time to implement this. Also, if your GPS has reasonable accuracy, then you aren't going to improve its estimate with a filter very much, but rather you will have smoother transitions between its (infrequent) measurements.

edit flag offensive delete link

add a comment

Fusing IMU + GPS with robot_location package

1 Answer

Question Tools

Stats

Related questions

Fusing IMU + GPS with robot_location package edit

1 Answer

Question Tools

Stats

Related questions

Fusing IMU + GPS with robot_location package