robot_localization pose estimate is jerky (fusing wheel odometry and SLAM coordinates)
For our robot we have an odometry calculated from wheel encoders (it is not a differential robot, but we have a model that describes its motion). We also use Hector SLAM for converting Lidar data into map coordinates. The problem with Hector coordinates is that they are jumping around a little bit so we naturally considered fusing calculated odometry and Hector coordinates to get smooth position.
We tried to configure robotlocalization node to do that. One thing we had to fix was the covariance matrix of the Hector poseupdate - it looks like they populate it with alternative data (https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/35) so we just removed it and tried putting some diagonal covariance matrix for testing. Covariance on odometry is left at 0 which according to robot_localization docs inserts a small value at diagonals.
What we effectively observe is that motion of base_link frame within map frame is still very jerky. I wonder if the general setup we chose is adequate. My question is whether we chose a correct approach to obtain smooth coordinates with Lidar correction and if so what should be the modifications to configuration that would allow us to get there?
Any advice would be greatly appreciated!
Here is the robot_localization configuration:
frequency: 30
sensor_timeout: 0.0
two_d_mode: true
reset_on_time_jump: true
transform_time_offset: 0.1
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: map # Defaults to the value of odom_frame if unspecified
# smooth_lagged_data: true
# history_length: 1.0
odom0: /odom_for_ukf
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
pose0: /pose_for_ukf
pose0_config: [true, true, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]
pose0_differential: false
pose0_relative: false
pose0_nodelay: false
process_noise_covariance: [1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-6, 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-3, 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-4, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-2, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-2, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-2]
initial_estimate_covariance: [1e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-4, 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-4, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-6, 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-4, 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
Here is an example of an odometry message:
header:
seq: 1647
stamp:
secs: 1548941091
nsecs: 911072015
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: -0.658128444912
y: 0.163952489637
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.0847220387025
w: 0.996404624717
covariance: [1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist:
twist:
linear:
x: 0.10000000149
y: 3.46944695195e-18
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.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]
Here is an example of a pose update message:
header:
seq: 565
stamp:
secs: 1548941091
nsecs: 319238833
frame_id: "map"
pose:
pose:
position:
x: 0.896168708801
y: 0.0289258956909
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.142605677247
w: 0.98977959156
covariance: [0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
(I can't attach rosbag and config because I don't have enough points)
Asked by merlin_ua on 2019-02-01 04:36:53 UTC
Answers
Thank you for including sample sensor data. Your config looks OK to me. Can you visualize the poses that are being generated by the Hector node? If the EKF generally tracks those poses, then you can try inflating the Hector covariances. Your process_noise_covariance is already pretty small, and the integrated error from the twist data ought to also be pretty tiny, so I would expect the filter to trust your odometry data.
Also, what happens if you just don't include the Hector data, and fuse only wheel encoder data? Does it stay still?
Asked by Tom Moore on 2019-02-13 08:02:50 UTC
Comments
Can you please remove all the comments from the configuration file?
Asked by gvdhoorn on 2019-02-01 04:51:06 UTC
Done, thanks :-)
Asked by merlin_ua on 2019-02-01 04:53:57 UTC
If you want to do the sensor fusion with odometry and laser source. I recommend you to use gmapping package(It's use particle filter to fuse two source).
Asked by HankYuan on 2019-02-12 04:43:18 UTC