From 3d51e3fd9c92f3c8e93afce5d43af341a421b317 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 3 Nov 2023 11:43:38 +0100 Subject: [PATCH] Upgrade EKF --- rosbot_xl_bringup/config/ekf.yaml | 112 +++++++----------- .../config/diff_drive_controller.yaml | 5 +- .../config/mecanum_drive_controller.yaml | 6 +- 3 files changed, 55 insertions(+), 68 deletions(-) diff --git a/rosbot_xl_bringup/config/ekf.yaml b/rosbot_xl_bringup/config/ekf.yaml index 88da4f9b..a3402e2d 100644 --- a/rosbot_xl_bringup/config/ekf.yaml +++ b/rosbot_xl_bringup/config/ekf.yaml @@ -1,83 +1,63 @@ -### ekf config file ### +# Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html + ekf_filter_node: ros__parameters: - predict_to_current_time: false + frequency: 25.0 - sensor_timeout: 0.04 + sensor_timeout: 0.1 two_d_mode: true - print_diagnostics: true - publish_tf: true + + transform_time_offset: 0.0 + transform_timeout: 0.1 map_frame: map odom_frame: odom base_link_frame: base_link world_frame: odom + publish_tf: true + publish_acceleration: false - odom0: /rosbot_xl_base_controller/odom - # in diff drive dY will be always 0, but it should be taken into account, as covariances won't be calculated correctly otherwise (they blow up in y direction) - odom0_config: [false, false, false, # X , Y , Z - false, false, false, # roll , pitch ,yaw - true, true, false, # dX , dY , dZ - false, false, false, # droll , dpitch ,dyaw - false, false, false] # ddX , ddY , ddZ - - odom0_queue_size: 10 - odom0_nodelay: true + odom0: /rosbot_base_controller/odom + odom0_config: [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + odom0_queue_size: 2 + odom0_nodelay: false odom0_differential: false - odom0_relative: false + odom0_relative: true imu0: /imu_broadcaster/imu - imu0_config: [false, false, false, # X , Y , Z - false, false, true, # roll , pitch ,yaw - false, false, false, # dX , dY , dZ - false, false, false, # droll , dpitch ,dyaw - false, false, false] # ddX , ddY , ddZ - imu0_queue_size: 10 - imu0_nodelay: true + imu0_config: [false, false, false, + false, false, false, + false, false, false, + false, false, true, + true, true, false] + imu0_queue_size: 5 + imu0_nodelay: false imu0_differential: false imu0_relative: true imu0_remove_gravitational_acceleration: true -# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is -# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each -# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. -# However, if users find that a given variable is slow to converge, one approach is to increase the -# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error -# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are -# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if -# unspecified. - process_noise_covariance: [0.05, 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.05, 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.06, 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.03, 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.03, 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.06, 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.025, 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.025, 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.04, 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.01, 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.01, 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.02, 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.01, 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.01, 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.015] -# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal -# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in -# question. Users should take care not to use large values for variables that will not be measured directly. The values -# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below -#if unspecified. - initial_estimate_covariance: [1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9] + reset_on_time_jump: false + predict_to_current_time: false + print_diagnostics: false + + # Selected values ​​experimentally so as to ensure relatively fast convergence (values ​​should be about 10x higher than the sensor variance values) + dynamic_process_noise_covariance: true + process_noise_covariance: [5e-3, 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, 5e-3, 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, 5e-3, 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, 1e-3, 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, 1e-3, 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, 1e-3, 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, 3e-3, 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, 3e-3, 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, 3e-3, 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, 1e-2, 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, 1e-2, 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, 1e-2, 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, 1e-5, 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, 1e-5, 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, 1e-5] diff --git a/rosbot_xl_controller/config/diff_drive_controller.yaml b/rosbot_xl_controller/config/diff_drive_controller.yaml index 6870a465..b1b097d7 100644 --- a/rosbot_xl_controller/config/diff_drive_controller.yaml +++ b/rosbot_xl_controller/config/diff_drive_controller.yaml @@ -32,6 +32,9 @@ imu_broadcaster: ros__parameters: sensor_name: "imu" frame_id: "imu_link" + static_covariance_orientation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally rosbot_xl_base_controller: ros__parameters: @@ -54,7 +57,7 @@ rosbot_xl_base_controller: odom_frame_id: odom base_frame_id: base_link pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally # Whether to use feedback or commands for odometry calculations open_loop: false diff --git a/rosbot_xl_controller/config/mecanum_drive_controller.yaml b/rosbot_xl_controller/config/mecanum_drive_controller.yaml index 80def4fd..8b10a178 100644 --- a/rosbot_xl_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_xl_controller/config/mecanum_drive_controller.yaml @@ -32,6 +32,10 @@ imu_broadcaster: ros__parameters: sensor_name: "imu" frame_id: "imu_link" + static_covariance_orientation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive + rosbot_xl_base_controller: ros__parameters: @@ -54,7 +58,7 @@ rosbot_xl_base_controller: odom_frame_id: odom base_frame_id: base_link pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally # Whether to use feedback or commands for odometry calculations open_loop: false