0
$\begingroup$

I have a specific question related to a robot localization package. I've been trying to get an answer on the Robot Stack Exchange for quite some time now, but I haven't received a response. I'm wondering if you could please take a moment to review my situation.

I'm using the Unscented Kalman Filter (UKF) in the package to fuse the absolute pose from SLAM with the linear velocity from wheel odometry. However, I've noticed that the position output of the Kalman filter appears to closely match the SLAM absolute pose, especially in terms of the position data provided by the wheel odometry and the covariance matrix values.

My question is whether this behavior is expected or if I might have made an error in my configuration. Additionally, I've observed that even when the SLAM data is cut off, the filter output continues to update based on data from the wheel odometry.

configuration file

frequency: 30

sensor_timeout: 0.1

two_d_mode: true

transform_time_offset: 0.0

transform_timeout: 0.0

print_diagnostics: true

debug: false

debug_out_file: /path/to/debug/file.txt

map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified

odom0: /odom_cov_ukf # wheel odometry
odom0_config: [false, false, false,
false, false,false,
true, true, false,
false, false, false,
false, false, true]
odom0_differential: false
odom0_relative: true
odom0_queue_size: 3
#odom1_pose_rejection_threshold: 0.01
#odom1_twist_rejection_threshold: 0.2
odom0_nodelay: false

odom1: /cam_aru_odom
odom1_config: [false,false, false,
false, false,false,
true, true, false,
false, false, false,
false, false, true]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 3
#odom1_pose_rejection_threshold: 0.01
#odom1_twist_rejection_threshold: 0.2
odom1_nodelay: false

pose0: /vo_ukf_cov #slam pose
pose0_config: [true, true, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]
pose0_differential: false
pose0_relative: fasle
pose0_queue_size: 1
#pose0_rejection_threshold: 2 # Note the difference in parameter name
pose0_nodelay: false

imu0: /imu_cov_data_ukf
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false,false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: fasle
imu0_queue_size: 5
#imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
#imu0_twist_rejection_threshold: 0.8 #
#imu0_linear_acceleration_rejection_threshold: 0.8 #
imu0_remove_gravitational_acceleration: false

use_control: true

stamped_control: false

control_timeout: 0.2

control_config: [true, false, false, false, false, true]

acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]

deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]

acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]

deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]

initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]

alpha: 0.001

kappa: 0

beta: 2

and here's the sensor message slam

slam:
header:
  seq: 72
  stamp:
    secs: 1698497751
    nsecs: 449593067
  frame_id: "camera"
pose:
  pose:
    position:
      x: 0.000905927071149
      y: 0.000373678404359
      z: 0.00112596100876
    orientation:
      x: 0.000162643538105
      y: 0.000183301626754
      z: 7.84188956183e-06
      w: 0.999999970198
  covariance: [1.8743429340134814e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.4518905529907774e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 8.392338064793856e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.542607486213591e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.4376012734276177e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0359938729265474e-09]

here's wheel odometry message

header:
  seq: 100
  stamp:
    secs: 1698497806
    nsecs: 247585945
  frame_id: "odom"
child_frame_id: "base_link"
pose:
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 6.08298869338e-05
      w: 1.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.00142620096449
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 6.862781704086771e-06]

here's the picture to show you what's confuse me this picture show the position in x axis in straight run collected from my robot the red graph is from slam, green is wheel odometry, blue is from the robot_localzation. The Kalman filter seem to always follow the slam position output is this normal behavior? thank you so much for looking into my question.enter image description here

$\endgroup$

1 Answer 1

0
$\begingroup$

Yes, that is what I'd expect to see.

Your pose measurements will generally have a stronger effect on your pose estimate than your velocity measurements. This is because the pose measurements from SLAM are used to directly update the pose in your EKF, whereas the velocity data gets integrated (along with their errors).

Let's focus on just the X dimension. Your SLAM pose data has a reported X variance of 1.8743429340134814e-08, which means the X measurement has a standard deviation of 1.3691e-04 (0.0001391 metries). I'm going to guess that's a bit of an under-estimate.

Secondly, your process_noise_covariance is set to 0.05 for X. So at every time step, the filter is going to add 0.05 to the X variance (0.2236 metre standard deviation) during the prediction step. Then your pose measurement arrives with a variance that is orders of magnitude smaller in the correction phase, so the filter pretty much just accepts the SLAM pose absolutely. Remember: an EKF is just doing optimal weighted averaging between its own state and the measurement, with the optimal weighting determiend by the relative values of the EKF's error and the measurement's error.

If you want the velocity to have a stronger effect on the overall output, you could:

  1. Give your wheel encoder data some actual covariance values. It's reporting all 0s, which makes the EKF make some up for you.
  2. Drastically lower the process_noise_covariance values for X, Y, Yaw, and their velocities.
  3. Increase the covariance values in your SLAM messages.

TL; DR, covariance values matter a lot in EKFs. Your sensor measurement covariances and the process_noise_covariance need to be tuned if you want to trust the wheel encoders more.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.