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 robot_localization node to do that. One thing we had to fix was the covariance matrix of the Hector pose_update - it looks like they populate it with alternative data ( https://github.com/tu-darmstadt-ros-p... ) 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 ...
Can you please remove all the comments from the configuration file?
Done, thanks :-)
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).