Ask Your Question
2

Discrete jumps in odom frame for robot_localization with GPS

asked 2014-08-07 11:14:03 -0600

updated 2014-08-07 14:13:04 -0600

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 ...
(more)
edit retag flag offensive close merge delete

Comments

can you edit your question and post your launch file as well as say what data you are using from what sensors?

l0g1x gravatar imagel0g1x ( 2014-08-07 12:14:22 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
4

answered 2014-08-07 14:45:39 -0600

Tom Moore gravatar image

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.

edit flag offensive delete link more

Comments

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!

Gary Servin gravatar imageGary Servin ( 2014-08-07 15:29:56 -0600 )edit

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?

Tom Moore gravatar imageTom Moore ( 2014-08-07 15:48:09 -0600 )edit

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.

Tom Moore gravatar imageTom Moore ( 2014-08-07 15:49:36 -0600 )edit
3

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.

tfoote gravatar imagetfoote ( 2014-08-07 16:08:34 -0600 )edit

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

Gary Servin gravatar imageGary Servin ( 2014-08-11 08:33:25 -0600 )edit

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.

Tom Moore gravatar imageTom Moore ( 2014-08-20 14:04:16 -0600 )edit
1

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.

Gary Servin gravatar imageGary Servin ( 2014-08-20 14:33:03 -0600 )edit

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

Huibuh gravatar imageHuibuh ( 2014-09-08 17:01:31 -0600 )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: 2014-08-07 11:14:03 -0600

Seen: 1,054 times

Last updated: Aug 07 '14