Download

ROS Extended Kalman Filter

Description

Odometry estimation with EKF (Extended Kalman Filter) for robot localization. From package robot_localization.
http://wiki.ros.org/robot_localization

Examples

...

Input List

There are 6 parameters.

Port NameMessage typeCallback
enableEmpty [std_srvs-ROS1/ServiceServer]
imu0Imu [sensor_msgs-ROS1/Subscriber]place_holder.cb
odom0Odometry [nav_msgs-ROS1/Subscriber]place_holder.cb
set_poseSetPose [robot_localization-ROS1/ServiceServer]
tf_inTFMessage [tf2_msgs-ROS1/Subscriber]
toggleToggleFilterProcessing [robot_localization-ROS1/ServiceServer]

Output List

There are 4 parameters.

Port NameMessage type
accel/filteredAccelWithCovarianceStamped [nav_msgs-ROS1/Publisher]
diagnosticsDiagnosticArray [diagnostic_msgs-ROS1/Publisher]
odometry/filteredOdometry [nav_msgs-ROS1/Publisher]
tf_outtfMessage [tf-ROS1/Publisher]

Parameter List

There are 36 parameters.

ParameterDescription
acceleration_gains[double] If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these gains. This is only used if use_control is set to true.

[0.8, 0.0, 0.0,
0.0, 0.0, 0.9]
acceleration_limits[double] How rapidly your robot can accelerate for each dimension. Matches the parameter order in control_config. Only used if use_control is set to true.

[1.3, 0.0, 0.0,
0.0, 0.0, 3.2]

base_link_frame[str] These parameters define the operating “mode” for robot_localization. REP-105 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 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, such as, due to the fusion of GPS data. Here is how to use these parameters –

Set the map_frame, odom_frame, and base_link_frame parameters to the appropriate frame names for your system.

Note – 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.

Note – If you are running multiple EKF instances and would like to “override” the output transform and message to have this frame for its child_frame_id, you may set this. The base_link_output_frame is optional and will default to the base_link_frame. This helps to enable disconnected TF trees when multiple EKF instances are being run. When the final state is computed, we “override” the output transform and message to have this frame for its child_frame_id.

If you are only 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 the state estimation nodes in robot_localization, and the most common use for it.

If you are fusing global absolute position data that is subject to discrete jumps (such as GPS or position updates from landmark observations) then –

Set your world_frame to your map_frame value.

Make sure something else is generating the odom->base_link transform. This can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data.

The default values for map_frame, odom_frame and base_link_frame are map, odom, and base_link, respectively. The base_link_output_frame parameter defaults to the value of base_link_frame. The world_frame parameter defaults to the value of odom_frame.
control_config[boolean] Controls which variables in the cmd_vel message are used in state prediction. The order of the values is X˙,Y˙,Z˙,roll˙,pitch˙,yaw˙. Only used if use_control is set to true.

[true, false, false,
false, false, true]

control_timeout[double] If use_control is set to true and no control command is received in this amount of time (in seconds), then the control-based acceleration term ceases to be applied.
debug[boolean] Flag that specifies whether or not to run in debug mode.
WARNING – Setting this to true will generate a massive amount of data. The data is written to the value of the debug_out_file parameter. Defaults to false.
debug_out_file[str] If debug is true, this is the file to which debug output is written.
deceleration_gains[double] If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these gains. Only used if use_control is set to true.
deceleration_limits[double] How rapidly your robot can decelerate for each dimension. Matches the parameter order in control_config. Only used if use_control is set to true.
frequency[int] The real-valued frequency, in Hz, at which the filter produces a state estimate.
imu0[str] For each sensor, users must define this parameter according to the message type. For example, when one source of Imu messages is defined and two sources of Odometry messages are defined, the configuration appears as follows –





The index for each parameter name is 0-based (such as odom0, odom1 and so on) and must be defined sequentially. For example, do not use pose0 and pose2 if you have not defined pose1. The values for each parameter are the topic name for that sensor.
imu0_config[boolean] Specific parameters –

~odomN_config
~twistN_config
~imuN_config
~poseN_config

