Implement EKF localization in map frame

[5yler]

We have [amcl]() localization up and running, as well as the gps_common package's utm_odometry_node which generates global odometry in the UTM frame based on GPS readings (see gps.launch).

EKF could fuse the GPS-based odometry with our encoder odometry. See Adding A GPS Sensor EKF tutorial.

[cjdesno]

Relevant: http://wiki.ros.org/robot_localization/Tutorials/GPS%20Integration

[cjdesno]

Also relevant: http://answers.ros.org/question/218137/using-robot_localization-with-amcl/

Note that we are using robot_localization's ekf node, not robot_pose_ekf, so the tutorial linked from the original issue post is probably not useful for implementing this.

[cjdesno]

Relevant? http://math.stackexchange.com/questions/1066479/how-to-estimate-variances-for-kalman-filter-from-real-sensor-measurements-withou

[cjdesno]

Note: may require intercepting IMU messages to set variances to nonzero values, though I am hoping this is not actually necessary or relevant.

[5yler]

This is happening.

[5yler]

Even on a terrible map, this seems to perform pretty well, but did not notice a difference in accuracy when adding in GPS velocity data.

[5yler]

Without GPS velocity data:

With GPS velocity data:

[5yler]

I think that in order to use ekf_localization_node in the odom frame, we would have to disable the odom->base_link transform broadcast by odom_tf_publisher, which currently both:

• publishes to the odom topic
• broadcasts the transform odom->base_link See Using robot_localization with amcl (ROS Answers)

I had some success using amcl on raw odom data by tuning the odom_alpha1 through odom_alpha4 parameters but EKF still needs work. Got loop closure for double loop around map:

The odometry/ekf_map is in black, while odom is yellow. The amcl_pose topic tracked the robot location consistently throughout the bag file.

[5yler]

ekf in map frame with all differential/relative parameters set to false:

<param name="odom0_differential" value="false"/> 
<param name="pose0_differential" value="false"/>-->
<param name="imu0_differential" value="false"/> <!-- if you have only one source of
           absolute orientation data, you should not set the differential parameter to true. -->

<param name="odom0_relative" value="false"/>
<param name="pose0_relative" value="false"/> 
<param name="imu0_relative" value="false"/> 

[5yler]

Relative and Differential Parameters

From robot_localization Standard Parameters (ROS Wiki):

~odomN_differential, ~imuN_differential, ~poseN_differential

For each of the sensor messages defined above that contain pose information, users can specify whether the pose variables should be integrated differentially.

If a given value is set to true, then for a measurement at time t from the sensor in question, we first subtract the measurement at time t-1, and convert the resulting value to a velocity.

This setting is especially useful if your robot has two sources of absolute pose information, e.g., yaw measurements from odometry and an IMU. In that case, if the variances on the input sources are not configured correctly, these measurements may get out of sync with one another and cause oscillations in the filter, but by integrating one or both of them differentially, we avoid this scenario. Users should take care when using this parameter for orientation data, as the conversion to velocity means that the covariance for orientation state variables will grow without bound (unless another source of absolute orientation data is being fused). If you simply want all of your pose variables to start at 0, then please use the _relative parameter. Please note that if you are fusing GPS information via navsat_transform_node or utm_transform_node, you should make sure that the _differential setting is false.

~odomN_relative, ~imuN_relative, ~poseN_relative

If this parameter is set to true, then any measurements from this sensor will be fused relative to the first measurement received from that sensor. This is useful if, for example, you want your state estimate to always start at (0, 0, 0) and with roll, pitch, and yaw values of (0, 0, 0).

It is similar to the _differential parameter, but instead of removing the measurement at time t-1, we always remove the measurement at time 0, and the measurement is not converted to a velocity.

[5yler]

Set the following parameters for ekf_map.launch:

<arg name="odom0_differential" value="true" />
<arg name="pose0_differential" value="false" />
<arg name="imu0_differential" value="true" />

<arg name="odom0_relative" value="false" />
<arg name="pose0_relative" value="true" />
<arg name="imu0_relative" value="true" />

Overall the robot tracked position well for two loops, though the localization drifted during the second top right corner turn (but recovered accurate position afterward).

[5yler]

Double loop closure!

    <arg name="odom0_differential" value="true" />
    <arg name="pose0_differential" value="false" />
    <arg name="imu0_differential" value="true" />

    <arg name="odom0_relative" value="false" />
    <arg name="pose0_relative" value="true" />
    <arg name="imu0_relative" value="false" />

[5yler]

Racetrack EKF Odometry in Map Frame

EKF localization in map frame worked live on Gigatron's first RC drive around the test track at AVC, on a pre-made map. Includes GPS and output of navsat_transform_node as outlined in issue #10.

Localization Drift

The localization drifted at the final turns before the straightaway, but eventually recovered and lined up with reality back at the starting point.