How to integrate RTK GPS data into an ekf_localization_node (from robot_localization pkg)?

asked 2017-01-25 09:45:12 -0500

Tyrone Nowell

48 ●4 ●8 ●10

updated 2017-02-01 07:57:30 -0500

Hi all,

I'm relatively new to ROS so please bear with me.

I am trying to implement an extended kalman filter node from the robot_localization package.

I am localizing my robot using a Piksi RTK GPS unit with a fixed base station and an IMU. This setup gives me three useful data streams; /gps/fix (sensor_msgs/NavSatFix), /gps/rtkfix (nav_msgs/Odometry) and /imu/data(_raw until I pass it through a imu_filter_madgwick node) (sensor_msgs/Imu).

Note that /gps/fix includes GPS points (decimal degree format) which are both the single point precision(SPP) points as well as the RTK corrected points. I had to modify the gps driver so that the status information reflects the message type (SPP/RTK). Does the navsat_transform_node take into consideration whether the point is augmented or not? Does it need to?

/gps/rtkfix is relative to the base station (which is placed at a surveyed position).

From what I understand, a single SPP GPS and a single IMU can work but may not be very accurate. How would I go about including the RTK position information? Should I set the datum to the base station position or would that mess up the global coordinates?

I know I haven't included very much data or in-depth info in this post but there is quite a lot so if you need anything more specific, please let me know.

Thanks.

EDIT 1:

Below is a sample of the /gps/fix stream. The first message is RTK augmented and therefore has status == 1. The second is a SPP message and has status == 0.

---
header: 
  seq: 9355
  stamp: 
    secs: 1484232874
    nsecs: 552879627
  frame_id: gps
status: 
  status: 1
  service: 1
latitude: 59.6727308353
longitude: 10.7179627288
altitude: 97.3549933056
position_covariance: [0.0016, 0.0, 0.0, 0.0, 0.0016, 0.0, 0.0, 0.0, 0.0016]
position_covariance_type: 0
---
header: 
  seq: 9356
  stamp: 
    secs: 1484232874
    nsecs: 636842456
  frame_id: gps
status: 
  status: 0
  service: 1
latitude: 59.6727627988
longitude: 10.718035228
altitude: 122.657179707
position_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
position_covariance_type: 0

EDIT 2:

The covariance matrices have been updated to reflect the correct and relevant info (previous edit updated). I'm not sure if I have set up the frames correctly or how I can go about outputting the gps (UTM) locations of each point. My current launch file is shown below.

<launch>
  <node pkg="tf" type="static_transform_publisher" name="link0_broadcaster" args="0 0 0 0 0 0 1 odom base_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 1 map gps 100" />

<!-- Global (map) EKF instance -->
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_ontrack_global" clear_params="true">
    <param name="frequency"       value="30"/>
    <param name="sensor_timeout"  value="0.1"/>
    <param name="two_d_node"      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="map"/>

    <param name="transform_time_offset" value="0.0"/>

    <param name ...

(more)

answered 2017-01-26 03:03:34 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

updated 2017-02-01 07:17:12 -0500

Note that /gps/fix includes GPS points (decimal degree format) which are both the single point precision(SPP) points as well as the RTK corrected points. I had to modify the gps driver so that the status information reflects the message type (SPP/RTK). Does the navsat_transform_node take into consideration whether the point is augmented or not? Does it need to?

So /gps/fix is one topic that is outputting a sensor_msgs/NavSatFix message, correct? I'm not quite following what you mean here; can you post a sample message? What did you modify in your driver, and how is that reflected in the message?

From what I understand, a single SPP GPS and a single IMU can work but may not be very accurate. How would I go about including the RTK position information? Should I set the datum to the base station position or would that mess up the global coordinates?

If the RTK data is outputting a world-frame pose as a nav_msgs/Odometry message, I would advise that you not bother with the /gps/fix data or navsat_transform_node. If you want to include the IMU data, I would just fuse the /gps/rtkfix odometry and IMU data as inputs to a single instance of the filter.

EDIT in response to updates:

A couple more things:

Can you post sample messages for the IMU as well? I don't see a static transform defined to any IMU frame, and I'm guessing that's the issue.
Your link0_broadcaster static transform broadcaster make sense, as you can replace the first-tier EKF that way (note that you could also just run the EKF and everything else in the odom frame, but this works too). However, I am concerned about the second static transform, link1_broadcaster. You have defined it from map->gps, but your GPS isn't located at (0, 0) in the map frame. It's affixed to your robot. The transform should go from base_link-gps.

edit flag offensive delete link

Comments

Hi Tom,

I've included a sample from the /gps/fix stream in the original question.

/gps/rtkfix is the x,y position of the roving receiver relative to the base station position. The problem is that the rtk fix is almost guaranteed to drop out so I need to it work with standard GPS and imu as well.

Tyrone Nowell ( 2017-01-26 03:23:06 -0500 )edit

Thanks. navsat_transform_node does not look at the NavSatStatus other than to make sure the sensor has a fix at all. The only difference in your case, unless I am mistaken, is that the covariance should change, but that should be reflected in the position_covariance field.

Tom Moore ( 2017-01-26 03:40:17 -0500 )edit

Ok, thanks. This driver doesn't seem to bother with covariance so I guess I'll have to handle that. Any tips on how best to calculate the covariance matrix?

Tyrone Nowell ( 2017-01-26 04:04:45 -0500 )edit

Your GPS manufacturer should advertise error rates for the device. Just formulate those as variance and put them in the message.

Tom Moore ( 2017-01-26 04:38:21 -0500 )edit

Hi Tom. I think I've got the covarinace matrices working correctly but the orientation doesnt appear to be working correctly. Should the base_link frame rotate? I've added my current launch file (EDIT 2) to the original question. The orientation always drifts back to 0 deg heading.

Tyrone Nowell ( 2017-01-31 04:02:13 -0500 )edit

Hi Tom. An odom-imu transform is broadcast by an imu_filter_madgwick node which filters the imu data. Perhaps being odom-imu instead of base_link-imu could be the problem? The gps frame is the base station. Its only to output the rtk odometry in rviz so that i can compare it to the ekf result.

Tyrone Nowell ( 2017-02-01 08:04:00 -0500 )edit

Yes, the IMU's frame should be relative to base_link.

Tom Moore ( 2017-05-04 02:08:45 -0500 )edit

Hi EyeTy and Tom, I have the exact same issue with heading drifting back to 0 heading. Did the transform change of imu_filter_madwick node from 'odom' to 'base_link resolve your problem? (the ~fixed_frame param)

MegaBanana ( 2017-05-19 07:58:18 -0500 )edit

add a comment

How to integrate RTK GPS data into an ekf_localization_node (from robot_localization pkg)?

1 Answer

Comments

Question Tools

Stats

Related questions

How to integrate RTK GPS data into an ekf_localization_node (from robot_localization pkg)? edit

1 Answer

Comments

Question Tools

Stats

Related questions

How to integrate RTK GPS data into an ekf_localization_node (from robot_localization pkg)?