0
$\begingroup$

Rosanswers logo

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="odom0" value="/gps/rtkfix"/>
    <param name="imu0"  value="/imu/data"/>

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

    <!-- IMU -->
    <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="odom0_relative"                          value="false"/>
    <param name="imu0_relative"                           value="false"/>

    <param name="imu0_remove_gravitational_acceleration"  value="true"/>
    <param name="print_diagnostics"                       value="true"/>

    <param name="odom0_queue_size"                        value="10"/>
    <param name="imu0_queue_size"                         value="10"/>

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

    <remap from="/odometry/filtered"  to="/odometry/global"/>

  </node>
<!-- Global (map) EKF instance -->

<!-- Navsat node transform -->
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">
    <param name="frequency"                     value="30"/>
    <param name="delay"                         value="3"/>
    <param name="magnetic_declination_radians"  value="0.054"/>
    <param name="yaw_offset"                    value="1.5707963"/>
    <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"/>

    <!-- <rosparam param="datum">[59,66727, 10.77337, 0.0, map, odom]</rosparam> -->

    <remap from="/imu/data"           to="/imu/data"/>
    <remap from="/gps/fix"            to="/gps/fix"/>
    <remap from="/odometry/filtered"  to="/odometry/global"/>

  </node>
<!-- Navsat node transform -->
</launch>

EDIT 3:

Here is a sample message from /imu/data

---
header: 
  seq: 603
  stamp: 
    secs: 1484232821
    nsecs: 269243016
  frame_id: imu
orientation: 
  x: 0.0909325752091
  y: -0.993837356153
  z: -0.00888735559103
  w: -0.0237232971259
orientation_covariance: [1e-05, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 1e-05]
angular_velocity: 
  x: 0.00298800367941
  y: 0.0135341556846
  z: -0.000234223185618
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: -0.540727223129
  y: 0.0771066032982
  z: -9.56023780893
linear_acceleration_covariance: [8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06]
---

Originally posted by Tyrone Nowell on ROS Answers with karma: 48 on 2017-01-25

Post score: 3

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

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:

  1. 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.
  2. 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.

Originally posted by Tom Moore with karma: 13689 on 2017-01-26

This answer was ACCEPTED on the original site

Post score: 3

Original comments

Comment by Tyrone Nowell on 2017-01-26:
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.

Comment by Tom Moore on 2017-01-26:
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.

Comment by Tyrone Nowell on 2017-01-26:
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?

Comment by Tom Moore on 2017-01-26:
Your GPS manufacturer should advertise error rates for the device. Just formulate those as variance and put them in the message.

Comment by Tyrone Nowell on 2017-01-31:
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.

Comment by Tyrone Nowell on 2017-02-01:
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.

Comment by Tom Moore on 2017-05-04:
Yes, the IMU's frame should be relative to base_link.

Comment by MegaBanana on 2017-05-19:
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)

Improve this answer
$\endgroup$

Your Answer

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