robot_pose_ekf and GPS

edit

Regarding a generic pose estimation solution, I also would like to point you to our hector_localization stack. Team Hector Darmstadt uses it since more than a year to estimate the full 3D pose of our rescue robot Hector, within the real-time loop of the Hector quadrotor and even for estimating the position, velocity and attitude of a small airplane as part of our flight mechanics lab.

The design goals have been similar to what Chad Rockey described in his answer. The core package currently provides a system model for generic 6DOF rigid body pose estimation based on pure IMU inputs (rates and accelerations), which can be specialized depending on the robot and for additional input values. As measurement models we currently provide direct and barometric height measurements, GPS postion and velocity, magnetic field sensors as heading reference and a generic pose and twist measurement to fuse nav_msgs/Odometry messages from arbitrary sources (e.g. wheel odometry or SLAM).

The stack consists of a core library without dependencies to ROS beside message types and additional packages providing a node, nodelet or Orocos Real-Time Toolkit TaskContext interface. The system and measurement models could also be implemented and loaded as plugins with some minor modifications. In the background an Extended Kalman Filter based on the Bayesian Filter Library is responsible for fusing all information into a consistent state estimate and additionally keeps track which state variables are observable and which are not.

The major drawback is that hector_localization currently lacks documentation. I hope to find the time to create some wiki pages soon. Currently I am working on a redesign to remove the dependency from BFL and use Eigen directly and give models even more flexibility, e.g. adding augmented variables to the state vector.

However, we never used robot_pose_ekf and I cannot tell if hector_localization can serve as a replacement. In general, I think it is a good idea to come up with a new solution to estimate the robot pose using all available information which is adaptable to as many robots as possbile. I would like to contribute to a community solution after having finished my thesis.

First to answer the question:

Initialization

robot_pose_ekf initializes to the odometry just so that it is comparable with the wheel odometry. It's arbitrary and could be fixed.

Corrections

robot_pose_ekf only uses the EKF logic to determine dx, dy, and dheading (equivalent to calculating the Twist) for the N sensors. It then integrates these outside of the EKF - producing an odometry - not a localization estimate. This is common when you do not have absolute references and your variance estimates for x, y, and yaw will tend toward infinity.

When using GPS in robot_pose_ekf, it is difficult to not lose the global reference. If you chose to use the global reference, then you tend to wash out your other corrections in GPS noise and your initial "corrections" will tend to be very large.

The future: I'm working on an odometry and localization filter to be released in Hydro. https://github.com/chadrockey/graft

It's built on Eigen and is probably going to be a UKF implementation, thus the generic name, although in version + N, I may support EKF implementations as well.

It's going to start out small and replace robot_pose_ekf level of functionality. The eventual difference is that it will also support global localization sensors. The best way to add GPS to these measurements is through a chain like the following:

drifty GPS frame (like 'map' from amcl) -> fused odometry -> robot

Then the GPS version just sends corrections from the origin of the fused odometry to the GPS origin/UTM origin. This will likely rely on geographic_info for the GPS to meter conversion, so I don't expect it the integration to be 100% in Hydro.

I'm not taking feature requests at this time, as my priority is to get something out for Hydro so that we can all stop struggling with using robot_pose_ekf outside of what it was designed for. It will support nav_msgs::Odometry and sensor_msgs::Imu in various configurations for the locally continuous version, so between this and my other projects, I'd prefer to properly unit test and rostest as opposed to supporting modifications just quite yet.