X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=config%2Frobot_localization.yaml;h=404986785e40370908419e4dec5ce26d7f7b884e;hp=ca599f95fa899053a387a75e628db6bca4442151;hb=HEAD;hpb=2211b9a871c413ff7e704ca4600dc38fb65369ec diff --git a/config/robot_localization.yaml b/config/robot_localization.yaml index ca599f9..4049867 100644 --- a/config/robot_localization.yaml +++ b/config/robot_localization.yaml @@ -46,13 +46,14 @@ world_frame: odom # 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 +#transform_time_offset: 0.05 # The filter accepts an arbitrary number of inputs from each input message type (Odometry, PoseStamped, # TwistStamped, Imu). To add a new one, 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: odom +odom1: gps imu0: imu imu0_queue_size: 10 @@ -63,13 +64,20 @@ imu0_queue_size: 10 # message types lack certain variables. 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. +# odom # x/y not included because of redundancy with velocities # vyaw not included in odom because too inaccurate odom0_config: [false, false, false, - false, false, false, + false, false, false, true, true, true, false, false, true, false, false, false] +# gps +odom1_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] imu0_config: [false, false, false, true, true, true, false, false, false, @@ -84,15 +92,16 @@ imu0_config: [false, false, false, # either increase the covariances for the variables in question, or you can simply set the sensor's differential # parameter to true. 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 absolute measurements, so setting differential to true for twit measurements has +# applies to sensors that provide absolute measurements, so setting differential to true for twist measurements has # no effect. -# +# # Users should take care when setting this to true for orientation variables: if you have only one source of # absolute orientation data, you should not set the differential parameter to true. This is due to the fact that # integration of velocities leads to slowly increasing error in the absolute (pose) variable. For position variables, # this is acceptable. For orientation variables, it can lead to trouble. Users should make sure that all orientation # variables have at least one source of absolute measurement. odom0_differential: false +odom1_differential: false imu0_differential: false # When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" for all @@ -100,6 +109,7 @@ imu0_differential: false # 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 +odom1_relative: false imu0_relative: false # If we're including accelerations in our state estimate, then we'll probably want to remove any acceleration that @@ -110,6 +120,49 @@ imu0_remove_gravitational_acceleration: true # if the node is unhappy with any settings or data. print_diagnostics: true -# If true, will dynamically scale the process_noise_covariance based on the robot?s velocity. This is useful, e.g., when you want your +# If true, will dynamically scale the process_noise_covariance based on the robot's velocity. This is useful, e.g., when you want your # robots estimate error covariance to stop growing when the robot is stationary. Defaults to false. dynamic_process_noise_covariance: true + +# 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. +process_noise_covariance: [2.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 2.5, 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.03, 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.06, 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.025, 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.01, 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.02, 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.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + +#initial_estimate_covariance: [0.0082783, 0.00015023, 0, 0, 0, 1.4766e-22, 0.0056327, -0.0038416, 0, 0, 0, 1.863e-22, 0.0031683, -0.0021608, 0, +# 0.00015023, 0.010788, 0, 0, 0, -3.5314e-22, 0.0038418, 0.0056329, 0, 0, 0, -4.4555e-22, 0.0021611, 0.0031687, 0, +# 0, 0, 0.0018072, 3.2715e-24, 1.5934e-23, 0, 0, 0, 0.0054835, 8.4669e-24, 4.1238e-23, 0, 0, 0, 0.00033362, +# 0, 0, 3.2715e-24, 0.00044495, -6.2008e-49, 0, 0, 0, -1.2846e-33, 0.0013386, -7.0886e-35, 0, 0, 0, -4.1097e-35, +# 0, 0, 1.5934e-23, -6.2142e-49, 0.00044495, 0, 0, 0, -6.2545e-33, 7.0886e-35, 0.0013386, 0, 0, 0, -2.0009e-34, +# 1.4766e-22, -3.5314e-22, 0, 0, 0, 0.006296, -2.6304e-31, 2.1336e-30, 0, 0, 0, 0.0091462, -4.0833e-31, 3.3122e-30, 0, +# 0.0056327, 0.0038418, 0, 0, 0, -2.6304e-31, 0.023382, 1.2088e-09, 0, 0, 0, -1.8639e-30, 0.011984, 1.8766e-09, 0, +# -0.0038416, 0.0056329, 0, 0, 0, 2.1336e-30, 1.2088e-09, 0.023382, 0, 0, 0, 1.3775e-29, 1.8766e-09, 0.011984, 0, +# 0, 0, 0.0054835, -1.2846e-33, -6.2545e-33, 0, 0, 0, 0.023656, 1.265e-37, 6.1588e-37, 0, 0, 0, 0.0020076, +# 0, 0, 8.4669e-24, 0.0013386, 7.0886e-35, 0, 0, 0, 1.265e-37, 0.005748, -6.135e-57, 0, 0, 0, 4.0468e-39, +# 0, 0, 4.1238e-23, -7.0886e-35, 0.0013386, 0, 0, 0, 6.1588e-37, -6.135e-57, 0.005748, 0, 0, 0, 1.9703e-38, +# 1.863e-22, -4.4555e-22, 0, 0, 0, 0.0091462, -1.8639e-30, 1.3775e-29, 0, 0, 0, 0.01982, -2.8935e-30, 2.1383e-29, 0, +# 0.0031683, 0.0021611, 0, 0, 0, -4.0833e-31, 0.011984, 1.8766e-09, 0, 0, 0, -2.8935e-30, 0.022337, 2.9131e-09, 0, +# -0.0021608, 0.0031687, 0, 0, 0, 3.3122e-30, 1.8766e-09, 0.011984, 0, 0, 0, 2.1383e-29, 2.9131e-09, 0.022337, 0, +# 0, 0, 0.00033362, -4.1097e-35, -2.0009e-34, 0, 0, 0, 0.0020076, 4.0468e-39, 1.9703e-38, 0, 0, 0, 0.0086215] +# +# +#debug: true +#debug_out_file: "/tmp/log.txt"