Skip to content

Localization: Wheel Odometry and IMU (EKF)

DiffBot and Remo estimate how far they drove and turned from their wheels. The wheels' estimate of turning is the weak part. A wheel that slips, or a wheel separation \(b\) that is a little off in the configuration, makes the estimated turn too small or too large. The wheel separation is the distance between the two drive wheels. The error in the heading then grows into an error in the position, the further the robot drives.

The estimate from the wheels is the odometry. The drive controller computes the odometry from the wheel encoders' angles. The odometry has two parts:

  • A pose in the odom frame: the position \(x\), \(y\) and the heading \(\theta\).
  • A velocity in the robot's own frame, base_footprint: the forward speed \(v_x\) and the yaw rate \(\omega_z\), the angular velocity about the upward axis.

The figure shows where the two frames sit on the floor, seen from above. A frame is a coordinate system with a name:

  • The odom frame: stays at one place. After a fresh start, its origin is where the robot stood when the odometry started.
  • The frame base_footprint: moves with the robot. Its origin lies on the floor below the middle of the wheel axle. Its x axis points forward and its y axis to the left, as REP 103's axis orientation says for a robot's body.

The position \(x\), \(y\) says where base_footprint lies along the two axes of odom. The heading \(\theta\) is the angle from the x axis of odom to the robot's forward direction. The transform odom → base_footprint holds this pose.

The floor seen from above. The odom frame has its origin where the robot started, with its x axis in the direction the robot faced. The robot has driven a left curve. Its frame base_footprint lies at x and y in the odom frame and is turned by the heading θ The floor from above the robot at the start x y odom odom → base_footprint θ base_footprint
In this setup, after a fresh start, the origin of odom is where the robot's frame base_footprint was at the start, with its x axis in the direction that the robot faced. The pose (x, y, θ) is the robot's estimated position and heading in odom. The figure leaves the drift out.

REP 105's odom frame is ROS's convention for such a frame: the robot's pose in the odom frame changes smoothly, without jumps, but drifts over time. The odom frame itself is fixed in the world. What drifts is the estimate: the odometry's small errors add up, so the estimated pose moves away from the robot's true pose. During navigation on a map, amcl, the node that estimates the robot's pose on a known map, adjusts the transform map → odom to make up for this error. map is the frame of that map. The EKF alone doesn't publish map → odom. REP 105's section on the relationship between the frames has a diagram of how the frames map and odom and the robot's base frame are attached to each other.

The IMU's gyroscope measures the turn directly. The gyroscope's yaw rate \(\omega_z\) doesn't depend on the wheels.

With ekf:=true, an extended Kalman filter (EKF) from the robot_localization package combines both sensors:

  • The forward speed \(v_x\): the filter takes the forward speed from the wheels.
  • The yaw rate \(\omega_z\): the filter takes the yaw rate mostly from the gyroscope, because the gyroscope's variance \(\sigma^2\) is far smaller than the wheels'. A variance says how far a value's errors spread. The smaller the variance, the more the filter trusts the value.

The filter publishes its estimate in two forms, which navigation and RViz use:

  • on the topic /diffbot/odometry/filtered
  • as the transform odom → base_footprint on the topic /tf

Not tested on the robot yet

The EKF runs in the simulation and in the tests that How it's tested lists. On Remo, the IMU's rate, axes and noise passed the bench checks, but the EKF itself hasn't run on a robot yet. Remo's URDF model in remo_description includes the IMU's position and orientation as the same bench checks confirmed them. DiffBot uses the same BNO055, but its mounting and the firmware still need checking on DiffBot. The variances that the firmware and the drive controller publish on the robot, and the IMU sample validator's time limits, are starting values, not measurements.

Reading the code on this page
  • The excerpts on this page show fixed revisions: the diffbot repository at one commit, robot_localization 2.7.7, ros_controllers 0.22.0, gazebo_ros_pkgs 2.9.3, the navigation stack 1.17.3 and ROS's common_msgs at one commit. The line numbers are those of the original files.
  • The link in the sentence before an excerpt opens that revision and those lines on GitHub.
  • Where a section names a function, a class, a member or a setting of the code, the name links to its line at that revision or to the section of this page that shows it, or the section shows the name in an excerpt. A file name links to the file, for the diffbot repository to its current version.

What goes in and what comes out

Measurements go into the filter, and an estimate of the robot's motion comes out. Three terms and their symbols describe this. The figures and the formulas on this page use them:

  • The state \(\mathbf{x}\): the values that describe the robot's motion, such as its position, its heading and its velocities. These are the values that the filter is to find out. The filter's state lists all 15.
  • A measurement \(\mathbf{z}\): the values that one message of a sensor delivers. The index names the sensor: \(\mathbf{z}_\text{wheels}\) is a measurement of the wheel odometry, and \(\mathbf{z}_\text{IMU}\) is a measurement of the IMU.
  • The estimate \(\hat{\mathbf{x}}\): the filter can't know the true state \(\mathbf{x}\). From the measurements, the filter computes its best guess of the state. The hat over the letter marks an estimated value.

A bold letter is a vector: the letter stands for several values at once.

The figure shows the way of the values as a loop, in two variants. The tab Real robot shows the loop on the robot. The tab Simulation shows the same loop with Gazebo in the place of the robot's hardware. From the drive controller and the IMU sample validator on, both tabs show the same parts and topics at the same places. The topics' names in the figure leave out their namespace /diffbot, so imu/data_raw stands for the topic /diffbot/imu/data_raw.

Read the figure from the robot at the top down to the filter and to RViz, and then along the dashed arrows back up to the robot:

  1. The sensors: a blue dot marks where the wheels' angles come from, and an orange square marks the IMU. \(v_x\) and \(\omega_z\) at the robot show the directions of the forward speed and of the yaw rate.
  2. The way to the filter: on the real robot, the firmware on the Teensy reads both sensors. In the simulation, Gazebo computes the wheel joints' angles and the IMU sensor's values, and two Gazebo plugins pass them on. A plugin is code that Gazebo loads and runs as a part of itself. The drive controller turns the wheel angles into the wheel odometry, the measurement \(\mathbf{z}_\text{wheels}\). The IMU sample validator forwards the IMU's valid samples, the measurement \(\mathbf{z}_\text{IMU}\). The label at an arrow names the topic that carries the values.
  3. The filter: the EKF fuses both measurements into its estimate \(\hat{\mathbf{x}}\).
  4. The two outputs: the EKF publishes the pose and the velocities of its estimate on a topic, and the pose as a transform. Both arrows end in a sketch of RViz with odom as its Fixed Frame. RViz can show the same two outputs for the real robot and for the simulation. RViz's RobotModel display places the robot where the transform puts it, and an Odometry display keeps arrows at poses of the topic's messages. The dashed line of the transform, the letters at the axes and the heading angle \(\theta\) are added in the sketch to explain the pose. RViz doesn't draw them. The sketch shows the same pose as the figure of the odom frame.
  5. The way back to the wheels: the dashed gray arrows show the way of the velocity commands. A velocity command says how fast the robot is to drive forward and how fast the robot is to turn: the target forward speed \(v_x^*\) and the target yaw rate \(\omega_z^*\). The star marks a target value. A command comes from a teleop node, with which a person steers the robot, or from the planner in move_base, the node that computes the commands that lead the robot to a navigation goal. The planner reads the EKF's two outputs, which the thin green arrow at the figure's right edge shows. The drive controller turns each command into two target wheel speeds, the angular velocities \(\omega_\text{l}^*\) for the left wheel and \(\omega_\text{r}^*\) for the right wheel. On the real robot, the firmware on the Teensy drives the motors so that the wheels reach these speeds. In the simulation, the Gazebo plugin gazebo_ros_control applies a torque \(\tau\) to each wheel joint. A torque is the turning effect of a force about an axis, here the joint's axis. The robot moves, the sensors measure the motion, and the loop starts again.
The loop on the real robot: the wheel encoders and the IMU, the firmware on the Teensy, the drive controller and the IMU sample validator, the EKF, its two outputs in a sketch of RViz, and the velocity commands that go back to the motors vx ωz wheel encoder IMU (BNO055) to the motors Teensy: firmware, over USB to rosserial ↓ wheel angles measured_joint_states ↑ target wheel speeds wheel_cmd_velocities IMU samples imu/data_raw drive controller mobile_base_controller in the node diffbot_base IMU sample validator imu_validator zwheels = (vx, vy, ωz) mobile_base_controller /odom zIMU = (ωz) imu/data_validated EKF, the node ekf_localization x: the filter's estimate of the state x xpublished = (x, y, θ, vx, vy, ωz) odometry/filtered with covariances (x, y, θ) as the transform odom → base_footprint RViz, fixed frame odom x y odom odom → base_footprint θ Odometry display: kept pose arrows a teleop node the planner in move_base ↑ velocity commands mobile_base_controller/cmd_vel
  • from the wheels
  • from the IMU
  • the filter's estimate
  • commands, back to the wheels
The loop on the real robot. The robot is drawn from above and not to scale.
The loop in the simulation: Gazebo's wheel joints and its IMU sensor, Gazebo's plugins, the drive controller, the adapter and the IMU sample validator, the EKF, its two outputs in a sketch of RViz, and the velocity commands that go back to the wheel joints Gazebo vx ωz wheel joint IMU sensor joint torques gazebo_ros_control and the IMU sensor's plugin ↓ wheel joints' angles ↑ target wheel speeds inside Gazebo, without a topic imu/gazebo imu_raw_adapter imu/data_raw drive controller mobile_base_controller inside gazebo_ros_control IMU sample validator imu_validator zwheels = (vx, vy, ωz) mobile_base_controller /odom zIMU = (ωz) imu/data_validated EKF, the node ekf_localization x: the filter's estimate of the state x xpublished = (x, y, θ, vx, vy, ωz) odometry/filtered with covariances (x, y, θ) as the transform odom → base_footprint RViz, fixed frame odom x y odom odom → base_footprint θ Odometry display: kept pose arrows a teleop node the planner in move_base ↑ velocity commands mobile_base_controller/cmd_vel
  • from the wheels
  • from the IMU
  • the filter's estimate
  • commands, back to the wheels
The loop in the simulation. Gazebo takes the place of the robot's hardware, and the rest of the loop is the same.

How it fits together goes through the nodes and the topics of the figure one by one.

The formulas below write the two measurements and the published estimate as vectors, with the symbols of the figure and of the filter's state. The position \(x\), \(y\) is in m, the heading \(\theta\) in rad, the speeds \(v_x\), \(v_y\) in m/s and the yaw rate \(\omega_z\) in rad/s.

The wheel odometry's measurement is the three velocities that the filter fuses from one odometry message:

\[ \mathbf{z}_\text{wheels} = (\,v_x,\ v_y,\ \omega_z\,) \]

The IMU's measurement is the yaw rate of one validated sample:

\[ \mathbf{z}_\text{IMU} = (\,\omega_z\,) \]

The topic carries the estimated pose in the odom frame and the estimated velocities in the frame base_footprint. With two_d_mode, the height, roll, pitch and their velocities stay at 0. The message stores the orientation as a quaternion, which here holds the heading \(\theta\). The six values that can differ from 0 form this vector:

\[ \hat{\mathbf{x}}_\text{published} = (\,x,\ y,\ \theta,\ v_x,\ v_y,\ \omega_z\,) \]

The list says more about each part:

  • In, on DiffBot and Remo: two measurements. The wheel odometry gives the forward speed \(v_x\) and the yaw rate \(\omega_z\) that the drive controller computes from the wheel encoders, and the sideways speed \(v_y\), which the controller sets to 0 for a differential drive. The IMU gives the yaw rate \(\omega_z\) that its gyroscope measures. What the filter fuses says why the filter takes these values and no others.
  • In, possible but not fed here: robot_localization can fuse any number of inputs of four kinds, which are odometry, poses, velocities and IMUs. robot_localization's page on preparing the sensor data says what each kind of message has to contain. Examples of such inputs are a GPS position \(x\), \(y\), a pose \(x\), \(y\), \(\theta\) from a camera, a second IMU, and more odometry, for example visual odometry. A GPS position needs a conversion first, which robot_localization's page on integrating GPS data describes with the node navsat_transform_node. On DiffBot and Remo, the filter gets none of these.
  • The estimate: robot_localization's state has 15 values. They are the position \(x\), \(y\), \(z\), the orientation as roll, pitch and the heading \(\theta\), the linear velocity \(v_x\), \(v_y\), \(v_z\), the angular velocity \(\omega_x\), \(\omega_y\), \(\omega_z\) and the linear acceleration \(a_x\), \(a_y\), \(a_z\). two_d_mode keeps the height \(z\), roll, pitch, their velocities \(v_z\), \(\omega_x\), \(\omega_y\) and the vertical acceleration \(a_z\) at 0, for a robot on a flat floor. Of what remains, the topic's message carries \(x\), \(y\), \(\theta\), \(v_x\), \(v_y\) and \(\omega_z\). The accelerations \(a_x\) and \(a_y\) stay inside the filter.
  • Out: the filter publishes its estimate in two forms, because ROS nodes get a robot's motion in two ways. The two outputs explains both forms and names the nodes that read each.

