Discrete jumps in odom frame for robot_localization with GPS

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

962 ●3 ●12 ●25 http://garyservin.word...

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)

Comments

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

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

add a comment

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

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

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

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 ( 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 ( 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 ( 2014-08-07 15:49:36 -0600 )edit

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 ( 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 ( 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 ( 2014-08-20 14:04:16 -0600 )edit

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 ( 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 ( 2014-09-08 17:01:31 -0600 )edit

add a comment

Discrete jumps in odom frame for robot_localization with GPS

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Discrete jumps in odom frame for robot_localization with GPS edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Discrete jumps in odom frame for robot_localization with GPS