0
$\begingroup$

Rosanswers logo

I'm testing sensor fusion with robot_localization package.

I've got a small wheeled robot with an xSense IMU and a quasi-UWB local positioning system.

The local positioning consists of two mounted ultrasonic beacons, each is separately localized against several static beacons spread around the room. The system is rather precise: manufacturer reports 2 cm standard deviation, and it's believable. But the update rate is low (around 1 Hz), and the beacons report only position, no orientation.

As the robot moves on the flat floor, I use relative position of two beacons to calculate robot position and 2D yaw (dumb middle point for position, dumb relative vector for orientation). The result is accurate, except for the position jumps between updates, and remarkable yaw jitter.

I try to use IMU data and robot_localization to provide smooth pose estimation between updates. As for now, my setup is as follows:

  • from the beacons' data I construct a PoseWithCovarianceStamped message. I do not have reported covariances, so I construct covariance matrix simply setting diagonal elements for X,Y,Z, and Z rotation as squared standard deviation (e.g. if x deviation of beacons is 0.02 m, I set covariance[0] to 0.0004). Frame_id is set to ‘map’ in the message.

  • The IMU sends out standard sensor_msgs/imu message, the only issue is that I have to remove the first '/' from its frame_id, as the ekf node sends out warnings. I've got a separate correction node for this. Also a static transform from base_link to IMU data frame_id is published.

  • ekf node 1 with world_frame set to map, inputs are IMU data and pose calculated from beacons

  • ekf node 2 with world_frame set to odom, IMU as the only input

  • constructed pose and output of odometry/filtered are visualized by rviz

Everything is up and running, but the result is far from desired. Filtered odometry follows the constructed pose closely: after each position jump (=update) the filtered pose drifts slowly towards new position for half a second, then jumps to new position altogether.

Questions:

  1. Could anyone hint me about what parameters I should try to tweak to achieve smooth continuous movement of filtered pose?

  2. Is there a ROS functionality to calculate robot pose from position of two mounted beacons? Say, I write static tf transforms from base_link to the two beacons' hardpoints, and ROS calculates the robot position/orientation for me, possibly with some cool Kalman magic? Then, of course, feeds it into the EKF node.

Below are the YAML parameter files for my EKF nodes. Note that I use 2d mode for both, but turning it off doesn't change the situation much.

IMU-only node:

frequency: 100

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
publish_tf: true

publish_acceleration: false

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

imu0: imu/data_c
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
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: false
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.1, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.1, 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-8, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1e-8, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,             
                     0,    0,    1e-8, 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]

IMU+pose node:

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

publish_tf: true

publish_acceleration: false

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: map           # Defaults to the value of odom_frame if unspecified

pose0: /pose_synth
pose0_config: [true,  true,  true,
               false, false, true,
               false, false, false,
               false, false, false,
               false, false, false]
pose0_differential: false
pose0_relative: false
pose0_queue_size: 5
pose0_rejection_threshold: 2  # Note the difference in parameter name
pose0_nodelay: false

imu0: imu/data_c

imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
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: false
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.1, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.1, 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-8, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1e-8, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    1e-8, 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]

Originally posted by teonik on ROS Answers with karma: 48 on 2017-09-26

Post score: 2


Original comments

Comment by georgie96 on 2021-04-02:
This looks like a great project, but I am confused about what message type your beacon node gives? Is it a PoseWithCovarianceStampedmessage type? Also, is the initial estimate covariance for the imu or the uwb beacon position?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

A few notes:

  • You are fusing yaw from your beacon poses and from the IMU. Do they have the same world frame? In other words, if you point your robot at some direction (say, magnetic north) and drive straight, do the IMU and your beacon have the same heading estimate? It seems that, if your IMU has a compass, it's going to read 0 at magnetic north. Unless you have your beacon system set up such that forward motion towards magnetic north produces a manufactured heading of 0, you're going to see your headings rapidly jump between both estimates.
  • I'm not sure that you need a dual EKF setup, because...
  • Integration of IMU data alone, or even IMU with some pose reference, will not get what you want. IMUs have accelerometers with biases in them, and those accelerations will be integrated into velocities, which are going to cause your robot's pose estimate to move rapidly away, and then once in a while get "jerked" back towards the pose estimate (whenever it's received). I have yet to add acceleration bias correction for the IMU data to the filter, but even if I did, I think you'd want a velocity reference.

Your setup is pretty much analagous to setups with GPS + IMU alone, and it's not a use case that is especially well-supported by r_l. If I added IMU bias correction, it might help, but time is an increasingly rare commodity. Another option is that you can try differentiating your poses and sending that velocity to the EKF as well. You're giving it the same info twice, but it might make things a bit smoother overall.


Originally posted by Tom Moore with karma: 13689 on 2017-11-15

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

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