Discrete jumps in odom frame for robot_localization with GPS

Question

Hi,

I'm currently using the excellent robot_localization package in order to combine data from an IMU, velocities and GPS.

Following the tutorials on the robot_localization wiki page I'm able to get the correct odometry and tf, but each time a new GPS fix comes in (~1Hz for our GPS) the /base_link frame does a discrete jump on the /odom frame. According to REP105, this should be continuous.

Any ideas on how to make the output continuous or at least smooth it?

Thanks!

EDIT: Added launchfile.

Launchfile:

<node pkg="gps_common" type="utm_odometry_node" name="utm_odometry_node">
   <param name="frame_id" value="utm"/>
</node>

<node pkg="robot_localization" type="utm_transform_node" name="utm_transform_node" respawn="true" output="screen">
  <param name="magnetic_declination_radians" value="0.234223186"/>
  <param name="roll_offset" value="0"/>
  <param name="pitch_offset" value="0"/>
  <param name="yaw_offset" value="0"/>
  <param name="zero_altitude" value="false"/>
  <remap from="/odometry/filtered" to="odom" />
  <remap from="/gps/fix" to="/fix" />
</node>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="imu0" value="/imu/data"/>
  <param name="odom0" value="/gps/utm"/>
  <param name="twist0" value="/twist_robot" />

  <!-- IMU data -->
  <rosparam param="imu0_config">[false, false, false,
                                 true, true, true,
                                 false, false, false,
                                 true, true, true]</rosparam>

  <!-- GPS data -->
  <rosparam param="odom0_config">[true, true, true,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <!-- Twist from velocities -->
  <rosparam param="twist0_config">[false, false, false,
                                   false, false, false,
                                   true, true, false,
                                   false, false, false]</rosparam>

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

  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

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

  <rosparam param="process_noise_covariance">[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.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.4, 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.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                      0.0, 0.0, 0.0, 0.0, 0.00, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                      0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0,
                                      0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0,
                                      0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0,
                                      0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0,
                                      0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0,
                                      0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004]</rosparam>

</node>

---

Originally posted by Gary Servin on ROS Answers with karma: 962 on 2014-08-07

Post score: 2

---

Original comments

Comment by l0g1x on 2014-08-07:
can you edit your question and post your launch file as well as say what data you are using from what sensors?

Answer 1

Hi Gary,

How much of a jump are we talking about? Is it jumping many meters away, or within the same general area? The REP does state that the odom frame should be continuous, but then fusing a GPS with data in that frame may not be the best approach.

What you could do (and something I've done previously) is to have your map frame be the UTM grid. If you're trying to plan paths from one location to another, you can determine the difference in UTM coordinates between the goal and current position, transform that difference into the odom frame, and then execute control commands with respect to the odom frame (which would be continuous if you don't fuse the GPS with the estimate). When you reach your goal in the odom frame, you can re-evaluate the distance in UTM coordinates (i.e., the map frame) and repeat the process until you've reached some distance to the goal.

Alternatively (and I've not tried this), you can run ekf_localization_node twice: for the map frame, you can fuse odometry, GPS, and IMU, and then for the odom frame, you can just fuse odometry and IMU data as usual. You can then define a static transform (of 0, I believe) between the map and odom frames. There may be some tf reparenting issues that will crop up if you go this route; I haven't thought through it enough to consider the ramifications. :)

I'll tell you what I tell everyone: feel free to shoot me a bag file and when we sort out your issue, we can update this question.

---

Originally posted by Tom Moore with karma: 13689 on 2014-08-07

This answer was ACCEPTED on the original site

Post score: 4

---

Original comments

Comment by Gary Servin on 2014-08-07:
The jumps are ~1m. I've already tried the second suggestion but didn't manage correctly the frame transformations. I'll try both suggestions and update here.

Thanks a lot!

Comment by Tom Moore on 2014-08-07:
Right, those jumps are well within the error radii for most GPS units. Still, I'm mildly surprised that they have that strong of an effect on your estimate. What are the covariances of the state estimate and GPS measurements?

Comment by Tom Moore on 2014-08-07:
Also, the utm_transform_node should be publishing a transform that will convert from UTM coordinates to your odom frame. Try using one of the tf transform utility methods to handle the conversion.

Comment by tfoote on 2014-08-07:
The design of REP 105 was to not have absolute update coming into the odom frame. That should be captured by the map -> odom transform as a correction to the baselink -> odom transform to correct for drift. Otherwise you cannot store things like observations of obstacles in your initial frame without worrying about GPS updates and correcting for past jumps.

Comment by Gary Servin on 2014-08-11:
Tom, sorry for the delay. We've set our twist covariance to 0.25 for both x and y and to 1.0 for our gps data

Comment by Tom Moore on 2014-08-20:
Re: @tfoote's statement, I should note that while robot_localization can easily fuse GPS data, it was originally designed to address the fusion of continuous data, hence its assumption of odom->base_link. I'll be adding new parameters to handle GPS data and still comply with REP-105.

Comment by Gary Servin on 2014-08-20:
I finally went with using to separate ekf nodes, the first fuses twist and IMU using odom->base_link, the second fuses GPS, IMU and twist using map->gps_base_link (needed some topic re-stamping here to match the gps_base_link output) and with that I calculated the tf map->odom as done in gmapping.

Comment by Huibuh on 2014-09-08:
Re:@'Gary Servin': Can you comment on how your setup using two instances of ekf nodes performed? Are GPS update integrations smooth enough to allow navigation in the odom frame? Cheers