0
$\begingroup$

Rosanswers logo

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/200071/how-to-fuse-imu-gps-using-robot_localization/ . 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/0BwCt69n0gpFbc0c0ek03TGd0aEk/view?usp=sharing

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.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.025, 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.025, 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.04, 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.01, 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.01, 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.02, 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.01, 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.01, 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.015]</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,    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,     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]</rosparam>

  </node>

  <!-- Parameters setting of the node: navsat_transform_node -->
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">
      <param name="magnetic_declination_radians" value="0.0036651914"/>
      <param name="yaw_offset" value="0.0"/>
      <param name="zero_altitude" value="false"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="publish_filtered_gps" value="true"/>
      <param name="use_odometry_yaw" value="false"/>
      <param name="wait_for_datum" value="false"/>

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

</launch>

Current problems: 1>. if I do not include the node [<node pkg="tf" type="static_transform_publisher" name="gps_tf" args="0 0 0 0 0 0 1 base_link gps_reddot 20"/>], all the outputs of /odometry/gps are zeros. If this tf node is included, the output of the /odometry/gps is as follows:

header: 
  seq: 5
  stamp: 
    secs: 1470557701
    nsecs: 931571006
  frame_id: odom
child_frame_id: ''
pose: 
  pose: 
    position: 
      x: 0.484485645778
      y: 0.158553831367
      z: -1.06257850911
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [1.6656841125133872, -0.020322677227241386, -0.5022024606421389, 0.0, 0.0, 0.0, -0.020322677227241386, 1.6207245363996199, 0.19335550081983582, 0.0, 0.0, 0.0, -0.5022024606421389, 0.19335550081983582, 6.390991351086994, 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, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [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, 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]

Only the position of Pose message are nonzero but the orientation and Twist message are zeros. Is there something wrong for my setting? If this code is not included, the output of /odometry/filered is close to zeros when I was walking in an outdoor environment because of the zero output of /odometry/gps for package navsat_transform_node. In addition, it seems from the link I followed that the previous author has not included this node.

2>. While including the tf node for gps, I have done an experiment. image link: https://drive.google.com/file/d/0BwCt69n0gpFbeVpnamlKYUxxTm8/view?usp=sharing The recorded data from /odometry/filtered are plotted by matching the google map, which are roughly accurate. However, it still has some location error. The frequency of IMU is 20HZ, and the frequency for GPS is 1HZ. Is this because of the low frequency of the GPS? The recording frequency is 2HZ. You can see that there are some jump at some of the segments.

Thanks very much for your help.


Originally posted by yrj on ROS Answers with karma: 3 on 2016-08-07

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

(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.


Originally posted by Tom Moore with karma: 13689 on 2016-08-30

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.