ekf_localization_node behavior when GPS goes bad
Shouldn't ekf_localization_node lean entirely on the internal model, when GPS measurements are extremely noisy?
Background: we have an outdoor robot equipped with RTK GPS, an IMU, and wheel odometry. We're using robot_localization in the dual-EKF configuration, as described in the docs and here. The robot is running Ubuntu 14.04, ROS Indigo, ros-indigo-robot-localization 2.3.3-0trusty-20171218-092847-0800. The whole thing performs well overall when the local GPS reception is good, but we're seeing a strange corner case when we park the robot in a shed. Our GPS fix goes down the tubes, as one would expect. The fix wanders all over the place, tens to hundreds of meters, and the GPS covariances explode. The problem is, our map->odom instance seems to continue to 'trust' the GPS measurement, and consequently the odom frame goes wandering all over the map. It's not a big problem, because we normally pull out of the shed and wait for good GPS before navigating, but this behavior runs counter to my understanding of how a Kalman filter works. We've tried setting dynamic_process_noise_covariance to true, and seems to help in simulation, but in the real-world, not-so-much.
My question: shouldn't the filter lean entirely on the internal model, and effectively ignore the crazy GPS? Any advice is greatly appreciated.
Here's our robot_localization config:
ekf_se_odom:
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# -------------------------------------
# Wheel odometry:
odom0: /husky_velocity_controller/odom
odom0_config: [false, false, false,
false, false, false,
true, false, false,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
# --------------------------------------
# IMU:
imu0: /imu/data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: false
use_control: false
ekf_se_map:
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
# -------------------------------------
# Wheel odometry:
odom0: /husky_velocity_controller/odom
odom0_config: [false, false, false,
false, false, false,
true, false, false,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
# -------------------------------------
# GPS odometry (from navsat_transform_node):
odom1: /odometry/gps
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: false
odom1_relative: false
# --------------------------------------
# IMU:
imu0: /imu/data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, false, false]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: false
use_control: false
dynamic_process_noise_covariance: true
I'm starting to think that it's just not reasonable to expect an EKF to be well-behaved in the presence of wacky inputs. I'm finding papers with statements like "...the Kalman filter (Kalman, ’60) is commonly used for real-time tracking, but it is not robust to outliers!".
Does the covariance of the GPS odometry change when that happens or is it a constant number (something like 0.1 for all coordinates)? If the covariance does not change over time then the Kalman filter has no way to know which values to trust.
Indeed, the covariances change (grow large) when the GPS signal is lousy.
Yes, then I am afraid you have to implement an additional outlier rejection method to discard those values with high covariances.
Thanks for your response; that's our next move, I think.
Rick, If you're willing, I'd really appreciate feedback on how you end up solving. You can see by my question here, I need to work though a similar issue.
https://answers.ros.org/question/3029...
I wonder if first bad GPS fix has high COV or later?
That's a good question, @billy. If there's a lag between bad fixes and big covariances, I suspect we'll have to low-pass filter or do RANSAC. I'll comment here if we eventually get it sorted.
If the GPS is giving you a the covariance along with the positioning, then could you just set a threshold yourself and throw out anything that is above a certain covariance?