For each of the sensor messages defined above, users must specify which variables of those messages should be fused into the final state estimate. An example odometry configuration might look like this –

[true, true, false,
false, false, true,
true, false, false,
false, false, true,
false, false, false]

The order of the boolean values are X,Y,Z,roll,pitch,yaw,X˙,Y˙,Z˙,roll˙,pitch˙,yaw˙,X¨,Y¨,Z¨. In this example, we are fusing X and Y position, yaw, X˙, and yaw˙.

Note – The specification is done in the frame_id of the sensor, not in the world_frame or base_link_frame. Please see the configuration tutorial GGG for more information.
imu0_differential[boolean] Specific parameters –

~odomN_differential
~imuN_differential
~poseN_differential

For each of the sensor messages defined above that contain pose information, users can specify whether the pose variables should be integrated differentially. If a given value is set to true, then for a measurement at time t from the sensor in question, we first subtract the measurement at time t−1, and convert the resulting value to a velocity. This setting is especially useful if your robot has two sources of absolute pose information, such as yaw measurements from odometry and an IMU. In that case, if the variances of the input sources are not configured correctly, these measurements may become out of sync with one another and cause oscillations in the filter, but by integrating one or both of them differentially, we avoid this scenario.

Users should take care when using this parameter for orientation data, as the conversion to velocity means that the covariance for orientation state variables will grow without bound (unless another source of absolute orientation data is being fused). If you simply want all of your pose variables to start at 0, then use the _relative parameter.

Note – If you are fusing GPS information via navsat_transform_node or utm_transform_node, you should make sure that the _differential setting is false.
imu0_nodelay[boolean] Specific parameters –

~odomN_nodelay
~twistN_nodelay
~imuN_nodelay
~poseN_nodelay

If true, sets the tcpNoDelay transport hint. There is some evidence that Nagle’s algorithm interferes with the timely reception of large message types, such as the nav_msgs/Odometry message. Setting this to true for an input disables Nagle’s algorithm for that subscriber. Defaults to false.
imu0_relative[boolean] Specific parameters –

~odomN_relative
~imuN_relative
~poseN_relative

If this parameter is set to true, then any measurements from this sensor will be fused relative to the first measurement received from that sensor. This is useful if, for example, you want your state estimate to always start at (0,0,0) and with roll, pitch, and yaw values of (0,0,0). It is similar to the _differential parameter, but instead of removing the measurement at time t−1, we always remove the measurement at time 0, and the measurement is not converted to a velocity.
imu0_remove_gravitational_accelerationIf fusing accelerometer data from IMUs, this parameter determines whether or not acceleration due to gravity is removed from the acceleration measurement before fusing it.