The two outputs

  • The topic /diffbot/odometry/filtered: carries the pose and the velocities of the filter's estimate, with their covariances, as a nav_msgs/Odometry message. The message has the pose \(x\), \(y\), \(\theta\) in the odom frame and the velocities \(v_x\), \(v_y\), \(\omega_z\) in the robot's frame base_footprint. The message has no field for the accelerations of the filter's state. A node that needs the velocities or the uncertainty subscribes to this topic.
  • The transform odom → base_footprint on the topic /tf: carries only the pose, as a translation and a rotation. A transform has no velocity and no covariance. TF, ROS's transform library, joins this transform with the robot's other transforms. So a node can ask TF where any frame of the robot is in the odom frame, without knowing which node estimated the pose.

The two forms hold the same pose. Each time the filter publishes, the filter builds the odometry message filteredPosition from its state and copies the message's pose into the transform worldBaseLinkTransMsg_, as lines 1955–1965 of ros_filter.cpp in robot_localization 2.7.7 show:

ros_filter.cpp, lines 1955–1965: the transform is a copy of the message's pose
1955
1956
1957
1958
1959
1960
1961
1962
1963
1964
1965
if (getFilteredOdometryMessage(filteredPosition))
{
  worldBaseLinkTransMsg_.transform = tf2::toMsg(tf2::Transform::getIdentity());
  worldBaseLinkTransMsg_.header.stamp = filteredPosition.header.stamp + tfTimeOffset_;
  worldBaseLinkTransMsg_.header.frame_id = filteredPosition.header.frame_id;
  worldBaseLinkTransMsg_.child_frame_id = filteredPosition.child_frame_id;

  worldBaseLinkTransMsg_.transform.translation.x = filteredPosition.pose.pose.position.x;
  worldBaseLinkTransMsg_.transform.translation.y = filteredPosition.pose.pose.position.y;
  worldBaseLinkTransMsg_.transform.translation.z = filteredPosition.pose.pose.position.z;
  worldBaseLinkTransMsg_.transform.rotation = filteredPosition.pose.pose.orientation;

The ROS navigation stack needs the odometry in both forms. With ekf:=true, the EKF supplies both. Two sources say what is needed:

  • The transform: REP 105 says in its section on frame authorities that one of the odometry sources computes and broadcasts the transform from odom to the robot's base frame. So only one node may publish this transform, here the EKF.
  • Both: the navigation stack's guide on setting up a robot says that the navigation stack requires the odometry both through TF and as a nav_msgs/Odometry message.

The following nodes read the two outputs when ekf:=true:

  • The topic: the local planner that move_base and Move Base Flex load, by default DWAPlannerROS, reads the robot's current velocities from the topic /diffbot/odometry/filtered: \(v_x\), \(v_y\) and \(\omega_z\), the message's fields twist.twist.linear.x, twist.twist.linear.y and twist.twist.angular.z. The planners' odometry topic shows the launch files' lines that select this topic. RViz can show the topic's poses with an Odometry display, as in Comparing the two odometries.
  • The transform: everything that asks TF where the robot is. RViz places the robot in its view with the transform. amcl, which localizes the robot on a map, asks TF for the robot's pose in the odom frame. The planners' costmaps ask TF for the robot's pose in their own frames, the local costmap in odom and the global costmap in map. Each of these requests passes through the transform odom → base_footprint.

The planner's subscriber is the function odomCallback() of the class OdometryHelperRos, which copies the three velocities and child_frame_id from each message, as lines 48–59 of odometry_helper_ros.cpp in the navigation stack 1.17.3 show:

odometry_helper_ros.cpp, lines 48–59: the velocities that the local planner reads
48
49
50
51
52
53
54
55
56
57
58
59
void OdometryHelperRos::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    ROS_INFO_ONCE("odom received!");

  //we assume that the odometry is published in the frame of the base
  boost::mutex::scoped_lock lock(odom_mutex_);
  base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
  base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
  base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
  base_odom_.child_frame_id = msg->child_frame_id;
//  ROS_DEBUG_NAMED("dwa_local_planner", "In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
//      base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
}

How it fits together

This section goes through the nodes and the topics of the loop figure, in the figure's order, and says where the real robot and the simulation differ. On the way to the filter, the data takes two branches that meet at the EKF. The wheels' angles pass the drive controller, step 2 of the list. The IMU's samples pass the IMU sample validator, step 3. The sensors', the odometry's and the commands' topics are in the /diffbot namespace. The transform is published on the global topic /tf.

  1. The source of the sensors' values: the real robot and the simulation differ here.
    • Real robot: the firmware on the Teensy reads the wheel encoders and the BNO055. rosserial, the connection between the Teensy and the robot's single-board computer (SBC), turns what the firmware sends into ROS topics. The firmware publishes the wheels' angles and the wheels' angular velocities \(\omega_\text{l}\) and \(\omega_\text{r}\), in rad/s, on the topic /diffbot/measured_joint_states. The firmware publishes the IMU's samples on the topic /diffbot/imu/data_raw at 50 Hz, as IMU: Bosch BNO055 explains.
    • Simulation: Gazebo computes the wheel joints' angles and the IMU sensor's values. The Gazebo plugin gazebo_ros_control reads the wheel joints' angles inside Gazebo, without a topic. The IMU sensor's plugin publishes the topic /diffbot/imu/gazebo, and the small adapter imu_raw_adapter.py republishes its samples on the topic /diffbot/imu/data_raw, as the Simulation page's IMU section explains.
  2. The wheels' branch, the drive controller: mobile_base_controller, a diff_drive_controller, computes the wheel odometry from the wheels' angles and publishes the wheel odometry on the topic /diffbot/mobile_base_controller/odom. On the real robot, the drive controller runs in the node diffbot_base, whose hardware interface takes the wheels' angles from the topic /diffbot/measured_joint_states. In the simulation, the drive controller runs inside Gazebo, in gazebo_ros_control. With the EKF on, the drive controller doesn't publish the transform odom → base_footprint. Two nodes that publish the same transform would make the robot's pose jump between their two estimates.
  3. The IMU's branch, the IMU sample validator: the node imu_validator.py subscribes to the topic /diffbot/imu/data_raw. The validator checks each sample against what this EKF configuration needs and forwards the valid samples, unchanged, on the topic /diffbot/imu/data_validated. The IMU sample validator lists its checks.
  4. The EKF: the node /diffbot/ekf_localization subscribes to the wheel odometry on the topic /diffbot/mobile_base_controller/odom and to the validated samples on the topic /diffbot/imu/data_validated. The EKF fuses the two. The EKF publishes the filtered odometry on the topic /diffbot/odometry/filtered and the transform odom → base_footprint on the topic /tf.
  5. The readers of the two outputs: RViz can show both outputs, as in the figure's sketch, for the real robot and for the simulation alike. For the topic, RViz needs an Odometry display, which Comparing the two odometries sets up. The planner in move_base or in Move Base Flex reads both outputs too. The two outputs names every node that reads them.
  6. The velocity commands: a node that wants the robot to move publishes geometry_msgs/Twist messages on the topic /diffbot/mobile_base_controller/cmd_vel. The drive controller subscribes to this topic and uses two values of a command: linear.x, the target forward speed \(v_x^*\) in m/s, and angular.z, the target yaw rate \(\omega_z^*\) in rad/s. The launch files of the diffbot repository start three kinds of nodes that publish commands:

    • A teleop node for the keyboard: keyboard_teleop.launch starts teleop_twist_keyboard, which turns key presses into commands.
    • A teleop window: in the simulation, diffbot_control.launch starts rqt_robot_steering, a window with sliders to drive the robot.
    • The planner: move_base and Move Base Flex compute the commands that lead the robot to a goal. With ekf:=true, the planner reads the EKF's two outputs, so the EKF's estimate is part of the loop. The planners' odometry topic shows the lines that connect the planner to both topics.

    The EKF doesn't read the velocity commands. robot_localization can use the commands in its prediction with the setting use_control, whose default is false, as robot_localization's description of use_control says. ekf.yaml doesn't set use_control, so the figure has no arrow from the commands to the EKF.

  7. The way back to the wheels: the drive controller first limits the command's velocities and accelerations to the limits in diffbot_control.yaml. From the limited command, the drive controller computes two target wheel speeds in rad/s, the angular velocities \(\omega_\text{l}^*\) for the left wheel and \(\omega_\text{r}^*\) for the right wheel:

    \[ \omega_\text{l}^* = \frac{v_x^* - \omega_z^* \, b / 2}{r}, \qquad \omega_\text{r}^* = \frac{v_x^* + \omega_z^* \, b / 2}{r} \]

    \(b\) is the wheel separation and \(r\) is the wheel radius, both in m. The real robot and the simulation differ in what happens to the two targets.

    • Real robot: the hardware interface in the node diffbot_base publishes the two targets on the topic /diffbot/wheel_cmd_velocities. The firmware on the Teensy receives them through rosserial. For each motor, a proportional–integral–derivative (PID) controller in the firmware compares the wheel's target with the wheel's angular velocity that the encoder measures, \(\omega_\text{l}\) or \(\omega_\text{r}\), and sets the motor driver's output for this motor. Low-Level PID Approach explains the firmware.
    • Simulation: gazebo_ros_control gets the two targets inside Gazebo, without a topic. For each wheel joint, a PID controller of gazebo_ros_control compares the target with the joint's angular velocity and applies a torque \(\tau\) in N m to the joint. Turning a wheel-speed command into torque explains this PID controller. The controller's gains are in the robot's pid.yaml, for Remo diffbot_control/config/remo/pid.yaml.

    The wheels turn, the robot moves, and the sensors of step 1 measure the motion.

The collapsed box shows the code of step 7, from a velocity command to the wheels.

The way back in the code: from a velocity command to the wheels

The table lists the quantities of step 7 with their names in the code of the four excerpts:

Symbol Unit Quantity Names in the code
\(v_x^*\) m/s target forward speed, after the limits curr_cmd.lin
\(\omega_z^*\) rad/s target yaw rate, after the limits curr_cmd.ang
\(b\) m wheel separation ws
\(r\) m wheel radius lwr for the left wheel, rwr for the right wheel
\(\omega_\text{l}^*\), \(\omega_\text{r}^*\) rad/s target wheel speeds vel_left and vel_right, joint_velocity_commands_, wheel_cmd_velocity_left_ and wheel_cmd_velocity_right_, joint_velocity_command_
\(\omega_\text{l}\), \(\omega_\text{r}\) rad/s measured wheel speeds angular_velocity_ of joint_state_left_ and joint_state_right_, joint_velocity_
\(\tau\) N m torque on a wheel joint effort

The drive controller's function update() computes vel_left and vel_right, the two target wheel speeds, from curr_cmd.lin and curr_cmd.ang, the limited command, the wheel separation ws and the wheel radii lwr and rwr. setCommand() hands each target to the hardware interface, as lines 491–500 of diff_drive_controller.cpp in ros_controllers 0.22.0 show:

491
492
493
494
495
496
497
498
499
500
// Compute wheels velocities:
const double vel_left  = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/lwr;
const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/rwr;

// Set wheels velocities:
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
  left_wheel_joints_[i].setCommand(vel_left);
  right_wheel_joints_[i].setCommand(vel_right);
}

On the real robot, the control loop of the node diffbot_base calls the hardware interface's function DiffBotHWInterface::write() after each update() of the drive controller. write() first scales the left target by (gain_ - trim_) / motor_constant_ and the right target by (gain_ + trim_) / motor_constant_. Both factors are 1 with the settings gain, trim and motor_constant in base.yaml. write() then copies the two targets from joint_velocity_commands_ into the message wheel_cmd_msg and publishes the message, as lines 169–186 of diffbot_hw_interface.cpp show:

169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
// adjusting k by gain and trim
double motor_constant_right_inv = (gain_ + trim_) / motor_constant_;
double motor_constant_left_inv = (gain_ - trim_) / motor_constant_;


joint_velocity_commands_[0] = joint_velocity_commands_[0] * motor_constant_left_inv;
joint_velocity_commands_[1] = joint_velocity_commands_[1] * motor_constant_right_inv;


// Publish the desired (commanded) angular wheel joint velocities
diffbot_msgs::WheelsCmdStamped wheel_cmd_msg;
wheel_cmd_msg.header.stamp = ros::Time::now();
for (int i = 0; i < NUM_JOINTS; ++i)
{
    wheel_cmd_msg.wheels_cmd.angular_velocities.joint.push_back(joint_velocity_commands_[i]);
}

pub_wheel_cmd_velocities_.publish(wheel_cmd_msg);

In the firmware, the function WheelControl::step() calls BaseController::read() on each control step. WheelControl::step() then calls BaseController::write(), unless the last command is older than the command timeout. BaseController::write() computes the two motor outputs motor_cmd_left_ and motor_cmd_right_ with the PID controllers motor_pid_left_ and motor_pid_right_. Each PID controller gets its wheel's target, wheel_cmd_velocity_left_ or wheel_cmd_velocity_right_, and the wheel's measured angular_velocity_. setSpeed() passes each output to the motor driver, as lines 484–488 of base_controller.h show:

484
485
486
487
488
motor_cmd_left_ = motor_pid_left_.compute(wheel_cmd_velocity_left_, joint_state_left_.angular_velocity_);
motor_cmd_right_ = motor_pid_right_.compute(wheel_cmd_velocity_right_, joint_state_right_.angular_velocity_);

p_motor_controller_left_->setSpeed(motor_cmd_left_);
p_motor_controller_right_->setSpeed(motor_cmd_right_);

