filtered odometry overestimation
So I'm using robot_localization package for localization, with wheel encoders+IMU (Phiget / Realsense IMU) for fusion. The filtered odometry I'm observing is being overestimated by around 15% of that of the wheel odometry. If I drive 10 m straight My filtered odometry tells me I traveled 13.5 m straight.
I tried using both the ekf/ukf localization nodes from the ros package. Here are my parameters:
enter frequency: 20
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
map_frame: world # 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: /burro_base/odom
odom0_config: [false, false, false,
false, false, false,
true, true, true,
true, true, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 10
odom0_twist_rejection_threshold: 10
imu0: /imu/data_raw
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_pose_rejection_threshold: 250
imu0_twist_rejection_threshold: 250
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
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.1, 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]
I think this is happening due to the differential parameter being set to true for odom0 which is estimation velocity by calculating the time difference (correct me if I'm wrong). But if I set that param to false the entire filtered odometry is messed up. Could anyone suggest any possible approaches to resolve this issue.
Here is the image of baseodomraw(yellow) and filtered/odometry (red):
Asked by ab17 on 2019-09-18 12:34:43 UTC
Answers
Alright I see a few things here that might help.
1) You're fusing your robot's odometry position information differentially. Thinking about that for a moment, what does that mean? You're taking some ticks from a wheel encoder, integrating them together, then differentiating them? That's probably not what you want. Try using in the velocity information from your odometry, not the position information, and turn off differential.
2) You've set these _threshold parameters, you sure those are good?
3) Your IMU is being fused with the yaw velocities. That's a reasonable thing to do if you IMU doesnt give you out a fused orientation. I'd stick with that if its given
4) You didn't show us your process / sensor covariances, that's relevant here.
Keep in mind this is filtering, there's going to be some error no matter what you do, this is what global updates are for. Good sensors aren't necessarily always replacable by highly tuned filtering depending on your requirements (which are...?)., but I would expect you to get better than 15% in that short of a period of travel.
I see you also filed a ticket on R_L, please do close that so we're not having cross conversations.
Asked by stevemacenski on 2019-09-18 16:58:24 UTC
Comments
Thanks for getting back. I have updated my config file according to your advice. I haven't noticed any difference in performance from before. Any other possible things I might look into ?
My robots are working in very constrained spaces thus overestimation is a notable problem for me.
Asked by ab17 on 2019-09-18 17:58:22 UTC
The threshold parameters made no difference as well.
Asked by ab17 on 2019-09-18 18:04:54 UTC
Comments
Please attach your screenshot directly to the question. Don't use an external hosting service.
I've given you sufficient karma.
Asked by gvdhoorn on 2019-09-18 12:39:18 UTC