# 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: 20 # 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 # If this 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: true # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies three principal coordinate frames: map, # odom, and base_link. base_link is the coordinate frame that is affixed to the robot. 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. 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. # * 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 instance of robot_localization! However, that instance should *not* fuse the global data. # Defaults to "map" if unspecified map_frame: map # Defaults to "odom" if unspecified odom_frame: odom # Defaults to "base_link" if unspecified base_link_frame: base_footprint # Defaults to the value of "odom_frame" if unspecified 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.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 # 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 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, 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, false, false, false, false, false, false] # The best practice for including new sensors in robot_localization's state estimation nodes is to pass in velocity # measurements and let the nodes integrate them. However, this isn't always feasible, and so the state estimation # nodes support fusion of absolute measurements. If you have more than one sensor providing absolute measurements, # however, you may run into problems if your covariances are not large enough, as the sensors will inevitably # diverge from one another, causing the filter to jump back and forth rapidly. To combat this situation, you can # 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 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 # 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 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 # is due to gravity for each IMU. If you don't want to, then set this to false. Defaults to false if unspecified. imu0_remove_gravitational_acceleration: true # 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 # 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]