In the simulation, the update callback of gazebo_ros_control, GazeboRosControlPlugin::Update(), calls DefaultRobotHWSim::readSim(), then the controllers' update() and then DefaultRobotHWSim::writeSim(). writeSim() computes the error between the target joint_velocity_command_ and the joint's velocity joint_velocity_. The PID controller pid_controllers_ turns the error into the torque effort, which clamp() keeps within the joint's effort limit. SetForce() applies the torque to Gazebo's joint, as lines 370–380 of default_robot_hw_sim.cpp in gazebo_ros_pkgs 2.9.3 show:

370
371
372
373
374
375
376
377
378
379
380
case VELOCITY_PID:
  double error;
  if (e_stop_active_)
    error = -joint_velocity_[j];
  else
    error = joint_velocity_command_[j] - joint_velocity_[j];
  const double effort_limit = joint_effort_limits_[j];
  const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
                              -effort_limit, effort_limit);
  sim_joints_[j]->SetForce(0, effort);
  break;

ekf.launch in diffbot_control starts the validator and the EKF, for the robot and the simulation alike.

Running it

The ekf argument switches the EKF on. The EKF is off by default. Pass ekf:=true to the launch file that you start:

Where Command
On the robot, when you start the bringup yourself roslaunch diffbot_bringup bringup.launch model:=remo ekf:=true (or bringup_with_laser.launch)
In the simulation roslaunch diffbot_gazebo diffbot.launch model:=remo ekf:=true
In the simulation, with an RViz window roslaunch diffbot_control diffbot.launch model:=remo ekf:=true
Simulation with navigation roslaunch diffbot_navigation diffbot.launch model:=remo ekf:=true
Navigation on the development PC, for a robot that runs with the EKF roslaunch diffbot_navigation diffbot_hw.launch model:=remo ekf:=true

The five commands start these launch files: diffbot_bringup/launch/bringup.launch, diffbot_gazebo/launch/diffbot.launch, diffbot_control/launch/diffbot.launch, diffbot_navigation/launch/diffbot.launch and diffbot_navigation/launch/diffbot_hw.launch.

The second and the third command start the same simulation: Gazebo with the robot, the robot's controllers, the IMU sample validator and the EKF. The third command also starts RViz. What RViz shows with the EKF describes the RViz window.

Which launch file starts what

A roslaunch command names one launch file. roslaunch reads that file and every file that it includes with the <include> tag, directly or through another included file. roslaunch starts the nodes that these files declare. A node is a running ROS program. A file can pass arguments such as ekf to a file that it includes. Starting an included file on its own doesn't read the file that includes it.

The two figures show which launch file includes which, and which file declares which node, one figure for the real robot and one for the simulation. The figures show only the launch files and the nodes that matter for the EKF. The same command reads more files and starts more nodes, and the text below each figure names them. The figures don't show the order in which the nodes start.

Read the figures as follows:

  • The pill-shaped box: your command.
  • A rectangle: a launch file, written as its package and its file name.
  • A rounded rectangle: the node or the two nodes that a file starts.
  • A thick arrow: an include that passes ekf to the included file.
  • The node with the thick orange border: robot_state_publisher, which the EKF needs.
  • The badge above a figure: says whether the figure is for the real robot or for the simulation.

On the real robot

When you start the bringup yourself, you start one of the two bringup files. This command is for Remo. For DiffBot, use model:=diffbot:

1
roslaunch diffbot_bringup bringup.launch model:=remo ekf:=true

With the robot container

