As a field robotics company, Clearpath Robotics loves using GPS systems!  However, ROS does not yet provide an effective method of incorporating GPS measurements into robots. A natural place to start incorporating GPS is in the navigation stack, specifically robot_pose_ekf. This package is primarily used to probabilistically combine data from various sensors that provide odometry data (wheel encoders, cameras, IMU) using an extended Kalman filter. 

Being absolute about position measurements

While, robot_pose_ekf is mostly used as an odometry filter and therefore incorporates differential position estimates, we thought this was a good place to start incorporating some absolute position measurements.  In order to achieve this, we forked the navigation repo and modified the main robot_pose_ekf node to incorporate absolute position measurements. This mod works by initializing the filter at the first GPS and IMU measurement it receives, as opposed to the first wheel/visual odometry measurement as before. After this, it applies the wheel encoder and visual odometry data differentially. However, any new GPS and/or IMU measurements are applied as absolute measurements of position. These absolute measurements can come from your favourite GPS or from an indoor positioning system such as Vicon or Optitrack. Note, that an indoor positioning system can also provide absolute IMU measurements. More information on using indoor positioning systems in ROS can be found here.

So what’s it look like?

Activating this feature in robot_pose_ekf is fairly simple. The launch file for robot_pose_ekf now has a ‘gps_used‘ parameter that can be used to turn on the use of absolute measurements in the filter. Moreover, to make sure IMU measurements are applied in an absolute sense the parameter ‘imu_absolute’ must also be set to true.   So, the general launch procedure that we use looks likes so:

<node pkg=”robot_pose_ekf” type=”robot_pose_ekf” name=”robot_pose_ekf” output=”screen”>
<param name=”output_frame” value=”odom”/>
<param name=”freq” value=”30.0″/>
<param name=”debug” value=”true”/>
<param name=”sensor_timeout” value=”1.0″/>
<param name=”odom_used” value=”true”/>
<param name=”imu_used” value=”true”/>
<param name=”vo_used” value=”false”/>
<param name=”gps_used” value=”true”/>
<param name=”imu_absolute” value=”true”/>
</node>

This modification, however, comes with a few requirements and caveats.

1. The GPS measurement should be provided on the /gps topic and should come in the form of an Odometry message. This includes an estimate of the covariance on the measurement. GPS is not a very good source for orientation information, so the covariance on the orientation and twist message should be exorbitantly high (I like 99999.0!) to make sure the filter ignores this data.

2. The GPS and IMU measurements should use the same frame. Ground rovers typically use the ENU frame (see reference figure below). GPS LLA messages can be converted to ECEF and then to a local ENU measurement using some standard methods.

3. Kalman filters are magical, but they are not magic. A usable output odometry from robot_pose_ekf will require that the GPS have a fairly good signal. I suggest mounting the GPS antenna as high as possible to get a clear view of the sky and picking a GPS that has access to both the L1 and L2 frequency bands. At Clearpath, we’ve already tested this modification with a few different GPS models.

EKF

Where can I get the code?

As we speak these modifications are in the process of being pushed to the main robot_pose_ekf repo and should be widely available. Meanwhile, you can get this code from the Clearpath robotics github.

There are some known issues with the package, especially when it comes to how it handles some covariances issues and general readability. It is unclear whether fixing these issues will require an overhaul of the package as is frequently suggested. Moreover, the proper integration of this version of robot_pose_ekf with other navigation packages like g-mapping will need a tweaking of how the TF’s are defined in the navigation stack as described in REP 105. We are hoping to tackle these issues with feedback from the community in the near future! As such, we’d love to hear from you and your experience with robot_pose_ekf and this modification.