From a8b92e965ce7934782a6f9abd86440bbfc68f4be Mon Sep 17 00:00:00 2001 From: Tom Moore Date: Fri, 20 May 2022 10:22:21 +0100 Subject: [PATCH] Adding support for setting diagonals for covariance matrices (#755) --- doc/state_estimation_nodes.rst | 4 + params/dual_ekf_navsat_example.yaml | 69 +------ params/ekf.yaml | 259 +++++++++++++------------- params/ukf.yaml | 273 ++++++++++++++-------------- src/ros_filter.cpp | 77 +++----- test/test_se_node_interfaces.yaml | 17 +- 6 files changed, 304 insertions(+), 395 deletions(-) diff --git a/doc/state_estimation_nodes.rst b/doc/state_estimation_nodes.rst index 350e5b441..b40816537 100644 --- a/doc/state_estimation_nodes.rst +++ b/doc/state_estimation_nodes.rst @@ -280,6 +280,8 @@ If ``debug`` is *true*, the file to which debug output is written. ^^^^^^^^^^^^^^^^^^^^^^^^^ The process noise covariance, commonly denoted *Q*, is used to model uncertainty in the prediction stage of the filtering algorithms. It can be difficult to tune, and has been exposed as a parameter for easier customization. This parameter can be left alone, but you will achieve superior results by tuning it. In general, the larger the value for *Q* relative to the variance for a given variable in an input message, the faster the filter will converge to the value in the measurement. +Specifying the full covariance matrix is supported, but can be cumbersome. For that reason, this parameter can also be used to simply specify the diagonal values. In that event, all off-diagonal values will be set to 0. + ~dynamic_process_noise_covariance ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ If *true*, will dynamically scale the ``process_noise_covariance`` based on the robot's velocity. This is useful, e.g., when you want your robot's estimate error covariance to stop growing when the robot is stationary. Defaults to *false*. @@ -288,6 +290,8 @@ If *true*, will dynamically scale the ``process_noise_covariance`` based on the ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ The estimate covariance, commonly denoted *P*, defines the error in the current state estimate. The parameter allows users to set the initial value for the matrix, which will affect how quickly the filter converges. For example, if users set the value at position :math:`[0, 0]` to a very small value, e.g., `1e-12`, and then attempt to fuse measurements of X position with a high variance value for :math:`X`, then the filter will be very slow to "trust" those measurements, and the time required for convergence will increase. Again, users should take care with this parameter. When only fusing velocity data (e.g., no absolute pose information), users will likely *not* want to set the initial covariance values for the absolute pose variables to large numbers. This is because those errors are going to grow without bound (owing to the lack of absolute pose measurements to reduce the error), and starting them with large values will not benefit the state estimate. +Specifying the full covariance matrix is supported, but can be cumbersome. For that reason, this parameter can also be used to simply specify the diagonal values. In that event, all off-diagonal values will be set to 0. + ~reset_on_time_jump ^^^^^^^^^^^^^^^^^^^ If set to *true* and ``ros::Time::isSimTime()`` is *true*, the filter will reset to its uninitialized state when a jump back in time is detected on a topic. This is useful when working with bag data, in that the bag can be restarted without restarting the node. diff --git a/params/dual_ekf_navsat_example.yaml b/params/dual_ekf_navsat_example.yaml index e607c340c..6d5520260 100644 --- a/params/dual_ekf_navsat_example.yaml +++ b/params/dual_ekf_navsat_example.yaml @@ -40,37 +40,10 @@ ekf_filter_node_odom: use_control: false - process_noise_covariance: [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, 0.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, 0.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, 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.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, 0.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, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.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, 0.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, 0.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, 0.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, 0.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, 0.3] - - 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, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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.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, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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.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.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.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.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.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.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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] + process_noise_covariance: [1e-3, 1e-3, 1e-3, 0.3, 0.3, 0.01, 0.5, 0.5, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3] + + initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1.0, 1.0, 1e-9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + ekf_filter_node_map: ros__parameters: frequency: 30.0 @@ -122,37 +95,9 @@ ekf_filter_node_map: use_control: false - process_noise_covariance: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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.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, 0.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, 0.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, 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.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, 0.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, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.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, 0.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, 0.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, 0.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, 0.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, 0.3] - - initial_estimate_covariance: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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.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, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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.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, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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.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.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.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.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.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.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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] + process_noise_covariance: [1.0, 1.0, 1e-3, 0.3, 0.3, 0.01, 0.5, 0.5, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3] + + initial_estimate_covariance: [1.0, 1.0, 1e-9, 1.0, 1.0, 1e-9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] navsat_transform: ros__parameters: diff --git a/params/ekf.yaml b/params/ekf.yaml index d44586979..44c3d81aa 100644 --- a/params/ekf.yaml +++ b/params/ekf.yaml @@ -1,131 +1,131 @@ ### ekf config file ### ekf_filter_node: ros__parameters: -# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin -# computation until it receives at least one message from one of the inputs. It will then run continuously at the -# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. + # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin + # computation until it receives at least one message from one of the inputs. It will then run continuously at the + # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. frequency: 30.0 -# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict -# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the -# filter will generate new output. Defaults to 1 / frequency if not specified. + # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict + # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the + # filter will generate new output. Defaults to 1 / frequency if not specified. sensor_timeout: 0.1 -# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is -# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar -# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected -# by, for example, an IMU. Defaults to false if unspecified. + # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is + # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar + # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected + # by, for example, an IMU. Defaults to false if unspecified. two_d_mode: false -# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for -# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if -# unspecified. + # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for + # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if + # unspecified. transform_time_offset: 0.0 -# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. -# Defaults to 0.0 if unspecified. + # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. + # Defaults to 0.0 if unspecified. transform_timeout: 0.0 -# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is -# unhappy with any settings or data. + # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is + # unhappy with any settings or data. print_diagnostics: true -# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by -# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious -# effects on the performance of the node. Defaults to false if unspecified. + # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by + # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious + # effects on the performance of the node. Defaults to false if unspecified. debug: false -# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. + # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. debug_out_file: /path/to/debug/file.txt -# Whether we'll allow old measurements to cause a re-publication of the updated state + # Whether we'll allow old measurements to cause a re-publication of the updated state permit_corrected_publication: false -# Whether to publish the acceleration state. Defaults to false if unspecified. + # Whether to publish the acceleration state. Defaults to false if unspecified. publish_acceleration: false -# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. publish_tf: true -# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and -# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. -# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be -# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom -# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your -# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based -# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. -# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. -# Here is how to use the following settings: -# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. -# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of -# odom_frame. -# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set -# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. -# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates -# from landmark observations) then: -# 3a. Set your "world_frame" to your map_frame value -# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state -# estimation node from robot_localization! However, that instance should *not* fuse the global data. + # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and + # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. + # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be + # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom + # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your + # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based + # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. + # ekf_localization_node and ukf_localization_node are not concerned with the earth frame. + # Here is how to use the following settings: + # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. + # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of + # odom_frame. + # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set + # "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. + # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates + # from landmark observations) then: + # 3a. Set your "world_frame" to your map_frame value + # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state + # estimation node from robot_localization! However, that instance should *not* fuse the global data. 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: odom # Defaults to the value of odom_frame if unspecified -# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, -# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, -# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, -# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no -# default values, and must be specified. + # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, + # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, + # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, + # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no + # default values, and must be specified. odom0: example/odom -# Each sensor reading updates some or all of the filter's state. These options give you greater control over which -# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only -# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the -# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types -# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message -# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false -# if unspecified, effectively making this parameter required for each sensor. + # Each sensor reading updates some or all of the filter's state. These options give you greater control over which + # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only + # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the + # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types + # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message + # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false + # if unspecified, effectively making this parameter required for each sensor. odom0_config: [true, true, false, false, false, false, false, false, false, false, false, true, false, false, false] -# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase -# the size of the subscription queue so that more measurements are fused. + # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase + # the size of the subscription queue so that more measurements are fused. odom0_queue_size: 2 -# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result -# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's -# algorithm. + # [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result + # of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's + # algorithm. odom0_nodelay: false -# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- -# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they -# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also -# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't -# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose -# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then -# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true -# for twist measurements has no effect. + # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- + # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they + # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also + # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't + # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose + # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then + # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true + # for twist measurements has no effect. odom0_differential: false -# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" -# for all future measurements. While you can achieve the same effect with the differential paremeter, the key -# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before -# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" + # for all future measurements. While you can achieve the same effect with the differential paremeter, the key + # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before + # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. odom0_relative: false -# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to -# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to -# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not -# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. -# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying -# the thresholds. + # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to + # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to + # numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not + # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. + # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying + # the thresholds. odom0_pose_rejection_threshold: 5.0 odom0_twist_rejection_threshold: 1.0 -# Further input parameter examples + # Further input parameter examples odom1: example/odom2 odom1_config: [false, false, true, false, false, false, @@ -174,48 +174,58 @@ ekf_filter_node: imu0_twist_rejection_threshold: 0.8 # imu0_linear_acceleration_rejection_threshold: 0.8 # -# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set -# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. + # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set + # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. imu0_remove_gravitational_acceleration: true -# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no -# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During -# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be -# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When -# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially -# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance -# for the velocity variable in question, or decrease the variance of the variable in question in the measurement -# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we -# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during -# predicition. Note that if an acceleration measurement for the variable in question is available from one of the -# inputs, the control term will be ignored. -# Whether or not we use the control input during predicition. Defaults to false. + # [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no + # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During + # correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be + # problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When + # this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially + # noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance + # for the velocity variable in question, or decrease the variance of the variable in question in the measurement + # itself. In addition, users can also take advantage of the control command being issued to the robot at the time we + # make the prediction. If control is used, it will get converted into an acceleration term, which will be used during + # predicition. Note that if an acceleration measurement for the variable in question is available from one of the + # inputs, the control term will be ignored. + # Whether or not we use the control input during predicition. Defaults to false. use_control: true -# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to -# false. + + # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to + # false. stamped_control: false -# The last issued control command will be used in prediction for this period. Defaults to 0.2. + + # The last issued control command will be used in prediction for this period. Defaults to 0.2. control_timeout: 0.2 -# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. + + # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. control_config: [true, false, false, false, false, true] -# Places limits on how large the acceleration term will be. Should match your robot's kinematics. + + # Places limits on how large the acceleration term will be. Should match your robot's kinematics. acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] -# Acceleration and deceleration limits are not always the same for robots. + + # Acceleration and deceleration limits are not always the same for robots. deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] -# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these -# gains + + # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these + # gains acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] -# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these -# gains + + # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these + # gains deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] -# [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. + + # [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, @@ -231,24 +241,11 @@ ekf_filter_node: 0.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] + + # [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 diagonal values below + # if unspecified. In this example, we specify only the diagonal of the matrix. + initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] diff --git a/params/ukf.yaml b/params/ukf.yaml index ff7a2b900..eb7732f76 100644 --- a/params/ukf.yaml +++ b/params/ukf.yaml @@ -1,131 +1,131 @@ ### ukf config file ### ukf_filter_node: ros__parameters: -# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin -# computation until it receives at least one message from one of the inputs. It will then run continuously at the -# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. + # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin + # computation until it receives at least one message from one of the inputs. It will then run continuously at the + # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. frequency: 30.0 -# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict -# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the -# filter will generate new output. Defaults to 1 / frequency if not specified. + # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict + # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the + # filter will generate new output. Defaults to 1 / frequency if not specified. sensor_timeout: 0.1 -# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is -# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar -# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected -# by, for example, an IMU. Defaults to false if unspecified. + # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is + # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar + # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected + # by, for example, an IMU. Defaults to false if unspecified. two_d_mode: false -# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for -# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if -# unspecified. + # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for + # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if + # unspecified. transform_time_offset: 0.0 -# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. -# Defaults to 0.0 if unspecified. + # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. + # Defaults to 0.0 if unspecified. transform_timeout: 0.0 -# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is -# unhappy with any settings or data. + # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is + # unhappy with any settings or data. print_diagnostics: true -# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by -# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious -# effects on the performance of the node. Defaults to false if unspecified. + # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by + # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious + # effects on the performance of the node. Defaults to false if unspecified. debug: false -# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. + # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. debug_out_file: /path/to/debug/file.txt -# Whether we'll allow old measurements to cause a re-publication of the updated state + # Whether we'll allow old measurements to cause a re-publication of the updated state permit_corrected_publication: false -# Whether to publish the acceleration state. Defaults to false if unspecified. + # Whether to publish the acceleration state. Defaults to false if unspecified. publish_acceleration: false -# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. publish_tf: true -# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and -# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. -# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be -# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom -# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your -# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based -# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. -# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. -# Here is how to use the following settings: -# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. -# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of -# odom_frame. -# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set -# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. -# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates -# from landmark observations) then: -# 3a. Set your "world_frame" to your map_frame value -# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state -# estimation node from robot_localization! However, that instance should *not* fuse the global data. + # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and + # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. + # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be + # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom + # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your + # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based + # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. + # ekf_localization_node and ukf_localization_node are not concerned with the earth frame. + # Here is how to use the following settings: + # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. + # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of + # odom_frame. + # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set + # "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. + # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates + # from landmark observations) then: + # 3a. Set your "world_frame" to your map_frame value + # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state + # estimation node from robot_localization! However, that instance should *not* fuse the global data. 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: odom # Defaults to the value of odom_frame if unspecified -# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, -# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, -# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, -# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no -# default values, and must be specified. + # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, + # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, + # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, + # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no + # default values, and must be specified. odom0: example/odom -# Each sensor reading updates some or all of the filter's state. These options give you greater control over which -# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only -# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the -# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types -# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message -# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false -# if unspecified, effectively making this parameter required for each sensor. + # Each sensor reading updates some or all of the filter's state. These options give you greater control over which + # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only + # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the + # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types + # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message + # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false + # if unspecified, effectively making this parameter required for each sensor. odom0_config: [true, true, false, false, false, false, false, false, false, false, false, true, false, false, false] -# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase -# the size of the subscription queue so that more measurements are fused. + # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase + # the size of the subscription queue so that more measurements are fused. odom0_queue_size: 2 -# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result -# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's -# algorithm. + # [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result + # of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's + # algorithm. odom0_nodelay: false -# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- -# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they -# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also -# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't -# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose -# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then -# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true -# for twist measurements has no effect. + # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- + # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they + # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also + # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't + # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose + # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then + # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true + # for twist measurements has no effect. odom0_differential: false -# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" -# for all future measurements. While you can achieve the same effect with the differential paremeter, the key -# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before -# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" + # for all future measurements. While you can achieve the same effect with the differential paremeter, the key + # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before + # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. odom0_relative: false -# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to -# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to -# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not -# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. -# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying -# the thresholds. + # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to + # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to + # numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not + # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. + # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying + # the thresholds. odom0_pose_rejection_threshold: 5.0 odom0_twist_rejection_threshold: 1.0 -# Further input parameter examples + # Further input parameter examples odom1: example/another_odom odom1_config: [false, false, true, false, false, false, @@ -174,48 +174,58 @@ ukf_filter_node: imu0_twist_rejection_threshold: 0.8 # imu0_linear_acceleration_rejection_threshold: 0.8 # -# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set -# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. + # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set + # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. imu0_remove_gravitational_acceleration: true -# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no -# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During -# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be -# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When -# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially -# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance -# for the velocity variable in question, or decrease the variance of the variable in question in the measurement -# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we -# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during -# predicition. Note that if an acceleration measurement for the variable in question is available from one of the -# inputs, the control term will be ignored. -# Whether or not we use the control input during predicition. Defaults to false. + # [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no + # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During + # correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be + # problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When + # this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially + # noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance + # for the velocity variable in question, or decrease the variance of the variable in question in the measurement + # itself. In addition, users can also take advantage of the control command being issued to the robot at the time we + # make the prediction. If control is used, it will get converted into an acceleration term, which will be used during + # predicition. Note that if an acceleration measurement for the variable in question is available from one of the + # inputs, the control term will be ignored. + # Whether or not we use the control input during predicition. Defaults to false. use_control: true -# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to -# false. + + # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to + # false. stamped_control: false -# The last issued control command will be used in prediction for this period. Defaults to 0.2. + + # The last issued control command will be used in prediction for this period. Defaults to 0.2. control_timeout: 0.2 -# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. + + # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. control_config: [true, false, false, false, false, true] -# Places limits on how large the acceleration term will be. Should match your robot's kinematics. + + # Places limits on how large the acceleration term will be. Should match your robot's kinematics. acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] -# Acceleration and deceleration limits are not always the same for robots. + + # Acceleration and deceleration limits are not always the same for robots. deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] -# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these -# gains + + # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these + # gains acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] -# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these -# gains + + # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these + # gains deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] -# [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. + + # [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, @@ -231,34 +241,21 @@ ukf_filter_node: 0.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] - -# [ADVANCED, UKF ONLY] The alpha and kappa variables control the spread of the sigma points. Unless you are familiar -# with UKFs, it's probably a good idea to leave these alone. -# Defaults to 0.001 if unspecified. + + # [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 diagonal values below + # if unspecified. In this example, we specify only the diagonal of the matrix. + initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] + + # [ADVANCED, UKF ONLY] The alpha and kappa variables control the spread of the sigma points. Unless you are familiar + # with UKFs, it's probably a good idea to leave these alone. + # Defaults to 0.001 if unspecified. alpha: 0.001 -# Defaults to 0 if unspecified. + # Defaults to 0 if unspecified. kappa: 0.0 -# [ADVANCED, UKF ONLY] The beta variable relates to the distribution of the state vector. Again, it's probably best to -# leave this alone if you're uncertain. Defaults to 2 if unspecified. + # [ADVANCED, UKF ONLY] The beta variable relates to the distribution of the state vector. Again, it's probably best to + # leave this alone if you're uncertain. Defaults to 2 if unspecified. beta: 2.0 diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 9c09c8a3f..f0671ff8a 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -1791,57 +1792,37 @@ void RosFilter::loadParams() } } - // Load up the process noise covariance (from the launch file/parameter - // server) - process_noise_covariance_.setZero(); - std::vector process_noise_covar_flat; - - this->declare_parameter("process_noise_covariance", rclcpp::PARAMETER_DOUBLE_ARRAY); - if (this->get_parameter( - "process_noise_covariance", - process_noise_covar_flat)) - { - assert(process_noise_covar_flat.size() == STATE_SIZE * STATE_SIZE); - - for (int i = 0; i < STATE_SIZE; i++) { - for (int j = 0; j < STATE_SIZE; j++) { - process_noise_covariance_(i, j) = - process_noise_covar_flat[i * STATE_SIZE + j]; - } - } - - RF_DEBUG( - "Process noise covariance is:\n" << - process_noise_covariance_ << "\n"); - - filter_.setProcessNoiseCovariance(process_noise_covariance_); - } - - // Load up the process noise covariance (from the launch file/parameter - // server) - initial_estimate_error_covariance_.setZero(); - std::vector estimate_error_covar_flat; - - this->declare_parameter("initial_estimate_covariance", rclcpp::PARAMETER_DOUBLE_ARRAY); - if (this->get_parameter( - "initial_estimate_covariance", - estimate_error_covar_flat)) - { - assert(estimate_error_covar_flat.size() == STATE_SIZE * STATE_SIZE); - - for (int i = 0; i < STATE_SIZE; i++) { - for (int j = 0; j < STATE_SIZE; j++) { - initial_estimate_error_covariance_(i, j) = - estimate_error_covar_flat[i * STATE_SIZE + j]; + auto load_covariance = [this](const std::string & parameter, Eigen::MatrixXd & covariance) + { + covariance.setZero(); + std::vector covar_flat; + + declare_parameter(parameter, rclcpp::PARAMETER_DOUBLE_ARRAY); + if (get_parameter(parameter, covar_flat)) { + if (covar_flat.size() == STATE_SIZE) { + RCLCPP_INFO_STREAM( + get_logger(), "Detected a " << parameter << " parameter with " + "length " << STATE_SIZE << ". Assuming diagonal values specified."); + covariance.diagonal() = Eigen::VectorXd::Map(covar_flat.data(), STATE_SIZE); + } else if (covariance.size() == STATE_SIZE * STATE_SIZE) { + covariance = Eigen::MatrixXd::Map(covar_flat.data(), STATE_SIZE, STATE_SIZE); + } else { + std::string error = "Invalid " + parameter + " specified. Expected a length of " + + std::to_string(STATE_SIZE) + " or " + std::to_string(STATE_SIZE * STATE_SIZE) + + ", received length " + std::to_string(covar_flat.size()); + RCLCPP_FATAL_STREAM(get_logger(), error); + throw std::invalid_argument(error); + } } - } + }; - RF_DEBUG( - "Initial estimate error covariance is:\n" << - initial_estimate_error_covariance_ << "\n"); + load_covariance("process_noise_covariance", process_noise_covariance_); + RF_DEBUG("Process noise covariance is:\n" << process_noise_covariance_ << "\n"); + filter_.setProcessNoiseCovariance(process_noise_covariance_); - filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); - } + load_covariance("initial_estimate_covariance", initial_estimate_error_covariance_); + RF_DEBUG("Initial estimate covariance is:\n" << initial_estimate_error_covariance_ << "\n"); + filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); } template diff --git a/test/test_se_node_interfaces.yaml b/test/test_se_node_interfaces.yaml index 2a5fb8e80..9d8c58031 100644 --- a/test/test_se_node_interfaces.yaml +++ b/test/test_se_node_interfaces.yaml @@ -78,7 +78,6 @@ test_se_node_interfaces: false, false, false, false, false, false] -### odom1_differential: true pose1_differential: true imu3_differential: true @@ -88,21 +87,7 @@ test_se_node_interfaces: odom_frame: odom base_link_frame: base_link - process_noise_covariance: [1e-6, 0.0, 0.0, 0.0, 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-6, 0.0, 0.0, 0.0, 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-6, 0.0, 0.0, 0.0, 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-6, 0.0, 0.0, 0.0, 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-6, 0.0, 0.0, 0.0, 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-6, 0.0, 0.0, 0.0, 0.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.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.002, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, - 0.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.01] + process_noise_covariance: [1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 0.025, 0.025, 0.05, 0.002, 0.002, 0.004, 0.01, 0.01, 0.01] # UKF-specific settings that the EKF will ignore alpha: 0.001