In the robot container, Docker Compose starts bringup.launch for you, without ekf, as the bringup service's command in compose.yaml says. So robot_state_publisher runs there and the EKF is off. The container's settings have no switch for the EKF. The issue Robot container: a setting that starts the bringup with the EKF (diffbot#202) asks for such a setting. Don't start a second bringup by hand next to the container's.

The figure shows which files this command reads on the real robot, bringup.launch, minimal.launch, diffbot_base/launch/diffbot.launch and ekf.launch, and which nodes they start:

Real robot

graph TB COMMAND([your roslaunch command<br/>with ekf:=true]) BRINGUP[diffbot_bringup:<br/>bringup.launch] MINIMAL[diffbot_bringup:<br/>minimal.launch] BASE[diffbot_base:<br/>diffbot.launch] EKF_LAUNCH[diffbot_control:<br/>ekf.launch] STATE_PUBLISHER(robot_state_publisher) EKF_NODES(imu_validator and<br/>ekf_localization) COMMAND -->|reads| BRINGUP BRINGUP ==>|includes,<br/>passes ekf| MINIMAL BRINGUP -.->|starts| STATE_PUBLISHER MINIMAL ==>|includes,<br/>passes ekf| BASE BASE -->|includes<br/>if ekf is true| EKF_LAUNCH EKF_LAUNCH -.->|starts| EKF_NODES class STATE_PUBLISHER view;

This command also starts the following nodes, which the figure leaves out:

minimal.launch takes the ekf argument because the bringup files pass ekf through minimal.launch. Starting minimal.launch on its own with ekf:=true doesn't read the bringup file, so nothing starts robot_state_publisher. Without another publisher that provides the transform between imu_link and base_footprint, the EKF can't use the IMU's samples, and the EKF's estimate follows the wheels.

In the simulation

In the simulation, you start one of two launch files that are both named diffbot.launch. diffbot_gazebo/launch/diffbot.launch starts the simulation. diffbot_control/launch/diffbot.launch starts the same simulation and also RViz. This command starts the file in diffbot_gazebo:

1
roslaunch diffbot_gazebo diffbot.launch model:=remo ekf:=true

The figure shows which files this command reads in the simulation, diffbot_gazebo/launch/diffbot.launch, diffbot_control.launch and ekf.launch, and which nodes they start:

Simulation

graph TB COMMAND([your roslaunch command<br/>with ekf:=true]) SIMULATION[diffbot_gazebo:<br/>diffbot.launch] CONTROL[diffbot_control:<br/>diffbot_control.launch] EKF_LAUNCH[diffbot_control:<br/>ekf.launch] STATE_PUBLISHER(robot_state_publisher) EKF_NODES(imu_validator and<br/>ekf_localization) COMMAND -->|reads| SIMULATION SIMULATION ==>|includes,<br/>passes ekf| CONTROL CONTROL -->|includes<br/>if ekf is true| EKF_LAUNCH CONTROL -.->|starts| STATE_PUBLISHER EKF_LAUNCH -.->|starts| EKF_NODES class STATE_PUBLISHER view;

This command also reads one more file, which the figure leaves out. diffbot_gazebo/launch/diffbot.launch also includes diffbot_view.launch. That file starts Gazebo through gazebo_ros's empty_world.launch, the adapter imu_raw_adapter.py and the node that puts the robot into Gazebo. Gazebo runs the drive controller.

diffbot_control/launch/diffbot.launch includes the same two files, diffbot_control.launch and diffbot_view.launch, and passes ekf to diffbot_control.launch in the same way. So the figure also holds for the following command, with diffbot_control: diffbot.launch in the place of diffbot_gazebo: diffbot.launch:

1
roslaunch diffbot_control diffbot.launch model:=remo ekf:=true

diffbot_control/launch/diffbot.launch also starts RViz. What RViz shows with the EKF describes the RViz window.

What the launch files do with ekf

The file at the end of the thick arrows uses the ekf value. On the real robot, that file is diffbot_base/launch/diffbot.launch. In the simulation, that file is diffbot_control.launch. Each of these two launch files loads the drive controller's configuration and then does two things with its ekf value, in two lines. The excerpt shows the two lines, from diffbot_base/launch/diffbot.launch, lines 45–46. diffbot_control.launch has the same two lines:

diffbot_base/launch/diffbot.launch: the two lines that use ekf
45
46
<param name="/diffbot/mobile_base_controller/enable_odom_tf" type="bool" value="$(eval not arg('ekf'))"/>
<include if="$(arg ekf)" file="$(find diffbot_control)/launch/ekf.launch"/>
  1. Line 45 sets the drive controller's parameter enable_odom_tf: the parameter says whether the drive controller publishes the transform odom → base_footprint. The line's value stands at the line's end: $(eval not arg('ekf')), which turns the value of ekf around. So the line writes false with the EKF and true without the EKF. With the EKF, the EKF publishes the transform, and the drive controller must not.
  2. Line 46 includes ekf.launch: the file starts the validator and the EKF. The line includes the file only if ekf is true.

Line 45 writes the parameter in both cases, also the true, for this reason: a parameter stays on the ROS parameter server as long as the ROS master runs, also after the launch that wrote the parameter has ended. Suppose that the line wrote only the false, and only with the EKF. Then this sequence would go wrong:

  1. You start the robot with ekf:=true. The line writes enable_odom_tf: false.
  2. You stop this launch. The ROS master keeps running, for example because you started roscore on its own. The false stays on the parameter server.
  3. You start the robot again, without the EKF. Nothing writes the parameter, so the drive controller reads the old false and doesn't publish the transform. No EKF runs either. Nothing publishes odom → base_footprint.

Because line 45 writes true without the EKF, every start sets the value that fits.

robot_state_publisher, the node with the thick orange border in the figures, publishes the transforms of the joints in the robot's URDF model. From these transforms, TF gets the transform between the IMU's frame imu_link and base_footprint. The EKF needs this transform to turn the IMU's yaw rate \(\omega_z\) into the robot's frame.

The planners' odometry topic

The navigation launch files pass ekf on in the same way. diffbot_navigation/launch/diffbot.launch includes diffbot_gazebo's diffbot.launch and move_base.launch and passes ekf to both.

move_base.launch starts no EKF. The file uses its ekf argument to choose the odometry topic that the planner reads. Line 8 computes the default of the argument odom_topic from ekf: /diffbot/odometry/filtered with the EKF, and the wheel odometry's topic /diffbot/mobile_base_controller/odom without the EKF. The excerpt shows the two arguments in lines 7 and 8, and above them the argument cmd_vel_topic for the topic of the planner's velocity commands, from move_base.launch, lines 6–8:

move_base.launch: the odometry topic follows ekf
6
7
8
<arg name="cmd_vel_topic" default="/diffbot/mobile_base_controller/cmd_vel" />
<arg name="ekf" default="false" doc="the robot runs with the EKF (ekf:=true): read its filtered odometry"/>
<arg name="odom_topic" default="$(eval '/diffbot/odometry/filtered' if arg('ekf') else '/diffbot/mobile_base_controller/odom')" />

The planner subscribes to a topic named odom. Line 28 remaps this name to the topic of odom_topic, as the excerpt of the node's two remappings shows, from move_base.launch, lines 27–28:

move_base.launch: the planner's odom is the chosen topic
27
28
<remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>

Line 27 connects the planner's output in the same way. The planner publishes its velocity commands on a topic named cmd_vel. Line 27 remaps this name to the topic of cmd_vel_topic, /diffbot/mobile_base_controller/cmd_vel, to which the drive controller subscribes. The loop figure shows both connections.

So with ekf:=true, the planner's velocities and the transform come from the same estimate. Two more files work in the same way:

  • Move Base Flex: move_base.launch.xml has the same argument and the same remapping, as the collapsed box shows.
  • Navigation for the real robot: diffbot_hw.launch starts no EKF. Its ekf argument only selects the odometry that the planner reads, for a robot whose bringup runs with ekf:=true.
move_base.launch.xml: the same lines for Move Base Flex

The excerpt shows the three arguments, from move_base.launch.xml, lines 5–7:

5
6
7
<arg name="cmd_vel_topic" default="/diffbot/mobile_base_controller/cmd_vel" />
<arg name="ekf" default="false" doc="the robot runs with the EKF (ekf:=true): read its filtered odometry"/>
<arg name="odom_topic" default="$(eval '/diffbot/odometry/filtered' if arg('ekf') else '/diffbot/mobile_base_controller/odom')" />

The next excerpt shows the two remappings, from move_base.launch.xml, lines 28–29:

28
29
<remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>

What RViz shows with the EKF

roslaunch diffbot_control diffbot.launch model:=remo ekf:=true starts RViz with the configuration diffbot_control/rviz/diffbot.rviz. The configuration sets RViz's Fixed Frame to odom, as the excerpt of its global options shows, from diffbot.rviz, lines 212–216:

diffbot.rviz: the Fixed Frame is odom
212
213
214
215
216
Global Options:
  Background Color: 122; 122; 122
  Default Light: true
  Fixed Frame: odom
  Frame Rate: 30

The configuration has four displays:

  • Grid: a grid with cells of 1 m on the ground plane of the odom frame.
  • RobotModel: the robot's URDF model, which the launch file loads into the parameter /diffbot/robot_description. The display is set to draw the URDF model's visual shapes and not its collision shapes.
  • TF: the odom frame and the frames of the robot's links, as axes with their names.
  • LaserScan: the lidar's scan from the topic /diffbot/scan.

RViz draws the robot's frames and the URDF model at the pose that the transform odom → base_footprint gives. With ekf:=true, the EKF publishes this transform. So the pose of the robot's frames in the RViz window is the EKF's estimate. What the RobotModel display adds to the frames depends on the robot:

  • DiffBot: the display draws DiffBot's body.
  • Remo: with these settings, the window shows Remo's frames and the scan, and no body. STL files on the Remo Description page says which mesh files the public remo_description repository has and how to get Remo's meshes. To see Remo's body without the meshes, tick Collision Enabled in the RobotModel display. The display then draws the collision shapes of Remo's URDF model, as in the picture of Comparing the two odometries.

The configuration has no Odometry display, so the window doesn't draw the messages of the topic /diffbot/odometry/filtered. To see these messages, add an Odometry display in the RViz window:

  1. Click Add in the Displays panel.
  2. Select Odometry on the tab By display type and click OK.
  3. Set the Topic of the new display to /diffbot/odometry/filtered.

RViz then draws arrows at poses of the EKF's odometry messages. With the display's default settings, an arrow is 1.3 m long, far longer than the robot. Comparing the two odometries lists the settings that fit a small robot and adds a second display for the wheel odometry.

Comparing the two odometries

To see what the filter does, compare the two odometries while the robot turns:

  • The wheel odometry: the drive controller's estimate, on the topic /diffbot/mobile_base_controller/odom.
  • The filtered odometry: the EKF's estimate, on the topic /diffbot/odometry/filtered.

You can compare them in two ways:

  • In RViz: set RViz's Fixed Frame to odom. Add one Odometry display for each of the two topics, and give the two displays different colors. Four settings of an Odometry display matter for a small robot. The source of RViz 1.14.26's Odometry display defines their defaults and the rule for adding an arrow:
    • Keep: the number of arrows that the display keeps. The default is 100, and 0 keeps all arrows.
    • Position Tolerance and Angle Tolerance: the display adds a new arrow only when the pose has moved by at least one of the two tolerances since the last arrow. The defaults are 0.1 m and 0.1 rad.
    • Shaft Length, Shaft Radius, Head Length and Head Radius: the size of an arrow. The default arrow is 1.3 m long, far longer than the robot.
    • Covariance: the display also draws each pose's covariance by default. Switch this setting off to see the arrows alone.
  • In a terminal: print the yaw rate \(\omega_z\) of both topics side by side, with rostopic echo /diffbot/mobile_base_controller/odom/twist/twist/angular/z and rostopic echo /diffbot/odometry/filtered/twist/twist/angular/z.

The picture shows such a comparison in RViz, for the simulated Remo with ekf:=true. The robot drove 1.5 s straight, then a left arc of 5 s, then 1.5 s straight again, at 0.2 m/s and with a commanded yaw rate of 0.5 rad/s in the arc. What the picture shows:

  • Blue arrows: the wheel odometry, as an Odometry display with Keep 2000 and a Position Tolerance of 0.045 m.
  • Green arrows: the filtered odometry, as a second Odometry display with Keep 2000 and a Position Tolerance of 0.09 m. The larger tolerance spaces the green arrows more widely along the path. Both displays have an Angle Tolerance of 0.5 rad.
  • White band: the robot's true path in Gazebo. A small script outside the repository republishes Gazebo's topic /gazebo/model_states in the odom frame for the picture.
  • The robot: RViz draws the robot at the pose of the transform odom → base_footprint, which is the EKF's estimate. The robot appears with the collision shapes of Remo's URDF model.
RViz seen from above. The simulated Remo drove a straight piece, a left half circle and a second straight piece. A white band marks the true path. Green arrows, the EKF's estimate, lie on the band up to the robot. Blue arrows, the wheel odometry, leave the band in the curve and end about 0.4 m away from the robot.
RViz from above after the drive, with the Gazebo test's wheel separation in the drive controller. The grid's cells are 0.1 m wide. The legend was added to the screenshot. The run was in a container of the dev container's image, at the commit ead3402 of the diffbot repository.

The two paths differ in this run for a reason. The picture's run has the set-up of the Gazebo test of How it's tested. The drive controller works with a wheel separation of 0.18 m instead of Remo's 0.144 m. The controller then turns the robot 25 % more than its wheel odometry reports. So the wheel odometry's heading falls behind in the arc, and its path leaves the true path. The EKF takes the yaw rate \(\omega_z\) mostly from the gyroscope, which measures the robot's real turn, so the EKF's path stays on the true path. The values at the end of the drive:

  • The wheel odometry: its heading \(\theta\) is 0.62 rad behind the true heading, and its position is 0.40 m away from the true position.
  • The EKF: its heading \(\theta\) differs from the true heading by 0.004 rad, and its position by 2 mm.

With the robot's own wheel separation, the two odometries hardly differ in this drive. In three runs of the same drive in the simulation, the wheel odometry ended within 0.5 mm of the true position, and the EKF's estimate ended within 0.005 rad and 9 mm of the wheel odometry. In these three runs, the EKF's estimate was not closer to the true pose at the end than the wheel odometry. So the picture needs a wheel odometry that is wrong. The wrong wheel separation stands in for an error of the wheel odometry that the gyroscope doesn't share.

What the filter fuses

ekf.yaml configures the filter. For each input, a list of 15 booleans says which values of the input's messages the filter fuses: odom0_config for the wheel odometry and imu0_config for the IMU. The two lists shows both lists.

To fuse means that the filter corrects its estimate \(\hat{\mathbf{x}}\) with a measurement \(\mathbf{z}\). One measurement can hold several values, for example the wheel odometry's three velocities, and the filter uses them together in one correction. Each value counts by its certainty. The filter computes the weights from two uncertainties: the covariance of the measurement, which holds the variance \(\sigma^2\) of each of its values, and the covariance of the filter's own estimate. The smaller a measured value's variance is next to the estimate's uncertainty, the closer the estimate moves to the measured value.

After its first measurement, the filter repeats two steps for every measurement that is newer than the measurement before:

  1. Prediction: the filter moves its estimate forward to the time of the next measurement, with a model of how the robot moves. The estimate's uncertainty grows.
  2. Correction: the filter fuses the measurement. The uncertainty of the fused values shrinks.

For a measurement that is not newer, the filter skips the prediction, as Why the EKF needs the validator explains.

The paper on robot_localization, A Generalized Extended Kalman Filter Implementation for the Robot Operating System by Moore and Stouch, gives the formulas of both steps.

The filter's state

The 15 booleans stand for the 15 variables that robot_localization's filter estimates, its state \(\mathbf{x}\):

\[ \mathbf{x} = (\,x,\ y,\ z,\ \text{roll},\ \text{pitch},\ \theta,\ v_x,\ v_y,\ v_z,\ \omega_x,\ \omega_y,\ \omega_z,\ a_x,\ a_y,\ a_z\,) \]

The table shows the state's five groups of three variables, with the labels that the comments in ekf.yaml use and the names on robot_localization's parameter page, which gives the same order:

Group Symbols Unit Labels in ekf.yaml Names on the parameter page
Position \(x\), \(y\), \(z\) m x, y, z \(X\), \(Y\), \(Z\)
Orientation roll, pitch, heading \(\theta\) rad roll, pitch, yaw roll, pitch, yaw
Linear velocity \(v_x\), \(v_y\), \(v_z\) m/s vx, vy, vz \(\dot X\), \(\dot Y\), \(\dot Z\)
Angular velocity \(\omega_x\), \(\omega_y\), \(\omega_z\) rad/s vroll, vpitch, vyaw the rates of roll, pitch and yaw
Linear acceleration \(a_x\), \(a_y\), \(a_z\) m/s² ax, ay, az \(\ddot X\), \(\ddot Y\), \(\ddot Z\)

The position and the orientation are the robot's pose in the filter's world_frame, here odom. The velocities and the acceleration are in the robot's own frame, the filter's base_link_frame, here base_footprint. The yaw rate \(\omega_z\) is the rate at which the heading \(\theta\) changes.

The two lists

The excerpt shows the wheel odometry's topic and list, from ekf.yaml, lines 54–59. Each row of the list is one group of the state, and the comment at the end of a row names the row's three variables:

ekf.yaml: the wheel odometry's topic and list
54
55
56
57
58
59
odom0: mobile_base_controller/odom
odom0_config: [false, false, false,   # x, y, z
               false, false, false,   # roll, pitch, yaw
               true,  true,  false,   # vx, vy, vz
               false, false, true,    # vroll, vpitch, vyaw
               false, false, false]   # ax, ay, az

The next excerpt shows the IMU's topic and list, from ekf.yaml, lines 73–78:

ekf.yaml: the IMU's topic and list
73
74
75
76
77
78
imu0: imu/data_validated
imu0_config: [false, false, false,   # x, y, z
              false, false, false,   # roll, pitch, yaw
              false, false, false,   # vx, vy, vz
              false, false, true,    # vroll, vpitch, vyaw
              false, false, false]   # ax, ay, az

The figure shows both lists in the file's layout. A green cell with a check mark is a true: the filter fuses this value from the input. A plain cell is a false: the filter ignores this value of the input, even when the message carries it. A dashed cell belongs to a variable that two_d_mode keeps at 0, whatever the list says.

The list odom0_config of ekf.yaml, for the wheel odometry, as a grid of five rows of three odom0_config wheel odometry Position x y z Orientation roll pitch θ Linear velocity vx ✓ vy ✓ vz Angular velocity ωx ωy ωz ✓ Linear acceleration ax ay az The list imu0_config of ekf.yaml, for the IMU, as a grid of five rows of three imu0_config IMU Position x y z Orientation roll pitch θ Linear velocity vx vy vz Angular velocity ωx ωy ωz ✓ Linear acceleration ax ay az
  • ✓true: the filter fuses this value from the input
  • false: the filter ignores this value of the input
  • kept at 0 by two_d_mode, whatever the list says
The two lists of ekf.yaml, in the file's layout: five rows of three, in the order of the filter's state. θ is the heading, the yaw angle.

The filter fuses four values, the four cells with a check mark in the figure. The list below describes each of the four values with three entries:

  • Message field: the field of the input's message that carries the value, of a nav_msgs/Odometry message for the wheel odometry and of a sensor_msgs/Imu message for the IMU.
  • Variance: the element of the message's covariance that holds the value's variance \(\sigma^2\), and the variance's number.
  • Frame: the coordinate frame in which the message gives the value.

The filter fuses these four values:

  1. Forward speed \(v_x\), from the wheel odometry:
    • Message field: twist.twist.linear.x
    • Variance: twist.covariance[0], 0.001 (m/s)²
    • Frame: base_footprint
  2. Sideways speed \(v_y\), from the wheel odometry:
    • Message field: twist.twist.linear.y
    • Variance: twist.covariance[7], 0.001 (m/s)²
    • Frame: base_footprint
  3. Yaw rate \(\omega_z\), from the wheel odometry:
    • Message field: twist.twist.angular.z
    • Variance: twist.covariance[35], 0.03 (rad/s)²
    • Frame: base_footprint
  4. Yaw rate \(\omega_z\), from the IMU:
    • Message field: angular_velocity.z
    • Variance: angular_velocity_covariance[8], 4 · 10⁻⁴ (rad/s)² on the robot and 2.5 · 10⁻⁵ (rad/s)² in the simulation
    • Frame: imu_link, rotated into base_footprint

The next three sections explain the entries: the messages with their fields, the variances, and the frames.

The messages and their fields

The wheel odometry arrives as nav_msgs/Odometry messages on the topic /diffbot/mobile_base_controller/odom. The collapsed box shows the message's definition, from Odometry.msg in ROS's common_msgs, and the definition of the type of its field twist, from TwistWithCovariance.msg:

Odometry.msg and TwistWithCovariance.msg: the wheel odometry's message
nav_msgs/msg/Odometry.msg
1
2
3
4
5
6
7
# This represents an estimate of a position and velocity in free space.  
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

The field twist has the type geometry_msgs/TwistWithCovariance, which holds the velocities and their covariance, as TwistWithCovariance.msg defines:

geometry_msgs/msg/TwistWithCovariance.msg
1
2
3
4
5
6
7
8
9
# This expresses velocity in free space with uncertainty.

Twist twist

# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
float64[36] covariance

Of an odometry message, the filter uses these fields:

  • header.stamp: the time of the measurement.
  • child_frame_id: the frame of the velocities, here base_footprint.
  • twist.twist: the velocities. twist.twist.linear.x is the forward speed \(v_x\), twist.twist.linear.y is the sideways speed \(v_y\) and twist.twist.angular.z is the yaw rate \(\omega_z\).
  • twist.covariance: the covariance of the velocities, 36 numbers.

The filter doesn't read the message's pose, because odom0_config fuses no pose value.

The IMU's samples arrive as sensor_msgs/Imu messages on the topic /diffbot/imu/data_validated. The collapsed box shows the message's definition with its comments, from Imu.msg in ROS's common_msgs:

Imu.msg: the IMU's message
sensor_msgs/msg/Imu.msg
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
# This is a message to hold data from an IMU (Inertial Measurement Unit)
#
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the 
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each 
# covariance matrix, and disregard the associated estimate.

Header header

geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z 

Of an IMU sample, the filter uses these fields:

  • header.stamp: the time of the measurement.
  • header.frame_id: the frame of the values, here imu_link.
  • angular_velocity: the angular velocity about the three axes of imu_link. After the rotation into base_footprint, its z component is the yaw rate \(\omega_z\).
  • angular_velocity_covariance: the covariance of the angular velocity, 9 numbers.

The filter doesn't read the sample's orientation and linear_acceleration, because imu0_config fuses no orientation and no acceleration.

The variances

A covariance matrix \(\boldsymbol\Sigma\) holds the uncertainties of several values at once. Its element in row \(i\) and column \(j\) is the covariance of the \(i\)-th and the \(j\)-th value. On the diagonal, where \(i = j\), the element is the variance \(\sigma^2\) of one value. Off the diagonal, the element says how the errors of two different values go together. For the IMU's angular velocity \(\boldsymbol\omega\), the matrix is:

\[ \boldsymbol\Sigma_{\boldsymbol\omega} = \begin{pmatrix} \sigma^2_{\omega_x} & \sigma_{\omega_x \omega_y} & \sigma_{\omega_x \omega_z} \\ \sigma_{\omega_y \omega_x} & \sigma^2_{\omega_y} & \sigma_{\omega_y \omega_z} \\ \sigma_{\omega_z \omega_x} & \sigma_{\omega_z \omega_y} & \sigma^2_{\omega_z} \end{pmatrix} \]

A message has no type for a matrix, so it stores the matrix as an array, row by row, as the comments in TwistWithCovariance.msg and Imu.msg say: first the elements of row 0, then those of row 1, and so on. With \(n\) rows and \(n\) columns, counted from 0, the element in row \(i\) and column \(j\) has the index \(k\) in the array:

\[ k = n\,i + j \]

twist.covariance, the field of a nav_msgs/Odometry message, holds the 6 × 6 matrix of the velocities \(v_x\), \(v_y\), \(v_z\), \(\omega_x\), \(\omega_y\) and \(\omega_z\), with \(n = 6\). Its variances lie at the indices 0, 7, 14, 21, 28 and 35. angular_velocity_covariance of a sensor_msgs/Imu message holds the 3 × 3 matrix above, with \(n = 3\) and the variances at 0, 4 and 8. The figure shows both matrices with the index of each element:

The covariance twist.covariance of the wheel odometry, 6 × 6 as a grid of 6 rows and 6 columns, with the index of each element in the array twist.covariance wheel odometry, 6 × 6 vx vx vy vy vz vz ωx ωx ωy ωy ωz ωz ✓0 1 2 3 4 5 6 ✓7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 ✓35 The covariance angular_velocity_covariance of the IMU, 3 × 3 as a grid of 3 rows and 3 columns, with the index of each element in the array angular_velocity_covariance IMU, 3 × 3 ωx ωx ωy ωy ωz ωz 0 1 2 3 4 5 6 7 ✓8
  • ✓the variance of a value that the filter fuses
  • a variance that two_d_mode replaces with 10⁻⁶
  • a covariance of two different values, 0 in both messages
The two covariances as matrices. A row's and a column's symbols name the two values of an element. The number in a cell is the element's index in the message's array.

What fills the elements, and what the filter does with them:

  • Only the diagonals are filled: the drive controller and the firmware write a variance onto each element of the diagonal and leave every other element at 0. So both messages say that the errors of two different values don't go together.
  • The filter copies all elements: robot_localization copies each array into a matrix, element by element, and rotates the matrix into base_footprint. The wheel odometry's velocities are in base_footprint already. The z axis of imu_link points up like the robot's, so the rotation leaves the yaw rate's variance at index 8 as it is.
  • The filter fuses four variances: the three of the wheel odometry's fused velocities, at 0, 7 and 35, and the IMU's yaw rate's, at 8. These are the cells with a check mark.
  • two_d_mode replaces the hatched variances: the variance of \(v_z\) becomes 10⁻⁶ (m/s)², and those of \(\omega_x\) and \(\omega_y\) become 10⁻⁶ (rad/s)², as What two_d_mode does explains.
The code that fills the arrays and copies them into matrices

The drive controller writes the six values of its parameter twist_covariance_diagonal onto the diagonal of twist.covariance and 0 everywhere else, in its function setOdomPubFields(), as lines 722–728 of diff_drive_controller.cpp in ros_controllers 0.22.0 show:

722
723
724
725
726
727
728
odom_pub_->msg_.twist.covariance = {
    static_cast<double>(twist_cov_list[0]), 0., 0., 0., 0., 0.,
    0., static_cast<double>(twist_cov_list[1]), 0., 0., 0., 0.,
    0., 0., static_cast<double>(twist_cov_list[2]), 0., 0., 0.,
    0., 0., 0., static_cast<double>(twist_cov_list[3]), 0., 0.,
    0., 0., 0., 0., static_cast<double>(twist_cov_list[4]), 0.,
    0., 0., 0., 0., 0., static_cast<double>(twist_cov_list[5]) };

The firmware's function ImuPublisher::publish() writes angular_velocity_variance to the indices 0, 4 and 8 of angular_velocity_covariance, as lines 220–224 of imu_publisher.h show:

220
221
222
223
224
for (int i = 0; i < 9; i += 4)
{
    message_.angular_velocity_covariance[i] = config_.angular_velocity_variance;
    message_.linear_acceleration_covariance[i] = config_.linear_acceleration_variance;
}

robot_localization's function copyCovariance() takes the element in row i and column j from the index dimension * i + j of the array, which is \(k = n\,i + j\) with dimension as \(n\), as lines 2336–2340 of ros_filter.cpp in robot_localization 2.7.7 show:

2336
2337
2338
2339
2340
for (size_t i = 0; i < dimension; i++)
{
  for (size_t j = 0; j < dimension; j++)
  {
    covariance(i, j) = arr[dimension * i + j];

These settings supply the numbers on the diagonals:

  • The wheel odometry's variances: the drive controller publishes the values of its parameter twist_covariance_diagonal in diffbot_control.yaml, on the robot and in the simulation.
  • The IMU's variance on the robot: the firmware publishes its setting angular_velocity_variance, 4 · 10⁻⁴ (rad/s)². What the firmware publishes on the BNO055 page lists the firmware's settings. The value is a starting value from the datasheet's noise and offsets. The bench checks on the same page measured the gyroscope's noise with the robot at rest: a variance of 2 to 6 · 10⁻⁷ (rad/s)². The starting value stays for now, because driving adds vibration and drift, which a recording at rest doesn't show.
  • The IMU's variance in the simulation: the simulated sensor publishes the square of gaussianNoise, the noise of 0.005 rad/s that the sensor adds to each value. The setting stands in DiffBot's common_sensors.xacro and in Remo's common_sensors.xacro.

The frames

The filter turns every velocity into its base_link_frame, here base_footprint, before the filter fuses the value:

  • The wheel odometry's velocities \(v_x\), \(v_y\) and \(\omega_z\): a nav_msgs/Odometry message names the frame of its velocities in child_frame_id, which is base_footprint already. So the filter takes the three velocities as they are.
  • The IMU's angular velocity \(\boldsymbol\omega = (\omega_x, \omega_y, \omega_z)\): a sensor_msgs/Imu message names its frame in header.frame_id, here imu_link. The filter rotates the angular velocity into base_footprint with the transform from TF. In the URDF models of both robots, the z axis of imu_link points up like the robot's z axis. So the rotation leaves the yaw rate \(\omega_z\), the field angular_velocity.z, as it is.

The collapsed box shows where robot_localization 2.7.7 reads these fields and frames.

ros_filter.cpp: the velocities of an odometry message and of an IMU sample

robot_localization passes the wheel odometry's velocities on with the frame child_frame_id and the target frame baseLinkFrameId_, the filter's base_link_frame, as lines 1794–1804 of ros_filter.cpp show:

1794
1795
1796
1797
1798
1799
1800
1801
1802
1803
1804
if (twistCallbackData.updateSum_ > 0)
{
  // Grab the twist portion of the message and pass it to the twistCallback
  geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();
  twistPtr->header = msg->header;
  twistPtr->header.frame_id = msg->child_frame_id;
  twistPtr->twist = msg->twist;  // Entire twist object, also copies covariance

  geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
  twistCallback(tptr, twistCallbackData, baseLinkFrameId_);
}

For an IMU sample, robot_localization copies angular_velocity and the nine elements of angular_velocity_covariance into a velocity measurement with the same target frame, as lines 558–574 of ros_filter.cpp show:

558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
// Repeat for velocity
geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();
twistPtr->header = msg->header;
twistPtr->twist.twist.angular = msg->angular_velocity;

// Copy the covariance
for (size_t i = 0; i < ORIENTATION_SIZE; i++)
{
  for (size_t j = 0; j < ORIENTATION_SIZE; j++)
  {
    twistPtr->twist.covariance[TWIST_SIZE * (i + ORIENTATION_SIZE) + (j + ORIENTATION_SIZE)] =
      msg->angular_velocity_covariance[ORIENTATION_SIZE * i + j];
  }
}

geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
twistCallback(tptr, twistCallbackData, baseLinkFrameId_);

Why these values and no others

Input Fused Not fused, and why
Wheel odometry forward speed \(v_x\), sideways speed \(v_y\), yaw rate \(\omega_z\) its pose \(x\), \(y\), \(\theta\): the controller computes the pose from the same velocities, so fusing both would count the wheels twice
IMU yaw rate \(\omega_z\) its orientation, which the firmware marks as unavailable in the message, and its acceleration, which would need its bias removed first

The sideways speed \(v_y\) is always 0 in the wheel odometry, because a differential drive robot can't move sideways. Fusing the 0 keeps the estimate from drifting sideways. Both inputs give the yaw rate \(\omega_z\). On the robot, the gyroscope's variance of 4 · 10⁻⁴ (rad/s)² is 75 times smaller than the wheels' 0.03 (rad/s)², so the filter's yaw rate lies close to the gyroscope's. In the simulation, the simulated gyroscope's variance is 1200 times smaller than the wheels'.

What two_d_mode does

DiffBot and Remo drive on a flat floor, so ekf.yaml sets two_d_mode to true, as the excerpt from ekf.yaml, lines 15–16 shows:

ekf.yaml: two_d_mode
15
16
# The robot drives on a flat floor: the filter keeps z, roll and pitch and their rates at 0
two_d_mode: true

The setting concerns seven of the 15 variables: the height \(z\), roll, pitch, their velocities \(v_z\), \(\omega_x\) and \(\omega_y\), and the vertical acceleration \(a_z\). For every measurement, the filter replaces the measurement's value for each of the seven with 0, gives the 0 the small variance 10⁻⁶ in the squared SI unit of each variable, and marks the variable as fused. The function forceTwoD() does this with its arguments measurement, measurementCovariance and updateVector, as lines 347–374 of ros_filter.cpp show:

ros_filter.cpp, lines 347–374: forceTwoD()
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
void RosFilter<T>::forceTwoD(Eigen::VectorXd &measurement,
                             Eigen::MatrixXd &measurementCovariance,
                             std::vector<int> &updateVector)
{
  measurement(StateMemberZ) = 0.0;
  measurement(StateMemberRoll) = 0.0;
  measurement(StateMemberPitch) = 0.0;
  measurement(StateMemberVz) = 0.0;
  measurement(StateMemberVroll) = 0.0;
  measurement(StateMemberVpitch) = 0.0;
  measurement(StateMemberAz) = 0.0;

  measurementCovariance(StateMemberZ, StateMemberZ) = 1e-6;
  measurementCovariance(StateMemberRoll, StateMemberRoll) = 1e-6;
  measurementCovariance(StateMemberPitch, StateMemberPitch) = 1e-6;
  measurementCovariance(StateMemberVz, StateMemberVz) = 1e-6;
  measurementCovariance(StateMemberVroll, StateMemberVroll) = 1e-6;
  measurementCovariance(StateMemberVpitch, StateMemberVpitch) = 1e-6;
  measurementCovariance(StateMemberAz, StateMemberAz) = 1e-6;

  updateVector[StateMemberZ] = 1;
  updateVector[StateMemberRoll] = 1;
  updateVector[StateMemberPitch] = 1;
  updateVector[StateMemberVz] = 1;
  updateVector[StateMemberVroll] = 1;
  updateVector[StateMemberVpitch] = 1;
  updateVector[StateMemberAz] = 1;
}

So the two lists can't change these seven variables. A false at their positions loses nothing, and a true would fuse the same 0. The figure of the two lists shows their cells dashed for this reason.

The other settings

This section explains the settings of ekf.yaml that the sections above don't, in the order of the file. Each setting comes with its lines of the file.

  • frequency: the filter produces its estimate 50 times per second, at the frequency of 50 Hz, the IMU's rate. In each of these runs, the filter processes the measurements that have arrived, up to the ROS time, and publishes the estimate. The excerpt shows the setting with its comment, from ekf.yaml, lines 8–12:

    ekf.yaml: frequency
     8
     9
    10
    11
    12
    # Hz, the IMU's rate. In the simulation, the drive controller's odometry arrives at 50 Hz too, its
    # publish_rate in diffbot_control.yaml. On the robot, it arrives at most 10 times per second: the
    # controller publishes at most one message each time it runs, and diffbot_base's control loop
    # runs it at 10 Hz (diffbot_base/src/diffbot_base.cpp)
    frequency: 50
    

    The inputs don't have to arrive at this rate. In the simulation, the wheel odometry arrives at a nominal 50 Hz too, in simulation time, the drive controller's publish_rate in diffbot_control.yaml. On the robot, the wheel odometry arrives at most 10 times per second, as the collapsed box explains.

    Why the robot's wheel odometry arrives at most 10 times per second

    Two rates in the code limit the wheel odometry on the robot:

    • The firmware's rate: the firmware on the Teensy sends the wheels' joint states at a nominal 20 Hz, its setting UPDATE_RATE_CONTROL.
    • The control loop's rate: the control loop of the node diffbot_base reads the latest joint states, runs the drive controller and writes the wheel commands at a nominal 10 Hz, with ros::Rate rate(10.0) in diffbot_base.cpp.

    The drive controller publishes at most one odometry message each time it runs, so the wheel odometry arrives at most 10 times per second. The drive controller's update() publishes the odometry only when publish_period_ has passed since last_state_publish_time_, as lines 430–432 of diff_drive_controller.cpp in ros_controllers 0.22.0 show:

    diff_drive_controller.cpp: when update() publishes the odometry
    430
    431
    432
    if (last_state_publish_time_ + publish_period_ < time)
    {
      last_state_publish_time_ += publish_period_;
    

    Between two wheel messages, the IMU's samples at 50 Hz keep the prediction and the correction going. So the estimate's yaw rate \(\omega_z\) follows the gyroscope at 50 Hz, while the wheels correct the forward speed \(v_x\) only when a wheel message arrives. These rates are set in the code and not measured on the robot. The issue Base: measure the control loop's rate on the robot, then decide (diffbot#218) measures them.

  • sensor_timeout: the sensor_timeout of 0.1 s, five of the IMU's sample periods, keeps the estimate moving when the inputs stop. When no measurement waits in the filter's queue and the ROS time is at least 0.1 s past the time of the filter's estimate, the filter predicts its estimate to the ROS time, without a correction. The prediction moves the time of the estimate to the ROS time. So while the inputs stay silent, the filter predicts about every 0.1 s, not in each of its runs. The function integrateMeasurements() does this. The excerpt shows the setting with its comment, from ekf.yaml, lines 13–14:

    ekf.yaml: sensor_timeout
    13
    14
    # When no measurement arrives for this long (s), the filter keeps predicting from its last state
    sensor_timeout: 0.1
    
  • The frames: five settings name the frames of REP 105 for this robot and say which transform the filter publishes. robot_localization's description of the frame settings explains them. The excerpt shows the five, from ekf.yaml, lines 18–24:

    ekf.yaml: the frames
    18
    19
    20
    21
    22
    23
    24
    map_frame: map
    odom_frame: odom
    base_link_frame: base_footprint
    # The filter estimates the pose in odom: continuous, without the jumps of a map-based localization
    # such as amcl, which publishes map → odom itself
    world_frame: odom
    publish_tf: true
    
    • map_frame, odom_frame and base_link_frame: the names of the frames map and odom and of the robot's frame base_footprint. The filter fuses the velocities in base_link_frame, as The frames explains.
    • world_frame: the value is odom, so the filter estimates the pose in odom. The estimate is continuous, without the jumps of a map-based localization such as amcl, which publishes map → odom itself. A filter with world_frame: map would publish map → odom instead. This filter doesn't, so map_frame only names the frame.
    • publish_tf: with true, the filter publishes the transform from world_frame to base_link_frame, here odom → base_footprint, as robot_localization's description of publish_tf says.
  • reset_on_time_jump: the value is true. In the simulation, a call of the service /gazebo/reset_simulation sets the clock back to zero, and every measurement after the call looks older than the one before. The filter then starts again from the new time. robot_localization does this only with simulated time, as its description of reset_on_time_jump says. On the robot, a clock that goes back makes the filter ignore the messages of an input, with a warning on the topic /diagnostics, until the input's stamps have caught up with its last stamp from before. The excerpt shows the setting with its comment, from ekf.yaml, lines 26–32:

    ekf.yaml: reset_on_time_jump
    26
    27
    28
    29
    30
    31
    32
    # In the simulation, a reset of Gazebo's world sets the clock back, and every measurement looks
    # older than the one before. The filter then starts again from the new time, instead of ignoring
    # the measurements until the clock has caught up. robot_localization does this only with simulated
    # time. On the robot, a clock that goes back makes the filter ignore the measurements until the
    # clock has caught up. The docs show the check in robot_localization's code:
    # https://ros-mobile-robots.com/packages/imu/robot_localization/#the-other-settings
    reset_on_time_jump: true
    

    The collapsed box shows in robot_localization's code why the filter starts over only with simulated time.

    Why only simulated time starts the filter over: ros_filter.cpp, lines 2197–2252

    robot_localization 2.7.7 decides what to do with a message that is older than the last one in the callback of the message's input. Both inputs of this configuration, the wheel odometry's velocities and the IMU's angular velocity, pass through the function twistCallback(). The function first tests that the message's stamp isn't older than lastMessageTimes_, the last stamp of the same input. For an older message, the function calls reset() when resetOnTimeJump_ is set and ros::Time::isSimTime() says that the ROS time is simulated. Otherwise the function ignores the message and adds a warning to the filter's diagnostics, as lines 2197–2252 of ros_filter.cpp show:

    2197
    2198
    2199
    2200
    2201
    2202
    2203
    2204
    2205
    2206
    2207
    2208
    2209
    2210
    2211
    2212
    2213
    2214
    2215
    2216
    2217
    2218
    2219
    2220
    2221
    2222
    2223
    2224
    2225
    2226
    2227
    2228
    2229
    2230
    2231
    2232
    2233
    2234
    2235
    2236
    2237
    2238
    2239
    2240
    2241
    2242
    2243
    2244
    2245
    2246
    2247
    2248
    2249
    2250
    2251
    2252
    // Make sure this message is newer than the last one
    if (msg->header.stamp >= lastMessageTimes_[topicName])
    {
      RF_DEBUG("Update vector for " << topicName << " is:\n" << callbackData.updateVector_);
    
      Eigen::VectorXd measurement(STATE_SIZE);
      Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
    
      measurement.setZero();
      measurementCovariance.setZero();
    
      // Make sure we're actually updating at least one of these variables
      std::vector<int> updateVectorCorrected = callbackData.updateVector_;
    
      // Prepare the twist data for inclusion in the filter
      if (prepareTwist(msg, topicName, targetFrame, updateVectorCorrected, measurement, measurementCovariance))
      {
        // Store the measurement. Add a "twist" suffix so we know what kind of measurement
        // we're dealing with when we debug the core filter logic.
        enqueueMeasurement(topicName,
                           measurement,
                           measurementCovariance,
                           updateVectorCorrected,
                           callbackData.rejectionThreshold_,
                           msg->header.stamp);
    
        RF_DEBUG("Enqueued new measurement for " << topicName << "_twist\n");
      }
      else
      {
        RF_DEBUG("Did *not* enqueue measurement for " << topicName << "_twist\n");
      }
    
      lastMessageTimes_[topicName] = msg->header.stamp;
    
      RF_DEBUG("Last message time for " << topicName << " is now " <<
        lastMessageTimes_[topicName] << "\n");
    }
    else if (resetOnTimeJump_ && ros::Time::isSimTime())
    {
      reset();
    }
    else
    {
      std::stringstream stream;
      stream << "The " << topicName << " message has a timestamp before that of the previous message received," <<
                " this message will be ignored. This may indicate a bad timestamp. (message time: " <<
                msg->header.stamp.toSec() << ")";
      addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
                    topicName + "_timestamp",
                    stream.str(),
                    false);
    
      RF_DEBUG("Message is too old. Last message time for " << topicName << " is " << lastMessageTimes_[topicName] <<
        ", current message time is " << msg->header.stamp << ".\n");
    }
    
  • The inputs' other settings, _differential and _queue_size: each input has the same two. The excerpt shows them for the wheel odometry, from ekf.yaml, lines 60–66:

    ekf.yaml: the wheel odometry's other settings
    60
    61
    62
    63
    64
    65
    66
    # true would fuse the change between two poses instead of the pose. No effect here, because the
    # filter fuses no pose value of this input
    odom0_differential: false
    # The size of the queue of the filter node's ROS subscriber for this topic. Messages that arrive
    # while the node is busy wait in the queue. With 10, at most ten wait, and ROS drops the oldest
    # one when another arrives. robot_localization's default is 1
    odom0_queue_size: 10
    

    The next excerpt shows them for the IMU, from ekf.yaml, lines 79–81:

    ekf.yaml: the IMU's other settings
    79
    80
    81
    # As odom0_differential and odom0_queue_size
    imu0_differential: false
    imu0_queue_size: 10
    
    • odom0_differential and imu0_differential: the _differential setting applies to pose values only. With true, the filter would fuse the change between two consecutive poses as a velocity, instead of the pose. Neither list fuses a pose value, so the setting has no effect here. false is robot_localization's default.
    • odom0_queue_size and imu0_queue_size: each of the two parameters sets the size of the queue of the filter node's ROS subscriber for the input's topic. Messages that arrive while the node is busy wait in the queue. With 10, at most ten wait, and ROS drops the oldest one when another arrives. robot_localization's default is 1.

The IMU sample validator

The IMU sample validator is the ROS node /diffbot/imu_validator, from imu_validator.py. The validator subscribes to the topic /diffbot/imu/data_raw and forwards each valid sample, unchanged and with its stamp, on the topic /diffbot/imu/data_validated, which the EKF reads. The validator drops every other sample.

Why the EKF needs the validator

robot_localization checks the stamp of every message of an input itself. But this stamp check allows equal stamps and doesn't limit a sample's age. Three facts about robot_localization 2.7.7, the version in the dev container, matter here. The figures and the formulas use four times, each in s:

  • \(t\): the ROS time, the clock of the receiving node when a sample arrives.
  • \(t_s\): the sample's stamp, msg->header.stamp.
  • \(t_\text{input}\): lastMessageTimes_, the last stamp of the same input that passed the filter's stamp check, even if the filter couldn't use the message afterwards, for example without the transform into base_footprint.
  • \(t_\text{filter}\): lastMeasurementTime_, the time to which the filter has moved its estimate. A measurement of either input whose stamp isn't earlier sets \(t_\text{filter}\) to its stamp. An older measurement leaves \(t_\text{filter}\) as it is. A prediction without a measurement, after sensor_timeout, moves \(t_\text{filter}\) to the ROS time.

1. The stamp check compares with the input's last stamp, not with the clock: the filter takes an IMU sample when \(t_s \ge t_\text{input}\). So an equal stamp passes, and so does a stamp that lies far behind the ROS time \(t\), as the figure shows:

robot_localization's stamp check on a time axis: a stamp passes when it is not earlier than the last stamp of the same input, however far it lies behind the ROS time ignored passes 0.11 s tinput now: t Sample P: forwarded. the same stamp as the last one of this input. The filter takes the sample. The validator drops this one already in check 4, because it is 0.14 s old. A recent sample with a repeated stamp fails check 5P Sample Q: forwarded. stamped 0.11 s before the ROS time. The filter takes the sample, because it compares the stamp only with the last one of this input. The validator drops it in check 4Q Sample R: dropped. earlier than the last stamp of this input. The filter ignores the sample, or starts over with simulated timeR
  • P✓ taken: the same stamp as the last one of this input. The filter takes the sample. The validator drops this one already in check 4, because it is 0.14 s old. A recent sample with a repeated stamp fails check 5
  • Q✓ taken: stamped 0.11 s before the ROS time. The filter takes the sample, because it compares the stamp only with the last one of this input. The validator drops it in check 4
  • R✕ ignored: earlier than the last stamp of this input. The filter ignores the sample, or starts over with simulated time
robot_localization's own check of an IMU sample's stamp. The axis shows stamps, with time running to the right, and the ROS time at the dashed line. Example values.

2. The filter predicts only forward in time: the time step \(\Delta t\), in s, is the time from the filter's time to the new measurement's stamp:

\[ \Delta t = t_s - t_\text{filter} \]
  • When \(\Delta t > 0\): the filter predicts its estimate forward by \(\Delta t\), to \(t_s\), and then corrects it with the measurement. \(t_s\) becomes the new \(t_\text{filter}\).
  • When \(\Delta t = 0\): the filter skips the prediction and corrects its estimate with the measurement. \(t_\text{filter}\) stays.
  • When \(\Delta t < 0\): the filter skips the prediction too, and corrects its estimate at \(t_\text{filter}\) with the older measurement. \(t_\text{filter}\) stays.

3. The filter doesn't go back in time: ekf.yaml leaves smooth_lagged_data at its default false, which robot_localization's description of smooth_lagged_data explains, so the filter doesn't return to \(t_s\) to process a late measurement in its place.

The figure shows the second and the third fact for a current and for a late IMU sample:

Δt > 0: the filter predicts, then corrects

A current IMU sample: its stamp lies after the filter's time, so the filter predicts its estimate forward to the stamp and then corrects it tfilter ts estimate wheels IMU now: t predict correct Δt > 0

Δt < 0: the filter corrects only

A late IMU sample: its stamp lies before the filter's time, so the filter corrects its estimate at the filter's time, with the old sample, and doesn't go back to the stamp tfilter ts estimate wheels IMU now: t correct Δt < 0
The filter's time step for an IMU sample, with the wheel odometry's messages that the filter processed before. The dots on the top row are the filter's estimate. The axis shows stamps. t is the ROS time when the IMU sample arrives. Example values.

So a delayed sample, or a sample with the same stamp as the sample before, can pass the filter's stamp check. If the sample also passes the filter's other input checks, such as the transform into base_footprint, the filter corrects its current estimate with the sample, as if the sample were current. A sample that waited in a buffer would count as the robot's yaw rate \(\omega_z\) now.

The collapsed box shows the code of the first two facts.

ros_filter.cpp and filter_base.cpp: the stamp check, and prediction only forward in time

The function twistCallback() checks the stamp of an angular velocity, as lines 2197–2221 of ros_filter.cpp show:

2197
2198
2199
2200
2201
2202
2203
2204
2205
2206
2207
2208
2209
2210
2211
2212
2213
2214
2215
2216
2217
2218
2219
2220
2221
// Make sure this message is newer than the last one
if (msg->header.stamp >= lastMessageTimes_[topicName])
{
  RF_DEBUG("Update vector for " << topicName << " is:\n" << callbackData.updateVector_);

  Eigen::VectorXd measurement(STATE_SIZE);
  Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);

  measurement.setZero();
  measurementCovariance.setZero();

  // Make sure we're actually updating at least one of these variables
  std::vector<int> updateVectorCorrected = callbackData.updateVector_;

  // Prepare the twist data for inclusion in the filter
  if (prepareTwist(msg, topicName, targetFrame, updateVectorCorrected, measurement, measurementCovariance))
  {
    // Store the measurement. Add a "twist" suffix so we know what kind of measurement
    // we're dealing with when we debug the core filter logic.
    enqueueMeasurement(topicName,
                       measurement,
                       measurementCovariance,
                       updateVectorCorrected,
                       callbackData.rejectionThreshold_,
                       msg->header.stamp);

The function processMeasurement() computes delta, \(\Delta t\), from measurement.time_, \(t_s\), and lastMeasurementTime_, \(t_\text{filter}\). The function calls predict() only when delta is positive, and then correct(), as lines 205–234 of filter_base.cpp show:

205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
void FilterBase::processMeasurement(const Measurement &measurement)
{
  FB_DEBUG("------ FilterBase::processMeasurement (" << measurement.topicName_ << ") ------\n");

  double delta = 0.0;

  // If we've had a previous reading, then go through the predict/update
  // cycle. Otherwise, set our state and covariance to whatever we get
  // from this measurement.
  if (initialized_)
  {
    // Determine how much time has passed since our last measurement
    delta = measurement.time_ - lastMeasurementTime_;

    FB_DEBUG("Filter is already initialized. Carrying out predict/correct loop...\n"
             "Measurement time is " << std::setprecision(20) << measurement.time_ <<
             ", last measurement time is " << lastMeasurementTime_ << ", delta is " << delta << "\n");

    // Only want to carry out a prediction if it's
    // forward in time. Otherwise, just correct.
    if (delta > 0)
    {
      validateDelta(delta);
      predict(measurement.time_, delta);

      // Return this to the user
      predictedState_ = state_;
    }

    correct(measurement);

At its end, the function moves lastMeasurementTime_ to the measurement's stamp only when delta is not negative, as lines 261–264 of filter_base.cpp show:

261
262
263
264
if (delta >= 0.0)
{
  lastMeasurementTime_ = measurement.time_;
}

Two settings of the EKF sound as if they covered this case. They don't:

Neither setting looks at a sample's age, its frame or a repeated stamp.

The five checks

A sample is valid only if it passes all five checks. For each sample, the node calls validate_and_record() of the class ImuValidator, which runs the checks and returns the reason of the first one that fails. In the figures and in the text, \(t\) is the ROS time, the node's clock at the moment the sample arrives. \(t_s\) is the sample's stamp, and \(t_\text{last}\) is the stamp of the last sample that the node forwarded.

Before the first check, the node drops every sample while the ROS time is zero. With simulation time, the ROS time is zero until the first message on Gazebo's topic /clock arrives. frame_id, max_age and max_future are the node's parameters, which ekf.launch sets.

Checks 1 to 3: the sample's form

The first three checks look at the sample's frame, values and variances, fields of its sensor_msgs/Imu message. Each of the three figures shows the fields that its check reads, for a sample that passes and for one that the node drops.

1. The frame: check_frame() checks that the sample's header.frame_id is the node's parameter frame_id, imu_link. The EKF needs this frame to turn the measured rate into the robot's frame with TF.

Check 1: a sample whose header.frame_id is imu_link passes. A sample with the frame base_link is dropped The node's parameter frame_id = imu_link header.frame_id "imu_link" ✓ forwarded header.frame_id "base_link" ✕ dropped: wrong frame
Check 1 reads the sample's header.frame_id. Example values.

2. The values: check_finite_values() checks that the three components of the angular velocity \(\boldsymbol\omega\) and of the accelerometer's reading \(\mathbf f\), the field linear_acceleration, are finite numbers, so that none is NaN or infinite.

Check 2: a sample whose angular velocity and linear acceleration are finite passes. A sample with a NaN in its angular velocity is dropped x y z angular_velocity rad/s 0.01 −0.02 0.48 linear_acceleration m/s² 0.12 0.05 9.79 ✓ forwarded angular_velocity rad/s 0.01 NaN 0.48 linear_acceleration m/s² 0.12 0.05 9.79 ✕ dropped: not finite
Check 2 reads the three components of the sample's angular_velocity, in rad/s, and of its linear_acceleration, in m/s². Example values.

3. The variances: check_angular_velocity_covariance() checks that the nine values of the angular velocity's covariance are finite, and that its diagonal, the three variances \(\sigma^2\), is positive. The figure lays the covariance out as the figure of the covariances does.

Check 3, a sample that passes: the three variances on the diagonal of angular_velocity_covariance are positive ✓ forwarded three positive variances ωx ωx ωy ωy ωz ωz 4·10⁻⁴ 0 0 0 4·10⁻⁴ 0 0 0 4·10⁻⁴ Check 3, a sample that is dropped: the variance of the yaw rate is 0 ✕ dropped a variance of 0 ωx ωx ωy ωy ωz ωz 4·10⁻⁴ 0 0 0 4·10⁻⁴ 0 0 0 0
Check 3 reads the sample's angular_velocity_covariance, laid out as in the figure of the covariances. Its diagonal holds the three variances, in (rad/s)². Example values: the firmware's variance 4 · 10⁻⁴ (rad/s)², and 0 as the error.

Check 4: the stamp against the ROS time

check_not_too_old() and check_not_from_the_future() check that the sample's stamp lies in this interval:

\[ t - \texttt{max\_age} \le t_s \le t + \texttt{max\_future} \]

A stamp can fail only one of the two checks, so their order doesn't matter. The figure shows samples inside and outside the interval:

Check 4 on a time axis: a stamp passes when it is at most max_age, 0.1 s, older than the ROS time and at most max_future, 0.02 s, ahead of it too old too far ahead max_future = 0.02 s max_age = 0.1 s t − 0.1 s now: t t + 0.02 s Sample A: dropped. stamped 0.13 s before the ROS time. The sample waited in a bufferA Sample B: forwarded. stamped 0.01 s before the ROS time. The usual delay on the wayB Sample C: forwarded. stamped 0.01 s ahead of the ROS time. The sender's clock is a little aheadC Sample D: dropped. stamped 0.04 s ahead of the ROS time. That is more than max_futureD
  • A✕ dropped: stamped 0.13 s before the ROS time. The sample waited in a buffer
  • B✓ forwarded: stamped 0.01 s before the ROS time. The usual delay on the way
  • C✓ forwarded: stamped 0.01 s ahead of the ROS time. The sender's clock is a little ahead
  • D✕ dropped: stamped 0.04 s ahead of the ROS time. That is more than max_future
Check 4: the stamp against the ROS time. The axis shows stamps, with time running to the right and one tick every 0.02 s, the IMU's sample period at 50 Hz. A solid green mark is a forwarded sample, a dashed red mark a dropped one. Each example assumes that the sample passes the other checks.
  • Not too old: the stamp is at most max_age, 0.1 s, older than the ROS time. That is five samples at the IMU's 50 Hz. On the robot, a sample takes time to travel from the Teensy over USB and through rosserial's node. So its stamp is normally a little older than the ROS time when the sample arrives, as sample B. A sample that waited long in a buffer on the way is older than max_age, as sample A.
  • Not too far ahead: the stamp is at most max_future, 0.02 s, ahead of the ROS time. That is one sample. The Teensy stamps a sample with its own clock, which rosserial synchronizes only approximately with the SBC's clock. The offset that remains between the two clocks can put a stamp ahead of the ROS time, as sample C. In the simulation, a sample can arrive before the node has received the matching message on the topic /clock. Sample D is ahead by more than max_future. max_future is the allowed difference, not a measured delay.

The values of max_age and max_future are initial policy values, not standard IMU limits. They stay until the latency of the robot's USB link and rosserial is measured. The issue IMU validator: measure the samples' age on the robot, then set max_age and max_future (diffbot#219) plans this measurement.

Check 5: the stamp against the last forwarded sample

check_newer_than_the_last() checks that the sample's stamp is later than the stamp of the last forwarded sample: \(t_s > t_\text{last}\). A sample with an older stamp arrived out of order, as sample E. A sample with the same stamp came twice, as sample F:

Check 5 on a time axis: a stamp passes when it is later than the last forwarded sample's stamp not later later stamp of the last forwarded sample Sample E: dropped. an older stamp. The sample arrived out of orderE Sample F: dropped. the same stamp. The sample came twiceF Sample G: forwarded. the next stamp, 0.02 s laterG
  • E✕ dropped: an older stamp. The sample arrived out of order
  • F✕ dropped: the same stamp. The sample came twice
  • G✓ forwarded: the next stamp, 0.02 s later
Check 5: the stamp against the last forwarded sample's. The ticks are 0.02 s apart. A solid green mark is a forwarded sample, a dashed red mark a dropped one. Each example assumes that the sample passes the other checks.

When the ROS time goes back, for example after a call of the service /gazebo/reset_simulation, the order check starts over from the new time.

What the validator doesn't check

  • Other fields of the message: the orientation, the acceleration's covariance, or whether a covariance matrix is positive definite. So the validator doesn't validate every field of a sensor_msgs/Imu message.
  • Whether a value is plausible: the validator looks at a sample's form and its time only. A yaw rate \(\omega_z\) of 100 rad/s with a fresh stamp is valid.

The validator also never gives an old sample a new stamp.

Judging a value is a statistical test inside robot_localization, which a rejection threshold switches on. An odometry input can have a threshold for its pose update and one for its velocity update. An IMU input can have one for its orientation, one for its angular velocity and one for its linear acceleration. With a threshold, the filter doesn't fuse an update that lies too far from the filter's own prediction. The filter measures "too far" as a Mahalanobis distance: the difference between the measurement and the prediction, counted in units of their combined uncertainty. For an update of one value, the distance is the number of standard deviations between the measurement and the prediction. For an update of several values, the distance is one number for all of them together, not a limit for each value. The test compares an input with the filter's prediction, not with the other input.

The figure below the list shows the distance \(d\) for one value and for two values, with example numbers:

  • One value, an IMU sample's yaw rate: this is the update that imu0_twist_rejection_threshold would test. The prediction of the yaw rate \(\omega_z\) has the standard deviation \(\sigma_\text{pred}\), and the IMU's measurement has the standard deviation \(\sigma_\text{IMU}\), 0.02 rad/s, the square root of the firmware's variance 4 · 10⁻⁴ (rad/s)². The difference \(y\) between the measurement and the prediction counts in units of both standard deviations together:

    \[ d = \frac{|y|}{\sqrt{\sigma_\text{pred}^2 + \sigma_\text{IMU}^2}} = \frac{0.05\ \text{rad/s}}{\sqrt{(0.015\ \text{rad/s})^2 + (0.02\ \text{rad/s})^2}} = \frac{0.05\ \text{rad/s}}{0.025\ \text{rad/s}} = 2 \]
  • Two values, the wheel odometry's forward speed and yaw rate: these are two of the velocities that odom0_twist_rejection_threshold would test together. The forward speed \(v_x\) and the yaw rate \(\omega_z\) each have their own difference, \(y_{v_x}\) and \(y_{\omega_z}\), and their own standard deviation of the prediction and the measurement together, \(\sigma_{v_x}\) and \(\sigma_{\omega_z}\). The wheel odometry's variances are 0.001 (m/s)² for \(v_x\) and 0.03 (rad/s)² for \(\omega_z\). With the same prediction of the yaw rate as above and a prediction of the forward speed with the standard deviation 0.024 m/s, the example's values are:

    \[ \sigma_{v_x} = \sqrt{0.001\ (\text{m/s})^2 + (0.024\ \text{m/s})^2} \approx 0.04\ \text{m/s}, \qquad \sigma_{\omega_z} = \sqrt{0.03\ (\text{rad/s})^2 + (0.015\ \text{rad/s})^2} \approx 0.17\ \text{rad/s} \]

    The wheel odometry's yaw rate has 75 times the IMU's variance, so its \(\sigma_{\omega_z}\) is about seven times the 0.025 rad/s of the IMU's example. Each difference counts in units of its own standard deviation. When the errors of the two values don't go together, as in the example, the squares of the two counted differences add up to the square of the distance:

    \[ d^2 = \left(\frac{y_{v_x}}{\sigma_{v_x}}\right)^2 + \left(\frac{y_{\omega_z}}{\sigma_{\omega_z}}\right)^2 \]

    All measurements with the same distance lie on an ellipse around the prediction, and a threshold is one of these ellipses. So a measurement can lie within two standard deviations in each value alone and still have a distance of more than 2, as Q.

The two cases are reduced examples. The updates of this configuration also hold the zeros that two_d_mode adds, and the wheel odometry's update also holds the sideways speed \(v_y\), as the collapsed box below the figure explains.

One value: an IMU sample's yaw rate

The yaw rate on an axis: the prediction and the IMU's measurement as two bell curves, 0.05 rad/s apart. The distance d counts this difference in units of the two standard deviations together, 0.025 rad/s, so d is 2 prediction σpred = 0.015 rad/s IMU σIMU = 0.02 rad/s 0.45 0.50 0.55 rad/s ωz 0 1 2 3 d

Two values: the wheel odometry's forward speed and yaw rate

The differences between measurement and prediction for the forward speed and the yaw rate as a plane. Around the prediction, the ellipses of the distances 1 and 2, and the rectangle of two standard deviations for each value alone yvx yωz 0.08 m/s −0.08 0.35 rad/s −0.35 d = 1 d = 2 Sample P: forwarded. the forward speed 1.25 and the yaw rate 0.6 standard deviations from the prediction: d = 1.4, inside the ellipseP Sample Q: dropped. each value 1.6 standard deviations from the prediction, so each value alone lies inside the rectangle. Together, d = 2.3, outside the ellipseQ
  • P✓ fused: the forward speed 1.25 and the yaw rate 0.6 standard deviations from the prediction: d = 1.4, inside the ellipse
  • Q✕ not fused: each value 1.6 standard deviations from the prediction, so each value alone lies inside the rectangle. Together, d = 2.3, outside the ellipse
The distance between a measurement and the prediction, counted in standard deviations, with an example threshold of 2. The wheel odometry has its own, larger variance of the yaw rate, so the second drawing's standard deviation of the yaw rate is larger than the first's. The second drawing's axes are the differences y from the prediction, the green dot. Example values: the variances of the firmware and of diffbot_control.yaml, chosen ones for the prediction. ekf.yaml sets no threshold.

ekf.yaml sets no rejection threshold, so this test is off for both inputs. The filter's other handling of an input stays: the stamp-order check, the transform into base_footprint, the selection by the two lists, and leaving out values that are not finite. The test is off for two reasons:

  • The robot's variances are starting values: the firmware and the drive controller publish fixed variances. Whether these fit while the robot drives isn't checked yet. A threshold counts in these uncertainties, so a number chosen today would mean little.
  • A threshold needs recorded drives: a threshold that is too tight rejects valid changes of the motion, for example the start of a turn. Recordings at rest can't show how the test behaves when the robot accelerates, turns and slips.

The issue EKF: evaluate outlier rejection on recorded drives (diffbot#205) covers the evaluation of the thresholds on recorded drives of the robot.

How robot_localization 2.7.7 computes the test, and the parameters' names

Once the filter has its first measurement, which sets the state without a test, the filter compares the squared distance \(d^2\) of every update with the square of the threshold. For one value, the formula is the one of the figure, with \(\mathbf{H} \mathbf{P} \mathbf{H}^\mathsf{T} = \sigma_\text{pred}^2\) and \(\mathbf{R} = \sigma_\text{IMU}^2\). The squared distance is:

\[ d^2 = \mathbf{y}^\mathsf{T} \left(\mathbf{H} \mathbf{P} \mathbf{H}^\mathsf{T} + \mathbf{R}\right)^{-1} \mathbf{y}, \qquad \mathbf{y} = \mathbf{z} - \mathbf{H} \hat{\mathbf{x}} \]
  • \(\mathbf{z}\): the values of this update after the transform into base_footprint, for example the three velocities of one wheel odometry message, together with the zeros that two_d_mode adds.
  • \(\hat{\mathbf{x}}\): the filter's state estimate right before this correction. The filter predicts the estimate to the measurement's stamp when that stamp is later than the filter's time. A late measurement is compared with the estimate as it is.
  • \(\mathbf{H}\): the matrix that picks the update's variables out of the state.
  • \(\mathbf{y}\): the innovation, the difference between the measurement and the state estimate.
  • \(\mathbf{P}\): the covariance of that state estimate, the filter's own uncertainty.
  • \(\mathbf{R}\): the covariance of the update's values after the transform, together with the variances that two_d_mode adds.
  • The raised \(\mathsf{T}\) transposes a vector or a matrix, and the raised \(-1\) inverts a matrix.

The filter doesn't fuse the update when \(d^2\) is the threshold's square or more. The function checkMahalanobisThreshold() computes sqMahalanobis, \(d^2\), from innovation, \(\mathbf{y}\), and invCovariance, the inverse in the formula. nsigmas is the threshold. Lines 384–401 of filter_base.cpp show the function:

384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
                                           const Eigen::MatrixXd &invCovariance,
                                           const double nsigmas)
{
  double sqMahalanobis = innovation.dot(invCovariance * innovation);
  double threshold = nsigmas * nsigmas;

  if (sqMahalanobis >= threshold)
  {
    FB_DEBUG("Innovation mahalanobis distance test failed. Squared Mahalanobis is: " << sqMahalanobis << "\n" <<
             "Threshold is: " << threshold << "\n" <<
             "Innovation is: " << innovation << "\n" <<
             "Innovation covariance is:\n" << invCovariance << "\n");

    return false;
  }

  return true;

The threshold for the wheel odometry's velocities is odom0_twist_rejection_threshold. The parameter tests the whole velocity update at once: \(v_x\), \(v_y\) and \(\omega_z\), together with the zeros that two_d_mode adds. When the update fails the test, the filter fuses none of the three velocities. No parameter tests the yaw rate alone.

The threshold for an IMU's angular velocity is imu0_twist_rejection_threshold. The parameter documentation calls it imu0_angular_velocity_rejection_threshold, but robot_localization 2.7.7 reads the name with twist, as lines 1414–1418 of ros_filter.cpp show:

1414
1415
1416
1417
1418
// Check for angular velocity rejection threshold
double twistMahalanobisThresh;
std::string imuTwistRejectionName =
  imuTopicName + std::string("_twist_rejection_threshold");
nhLocal_.param(imuTwistRejectionName, twistMahalanobisThresh, std::numeric_limits<double>::max());

The validator's diagnostics

The validator publishes its counts on the topic /diagnostics, next to the EKF's own status there: how many samples it forwarded, and how many it dropped for which reason. The function health() sets the status. The status is an error in three cases: before the validator has forwarded its first sample, after the ROS time went back until the validator forwards a sample again, and when the validator forwarded no sample for more than SILENCE_LIMIT, 1 s. The status is a warning when the validator dropped samples since its last report.

rostopic echo /diagnostics prints the reports as text. rosrun rqt_runtime_monitor rqt_runtime_monitor shows them in a window, sorted into errors, warnings and OK, with the values of the entry that you click. rqt_robot_monitor shows nothing here, because it reads the topic /diagnostics_agg of a diagnostic aggregator, which the launch files don't start.

The example comes from the simulation, at the commit ead3402 of the diffbot repository, with Remo and ekf:=true. Five IMU samples stamped 0.5 s before the ROS time were sent by hand on the topic /diffbot/imu/data_raw, next to the simulated IMU's own samples. The validator dropped four of them before its report at the ROS time 16 s, and the fifth before its next report. The picture shows the report at 16 s in rqt_runtime_monitor, next to the two entries of robot_localization's node:

rqt_runtime_monitor's two panes, one above the other. The upper pane lists the entry of the IMU sample validator under Warnings and two entries of the EKF under Ok. The lower pane shows the selected validator entry: the message "dropped 4 samples since the last report", 792 samples forwarded and 4 dropped as too old.
rqt_runtime_monitor in the simulation, at the ROS time 16 s. The window shows the list of entries and the selected entry's values side by side. The picture puts the list above the values. The robot's diagnostics are not recorded yet.

The collapsed box shows the same report as rostopic echo /diagnostics prints it. The level 1 is a warning.

The validator's report at 16 s, as text
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
header:
  seq: 8
  stamp:
    secs: 16
    nsecs:         0
  frame_id: ''
status:
  -
    level: 1
    name: "diffbot/imu_validator: IMU sample validator (imu/data_raw to imu/data_validated)"
    message: "dropped 4 samples since the last report"
    hardware_id: "imu"
    values:
      -
        key: "Samples forwarded"
        value: "792"
      -
        key: "Dropped, too old"
        value: "4"

How it's tested

Four tests check the EKF without the robot, in CI with the repository's other tests:

  • The validator alone: test_imu_validator.py calls the validator's checks directly, without a ROS master. Its 27 unit tests cover each reason to drop a sample, the limits max_age and max_future, a clock that goes back, and the diagnostics: the status for each case and the counts of a report.
  • On a stepped clock: ekf_synthetic.test starts ekf.launch with DiffBot's URDF for the IMU's frame. The test publishes the simulated clock itself and advances it 20 ms at a time: it publishes a step's inputs, waits until the validator's output has arrived and a little longer, moves the clock on, and waits for the filter's output, each wait with a time limit. These bounded waits reduce how much the results depend on the computer's speed, although they can't prove that the filter received each input before the clock moved. The wheels report a yaw rate \(\omega_z\) of 0.4 rad/s, as with a wheel separation that is too large, and the IMU 0.5 rad/s: the estimate is 0.499 rad/s. The test then sends IMU samples that are 0.5 s old, from the future, repeated or out of order, each reporting 2.0 rad/s. The validator drops them, and the estimate stays with the valid samples, or goes to the wheels' 0.4 rad/s when only old samples come. After the clock goes back, the filter starts over. The validator's counts on the topic /diagnostics have to match the samples the test sent.
  • In Gazebo: sim_ekf_diffbot.test and sim_ekf_remo.test give the drive controller a wheel separation 25 % larger than the robot's. The controller then turns the robot 25 % more than commanded, while its odometry reports the commanded turn. In a 3 s left turn, DiffBot turned 1.849 rad, its odometry said 1.479 rad and the EKF 1.844 rad. Remo turned 1.873 rad, its odometry said 1.499 rad and the EKF 1.870 rad. These numbers are from a run in a container of the dev container's image, at the commit 3ef4a91 of the pull request EKF: fuse the wheel odometry with the IMU's yaw rate (diffbot#154). The test also checks that the controller's wheel separation is 1.25 times the robot's, and that only the EKF publishes the transform odom → base_footprint on the topic /tf.
  • The switch: test_ekf_switch.py reads the launch files with roslaunch's loader, without starting them, and checks both positions of ekf and its default: which nodes start, enable_odom_tf, and which odometry the planners read. With ekf:=true, the two bringup files and the two simulation files, diffbot_gazebo's diffbot.launch and diffbot_control's diffbot.launch, also have to start robot_state_publisher. The planners' launch files start no EKF, so the test checks only the odometry topic that their planner reads.

These tests don't show four things:

Limits

  • A clock that goes back: the filter recovers differently on the robot and in the simulation. In the simulation, the filter starts over. On the robot, the filter ignores each input until the input's stamps have caught up. The reason is robot_localization's code, which starts over only with simulated time, as reset_on_time_jump in "The other settings" shows. So on the robot, a clock that goes back pauses the filter's inputs for as long as the clock went back.
  • The wheel odometry after a reset of the simulation: the drive controller's odometry pauses. /gazebo/reset_simulation sets the simulation's clock back to zero. The simulated IMU, the validator and the filter go on from the new time right away. diff_drive_controller publishes again only when the clock has passed last_state_publish_time_, the time of its last message from before the reset, as the excerpt of diff_drive_controller.cpp shows. Until then, the filter gets the IMU's yaw rate but no wheel speeds. In a test run that was reset after 9.7 s, the filter's first output after the reset was stamped 0.08 s, and the wheel odometry's first message 9.75 s.
  • The poses after a reset of the simulation: the two pose estimates don't start again together. The EKF's reset clears the filter's state, so the EKF's position \(x\), \(y\) starts again at 0 m and its heading \(\theta\) at 0 rad. The EKF doesn't take over the drive controller's pose. The reset doesn't restart the drive controller, and the drive controller doesn't clear the pose that its odometry has added up. So a reset of the simulation is not a fresh start for both odometries, and the figure of the odom frame describes a fresh start only. These statements follow from the code and are not measured in a run, as the collapsed box says.

    The code behind the two poses after a reset

    The statements follow from the functions RosFilter::reset() and FilterBase::reset() of robot_localization 2.7.7, the class Odometry of ros_controllers 0.22.0 with its methods in odometry.cpp, and GazeboRosControlPlugin::Reset() of gazebo_ros_pkgs 2.9.3.

  • Starting values: the robot's variances and the validator's time limits are not measured yet. These are the variances that the firmware and the drive controller publish on the robot, and the IMU sample validator's max_age and max_future. The bench checks measured the IMU's noise at rest only, and measurements while driving still have to show the values that the EKF should use. So on the robot, the filter's weights of the two inputs are a first choice. In the simulation, the IMU's variance describes the Gaussian noise that the simulated sensor adds.

  • Drift: the pose drifts over time, as with any odometry. The gyroscope helps with the heading, but no input measures the position independently of the wheels, so the position's drift remains. A map-based localization such as amcl corrects the drift in the map frame.

More about robot_localization

Comments

Comments are GitHub Discussions, shown with giscus. Loading them connects to giscus.app (hosted by Vercel, USA) and GitHub (USA); you need a GitHub account to comment.