Ask Your Question
1

ekf_localization_node behavior when GPS goes bad

asked 2018-12-28 22:26:38 -0500

Rick Armstrong gravatar image

updated 2018-12-29 12:20:08 -0500

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
edit retag flag offensive close merge delete

Comments

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!".

Rick Armstrong gravatar imageRick Armstrong ( 2018-12-29 14:24:57 -0500 )edit

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.

juanlu gravatar imagejuanlu ( 2019-01-03 07:34:27 -0500 )edit

Indeed, the covariances change (grow large) when the GPS signal is lousy.

Rick Armstrong gravatar imageRick Armstrong ( 2019-01-03 23:49:14 -0500 )edit

Yes, then I am afraid you have to implement an additional outlier rejection method to discard those values with high covariances.

juanlu gravatar imagejuanlu ( 2019-01-04 02:20:52 -0500 )edit

Thanks for your response; that's our next move, I think.

Rick Armstrong gravatar imageRick Armstrong ( 2019-01-04 14:21:28 -0500 )edit

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?

billy gravatar imagebilly ( 2019-01-04 15:31:56 -0500 )edit

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.

Rick Armstrong gravatar imageRick Armstrong ( 2019-01-04 15:37:34 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2019-01-04 14:45:41 -0500

Rick Armstrong gravatar image

updated 2019-01-04 14:45:55 -0500

A brief survey of the available literature indicates that EKFs are sensitive to outliers [1], so looks like this is expected behavior, and there's no way to _tune_ our way out of this problem. Garbage in == garbage out. On to outlier rejection!

[1] D. Simon, Optimal State Estimation, 2006

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2018-12-28 22:26:38 -0500

Seen: 148 times

Last updated: Jan 04