diff --git a/rosbot_bringup/config/ekf.yaml b/rosbot_bringup/config/ekf.yaml index e042a542..2a1de09b 100644 --- a/rosbot_bringup/config/ekf.yaml +++ b/rosbot_bringup/config/ekf.yaml @@ -1,85 +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: 30.0 - sensor_timeout: 0.06 + + frequency: 25.0 + 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_base_controller/odom - 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_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. - # Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support - # both full specification or specification of only the diagonal values. - 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] + reset_on_time_jump: false + predict_to_current_time: false + print_diagnostics: true - # [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-8, 0.0, 0.0, 0.0, 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] + # 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, 2e-4, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2e-4, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2e-4, 0.0, 0.0, 0.0, + 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] \ No newline at end of file diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index 8167c495..4fb657c7 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -33,6 +33,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.8e-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_base_controller: ros__parameters: @@ -55,7 +58,7 @@ rosbot_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_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 4f4df446..6c45a6d4 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -28,6 +28,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] # Values measured experimentally + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8e-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_base_controller: ros__parameters: @@ -51,7 +54,7 @@ rosbot_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