Note – This assumes that the IMU that is providing the acceleration data is also producing an absolute orientation. The orientation data is required to correctly remove gravitational acceleration.
initial_estimate_covariance[double] 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 [0,0] to a very small value, such as 1e-12, and then attempt to fuse measurements of X position with a high variance value for X, then the filter will be very slow to “trust” those measurements, and the time required for convergence will increase. Again, users should be careful this parameter. When only fusing velocity data (such as 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.
map_frame[str] These parameters define the operating “mode” for robot_localization. REP-105 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 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.

For example, due to the fusion of GPS data. Here is how to use these parameters –

Set the map_frame, odom_frame, and base_link_frame parameters to the appropriate frame names for your system.

Note – 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.

Note – If you are running multiple EKF instances and would like to “override” the output transform and message to have this frame for its child_frame_id, you may set this. The base_link_output_frame is optional and will default to the base_link_frame. This helps to enable disconnected TF trees when multiple EKF instances are being run. When the final state is computed, we “override” the output transform and message to have this frame for its child_frame_id.

If you are only 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 the state estimation nodes in robot_localization, and the most common use for it.

If you are fusing global absolute position data that is subject to discrete jumps (such as GPS or position updates from landmark observations) then –

Set your world_frame to your map_frame value.

Make sure something else is generating the odom->base_link transform. This can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data.

The default values for map_frame, odom_frame, and base_link_frame are map, odom, and base_link, respectively. The base_link_output_frame parameter defaults to the value of base_link_frame. The world_frame parameter defaults to the value of odom_frame.
odom0For each sensor, users must define this parameter based on the message type. For example, if we define one source of Imu messages and two sources of Odometry messages, the configuration would look like this –





The index for each parameter name is 0-based (for example, odom0, odom1 and so on) and must be defined sequentially (for example, do not use pose0 and pose2 if you have not defined pose1). The values for each parameter are the topic name for that sensor.
odom0_config[boolean] Specific parameters –

~odomN_config
~twistN_config
~imuN_config
~poseN_config

For each of the sensor messages defined above, users must specify which variables of those messages should be fused into the final state estimate. An example odometry configuration might look like this –

[true, true, false,
false, false, true,
true, false, false,
false, false, true,
false, false, false]

The order of the boolean values are X,Y,Z,roll,pitch,yaw,X˙,Y˙,Z˙,roll˙,pitch˙,yaw˙,X¨,Y¨,Z¨. In this example, we are fusing X and Y position, yaw, X˙, and yaw˙.

Note – The specification is done in the frame_id of the sensor, not in the world_frame or base_link_frame. Please see the configuration tutorial GGG for more information.
odom0_differential[boolean] Specific parameters –

~odomN_differential
~imuN_differential
~poseN_differential

For each of the sensor messages defined above that contain pose information, users can specify whether the pose variables should be integrated differentially. If a given value is set to true, then for a measurement at time t from the sensor in question, we first subtract the measurement at time t−1, and convert the resulting value to a velocity. This setting is especially useful if your robot has two sources of absolute pose information, such as yaw measurements from odometry and an IMU. In that case, if the variances on the input sources are not configured correctly, these measurements may become out of sync with one another and cause oscillations in the filter. However, by integrating one or both of them differentially, we avoid this scenario.

Users should take care when using this parameter for orientation data, as the conversion to velocity means that the covariance for orientation state variables will grow without bound (unless another source of absolute orientation data is being fused). If you simply want all of your pose variables to start at 0, then use the _relative parameter.

Note – If you are fusing GPS information via navsat_transform_node or utm_transform_node, you should make sure that the _differential setting is false.
odom0_nodelay[boolean] Specific parameters –

~odomN_nodelay
~twistN_nodelay
~imuN_nodelay
~poseN_nodelay

If true, sets the tcpNoDelay transport hint. There is some evidence that Nagle’s algorithm interferes with the timely reception of large message types, such as the nav_msgs/Odometry message. Setting this to true for an input disables Nagle’s algorithm for that subscriber. Defaults to false.
odom0_queue_size[int] Specific parameters –

~odomN_queue_size
~twistN_queue_size
~imuN_queue_size
~poseN_queue_size

Users can use these parameters to adjust the callback queue sizes for each sensor. This is useful if your frequency parameter value is much lower than your sensor’s frequency, as it allows the filter to incorporate all measurements that arrived in between update cycles.
odom0_relative[boolean] Specific parameters –

~odomN_relative
~imuN_relative
~poseN_relative

If this parameter is set to true, then any measurements from this sensor will be fused relative to the first measurement received from that sensor. This is useful if, for example, you want your state estimate to always start at (0,0,0) and with roll, pitch, and yaw values of (0,0,0). It is similar to the _differential parameter, but instead of removing the measurement at time t−1, we always remove the measurement at time 0, and the measurement is not converted to a velocity.
odom_frame[str] These parameters define the operating “mode” for robot_localization. REP-105 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 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. For example, due to the fusion of GPS data.

Here is how to use these parameters:

Set the map_frame, odom_frame, and base_link_frame parameters to the appropriate frame names for your system.

Note – 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.

Note – If you are running multiple EKF instances and would like to “override” the output transform and message to have this frame for its child_frame_id, you may set this. The base_link_output_frame is optional and will default to the base_link_frame. This helps to enable disconnected TF trees when multiple EKF instances are being run. When the final state is computed, we “override” the output transform and message to have this frame for its child_frame_id.

If you are only 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 the state estimation nodes in robot_localization, and the most common use for it.

If you are fusing global absolute position data that is subject to discrete jumps (such as GPS or position updates from landmark observations) then –

Set your world_frame to your map_frame value.

Make sure something else is generating the odom->base_link transform. This can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data.

The default values for map_frame, odom_frame, and base_link_frame are map, odom, and base_link, respectively. The base_link_output_frame parameter defaults to the value of base_link_frame. The world_frame parameter defaults to the value of odom_frame.
print_diagnostics[boolean] If true, the state estimation node will publish diagnostic messages to the /diagnostics topic. This is useful for debugging your configuration and sensor data.
process_noise_covariance[double] 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.
publish_acceleration[boolean] If true, the state estimation node will publish the linear acceleration state. Defaults to false.
publish_tf[boolean] If true, the state estimation node will publish the transform from the frame specified by the world_frame parameter to its child. If the world_frame is the same as the map_frame it will publish the transform from the map_frame to the odom_frame and if the world_frame is the same as the odom_frame it will publish the transform from the odom_frame to the base_link_frame. Defaults to true.
sensor_timeout[double] The real-valued period, in seconds, after which we consider any 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 inverse of the minimum frequency at which the filter will generate new output.
stamped_control[boolean] If true and use_control is also true, looks for a geometry_msgs/TwistStamped message instead of a geometry_msgs/Twist message.
transform_time_offset[int] Some packages require that your transforms are future-dated by a small time offset. The value of this parameter will be added to the timestamp of map->odom or odom->base_link transform being generated by the state estimation nodes in robot_localization.
transform_timeout[double] The robot_localization package uses tf2’s lookupTransform method to request transformations. This parameter specifies how long we would like to wait if a transformation is not available yet. Defaults to 0 if not set. The value 0 means we just get us the latest available (see tf2 implementation) transform so we are not blocking the filter. Specifying a non-zero transform_timeout affects the filter’s timing since it waits for a maximum of the transform_timeout for a transform to become available. This directly implies that mostly the specified desired output rate is not met since the filter has to wait for transforms when updating.
two_d_mode[boolean] If your robot is operating in a planar environment and you’re comfortable with ignoring the subtle variations in the ground (as reported by an IMU), then set this to true. It will fuse 0 values for all 3D variables (Z, roll, pitch, and their respective velocities and accelerations). This keeps the covariances for those values from exploding, while ensuring that your robot’s state estimate remains affixed to the X-Y plane.
use_control[boolean] If true, the state estimation node will listen to the cmd_vel topic for a geometry_msgs/Twist message, and use that to generate an acceleration term. This term is then used in the robot’s state prediction. This is especially useful in situations where even small amounts of lag in convergence for a given state variable cause problems in your application (for example, LIDAR shifting during rotations). Defaults to false.

Note – The presence and inclusion of linear acceleration data from an IMU will currently “override” the predicted linear acceleration value.
world_frame[str] These parameters define the operating “mode” for robot_localization. REP-105 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 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. For example, due to the fusion of GPS data.

Here is how to use these parameters –

Set the map_frame, odom_frame, and base_link_frame parameters to the appropriate frame names for your system.

Note – 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.

Note – If you are running multiple EKF instances and would like to “override” the output transform and message to have this frame for its child_frame_id, you may set this. The base_link_output_frame is optional and will default to the base_link_frame. This helps to enable disconnected TF trees when multiple EKF instances are being run. When the final state is computed, we “override” the output transform and message to have this frame for its child_frame_id.

If you are only 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 the state estimation nodes in robot_localization, and the most common use for it.

If you are fusing global absolute position data that is subject to discrete jumps (for example, GPS or position updates from landmark observations) then –

Set your world_frame to your map_frame value.

Make sure something else is generating the odom->base_link transform. This can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data.

The default values for map_frame, odom_frame, and base_link_frame are map, odom, and base_link, respectively. The base_link_output_frame parameter defaults to the value of base_link_frame. The world_frame parameter defaults to the value of odom_frame.

More Info