Skip to content

Test: The EKF on a Stepped Clock

test_ekf_synthetic.py runs the two nodes of ekf.launch, the IMU sample validator and robot_localization's EKF, with the parameters that the robot and the simulation use. The test computes their inputs itself: wheel odometry and IMU samples. The test also owns the clock. It publishes the topic /clock and moves the ROS time \(t\) on in steps of 20 ms. So the test decides which messages are published, with which stamps and at which ROS time, without Gazebo and without a robot.

One step of the clock

The ROS time of the nodes is the time of the last message on the topic /clock. So the ROS time stands still between two of the test's clock messages, while the computer's own clock, the wall clock, runs on. The test can wait on the wall clock as long as a step needs, and no ROS time passes for the nodes. A step at the start of a scenario, at the ROS time \(t\) = 100.00 s, the test's constant START, runs in five parts:

  1. The inputs: the test publishes a wheel message and an IMU sample, both stamped 100.00 s.
  2. The validator: the test waits on the wall clock until the validator's output, the forwarded IMU sample, has reached the test. The ROS time stays 100.00 s.
  3. The delivery: the test sleeps DELIVERY, 20 ms of wall time, so that the inputs reach the EKF too. The ROS time still stays 100.00 s.
  4. The clock: the test publishes the ROS time 100.02 s, which is STEP, 20 ms, later. The EKF updates 50 times per second of ROS time, so the new clock lets the EKF run its update at 100.02 s, which fuses the two inputs.
  5. The output: the EKF publishes its output at the ROS time 100.02 s, stamped 100.00 s, the time of the inputs that it fused. The test waits on the wall clock until an output arrives that is stamped from the step's 100.00 s up to the clock's 100.02 s.
One step of the stepped-clock test, from top to bottom: the test publishes the wheel odometry and an IMU sample, the validator forwards the valid sample to the EKF and to the test, the test waits and publishes the new clock, and the EKF publishes its output, the filtered odometry The test node, test_ekf_synthetic.py test The IMU sample validator, the node imu_validator validator robot_localization's EKF, the node ekf_localization EKF 1 wheel odometry stamped t 1 IMU sample stamped t 2 valid sample 2 valid sample wall clock runs 3 waits DELIVERY 20 ms of wall time ROS time stays t 4 new clock ROS time t + 20 ms 5 EKF output stamped t to t + 20 ms
  • 1 publish_inputs()
  • 2 wait_for_forwarded()
  • 3 DELIVERY
  • 4 advance_clock()
  • 5 step()
One step of the test with a valid IMU sample, from top to bottom. t is the ROS time of the step. The numbers are those of the list in the docstring of step(). The key below the drawing names the code of each number.
How to read the figure

The figure shows the messages of this step. Read the figure from top to bottom. The three boxes are the test, the validator and the EKF. Each arrow is a message, from the node that publishes it to the node that receives it. The label above an arrow names the message, and the note below the arrow says how the message is stamped or which clock runs. \(t\) is the ROS time of the step. The numbers are those of the list in the docstring of step(), and the key below the drawing names the code of each number.

Run the test

Each scenario has a .test file of its own, so each starts with a fresh EKF and a fresh validator. To run one scenario, open a shell in which the workspace is sourced:

1
2
rostest diffbot_control ekf_synthetic.test
rostest diffbot_control ekf_synthetic_robot_rates.test

A pass ends with * RESULT: SUCCESS under SUMMARY. catkin registers both files with the package's tests, and CI runs them on every pull request, as Testing and CI describes. Runs shows what a run prints.

What a pass proves and what a failure means

Each of the two scenarios is one test that runs its phases in order on one filter, from the filter's start:

  • invalid_imu_samples: a wheel message in every step, 50 times per second of ROS time, as in the simulation, and IMU samples in phases 1 to 6. A pass proves that the estimate of the yaw rate \(\omega_z\) follows the IMU while valid samples arrive, goes to the wheels' yaw rate when none arrive, keeps out the samples that the validator drops, and follows the IMU again after the clock went back. The validator's counts on /diagnostics match the samples that the test sent.
  • robot_rates: the robot's nominal rates: a wheel message in every fifth step, and an IMU sample in every step until the IMU stops in phase 2. A pass proves that the estimate follows the IMU's yaw rate and the wheels' forward speed \(v_x\), also when the speed changes, and that after the IMU stops, each of the 20 measured wheel periods of 100 ms holds one wheel message and the header stamp of exactly one output. The last output then has the wheels' forward speed and yaw rate.

A failure names the scenario and the assertion. The messages of the phases' checks name their phase, for example "phase 1: the forward speed comes from the wheels". step() fails with "no EKF output for the step at" and the step's ROS time when the test receives no new output stamped from the step's time to the clock's new time within WAIT, 5 s of wall time.

What the test doesn't show

  • The rates on the robot: robot_rates uses the robot's nominal rates, not measured ones.
  • Whether sensor_timeout fits the robot: the scenario runs with one wheel message per period, without jitter.
  • The two clocks of the robot's inputs: in the test, a step's wheel message and its valid IMU sample have the same stamp.
  • Real sensors: the inputs have no noise, no bias and no delay.
  • The pose and the transform: the test checks the speed, the yaw rate and the output's frames.
  • Where the validator's limits lie: the samples that phases 2 and 4 send to be dropped lie far outside the time limits. The unit tests check the boundary.
The details
  • The rates on the robot: the scenario robot_rates uses the robot's nominal rates, the IMU at 50 Hz and the wheel odometry at 10 Hz. These rates are not measured on the robot yet. The issue Base: measure the control loop's rate on the robot, then decide (diffbot#218) measures them.
  • Whether sensor_timeout fits the robot: the scenario runs with the robot's sensor_timeout of 0.1 s, the nominal wheel period. The filter runs its update 50 times per second, frequency in ekf.yaml. An update whose queue holds no new measurement predicts without one, and publishes the prediction, when the time since the filter's last measurement computes to at least sensor_timeout (ros_filter.cpp, lines 680–711). In phase 2, only the wheels report, every 0.1 s. The update at a wheel message's stamp runs before that message is in the filter's queue. In the first period, that update still fuses the last IMU sample, and the next update fuses the wheel message. In 8 of the other 19 periods, the time since the last measurement computes to 0.1 s or a little more at that update, and the update publishes a prediction. In the other 11, it computes to a little less, and the next update, 20 ms later, fuses the wheel message. With a larger value, for example 0.12 s, the update at a wheel message's stamp wouldn't predict, and the next update would fuse the message, if the message is in the queue by then. The price: when all inputs stop, the filter predicts first at the first update at which the time since its last measurement computes to at least 0.12 s. With the last measurement at 103.20 s, 103.32 s minus 103.20 s computes to 0.11999999999999034 s, so the first prediction comes at 103.34 s, 0.14 s after the last measurement. On the robot, the value has to cover the wheel period and the delay of the wheel messages. The issue Base: measure the control loop's rate on the robot, then decide (diffbot#218) measures both and sets the value.
  • The two clocks of the robot's inputs: in the test, a wheel message and a valid IMU sample of the same step have the same stamp. On the robot, the wheel odometry is stamped with the time of the control loop, and the IMU's samples with the time of the Teensy, which rosserial keeps in step with the robot's single-board computer (SBC). The test doesn't show how the filter handles a difference between the two.
  • Real sensors: the inputs have no noise, no bias and no delay. How the real gyroscope behaves is a measurement on the robot, as Limits says.
  • The pose and the transform: the test checks the forward speed \(v_x\) and the yaw rate \(\omega_z\) of the filter's output, and the two frame names in the output's message: header.frame_id has to be odom and child_frame_id base_footprint. assert_fused_estimate() checks them in test_ekf_synthetic.py, which the excerpt in the code of one step shows. It doesn't check the position, the heading \(\theta\) or the transform odom → base_footprint on the topic /tf. The test in Gazebo compares the heading with the simulated robot's true turn.
  • Where the validator's limits lie: the samples that phases 2 and 4 send to be dropped are stamped 0.5 s before or after the step's time, far outside max_age, 0.1 s, and max_future, 0.02 s. So the test shows that the running node drops clearly invalid samples, not where the boundary between valid and invalid lies. The validator's unit tests check the boundary: the validator accepts a sample exactly at a limit and rejects one 1 µs beyond it, as Check 4: the two time limits shows.

What the test files start

Each .test file includes ekf_synthetic.launch, which starts the two nodes of ekf.launch with the ROS time from the test, DiffBot's URDF model and robot_state_publisher, and runs test_ekf_synthetic.py with the scenario's name.

The launch file, the two .test files and how the test node picks its scenario

Each scenario has a .test file of its own, a launch file that rostest reads. rostest starts a ROS master and the nodes that the file declares, and runs the test node. So each scenario starts with a fresh EKF and a fresh validator, and no scenario sees what another one left in the filter.

Both .test files include the same nodes from ekf_synthetic.launch.

The nodes of both scenarios: ekf_synthetic.launch:

The excerpt shows the whole file, from ekf_synthetic.launch, lines 1–12:

ekf_synthetic.launch
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
<launch>
    <!-- For the stepped-clock tests of test_ekf_synthetic.py: the EKF of ekf.launch
         (imu_validator.py and robot_localization), which the test feeds, on the simulated clock
         that the test publishes. DiffBot's URDF gives the IMU's frame: its robot_state_publisher
         publishes the fixed joints, imu_joint among them, as on the robot.
         Included by ekf_synthetic.test and ekf_synthetic_robot_rates.test -->
    <param name="use_sim_time" value="true"/>
    <param name="diffbot/robot_description"
           command="$(find xacro)/xacro '$(find diffbot_description)/urdf/diffbot.urdf.xacro'"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" ns="diffbot"/>
    <include file="$(find diffbot_control)/launch/ekf.launch"/>
</launch>
  • use_sim_time: every node takes the ROS time from the topic /clock. No simulator runs here, so the only publisher of that topic is the test.
  • diffbot/robot_description: DiffBot's URDF model, which xacro builds from diffbot.urdf.xacro.
  • robot_state_publisher: publishes the fixed joints of this URDF model, the joint imu_joint among them. From them, TF knows where the IMU's frame imu_link sits on base_footprint, as on the robot. The EKF needs this transform to use the IMU's samples.
  • ekf.launch: starts the nodes /diffbot/imu_validator and /diffbot/ekf_localization with the parameters of the robot: the validator's max_age and max_future, and ekf.yaml for the filter.

The first .test file runs the scenario invalid_imu_samples, the second the scenario robot_rates.

The two .test files:

The first excerpt shows the whole file of invalid_imu_samples, from ekf_synthetic.test, lines 1–9:

ekf_synthetic.test
1
2
3
4
5
6
7
8
9
<launch>
    <!-- The EKF of ekf_synthetic.launch with the wheel odometry and the IMU at 50 Hz, as in the
         simulation, and IMU samples that the validator has to drop: test_ekf_synthetic.py's
         scenario invalid_imu_samples -->
    <include file="$(find diffbot_control)/test/ekf_synthetic.launch"/>

    <test test-name="ekf_synthetic" pkg="diffbot_control" type="test_ekf_synthetic.py"
          args="--scenario invalid_imu_samples" time-limit="120"/>
</launch>

The second excerpt shows the whole file of robot_rates, from ekf_synthetic_robot_rates.test, lines 1–9:

ekf_synthetic_robot_rates.test
1
2
3
4
5
6
7
8
9
<launch>
    <!-- The EKF of ekf_synthetic.launch at the robot's rates, the IMU at 50 Hz and the wheel
         odometry at 10 Hz, and with the IMU stopped: test_ekf_synthetic.py's scenario
         robot_rates. A .test file of its own starts the scenario on a fresh EKF -->
    <include file="$(find diffbot_control)/test/ekf_synthetic.launch"/>

    <test test-name="ekf_synthetic_robot_rates" pkg="diffbot_control" type="test_ekf_synthetic.py"
          args="--scenario robot_rates" time-limit="120"/>
</launch>
  • The <include> tag: starts the nodes of ekf_synthetic.launch.
  • The <test> tag: runs test_ekf_synthetic.py as the test node. args gives the node the argument --scenario with the scenario's name. time-limit gives the test 120 s of wall time.

The test node picks the scenario's class by this argument.

How the test node picks its scenario:

The excerpt shows the table of the scenarios and the start of the node, from test_ekf_synthetic.py, lines 801–822:

test_ekf_synthetic.py: SCENARIOS, scenario_from_arguments() and the start
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
# The scenarios, by the name that a .test file gives the test node with --scenario
SCENARIOS: Dict[str, Type[SteppedClockTestCase]] = {
    'invalid_imu_samples': TestEkfWithInvalidImuSamples,
    'robot_rates': TestEkfAtTheRobotRates,
}


def scenario_from_arguments(arguments: Sequence[str]) -> Type[SteppedClockTestCase]:
    """Return the scenario that the argument --scenario names.

    arguments may hold others, such as the results file that rostest gives each test node.
    """
    parser = argparse.ArgumentParser()
    parser.add_argument('--scenario', choices=sorted(SCENARIOS), required=True)
    known_arguments, _ = parser.parse_known_args(arguments)
    return SCENARIOS[known_arguments.scenario]


if __name__ == '__main__':
    scenario = scenario_from_arguments(rospy.myargv()[1:])
    rospy.init_node('test_ekf_synthetic')
    rostest.rosrun('diffbot_control', 'test_ekf_synthetic', scenario)
  1. rospy.myargv() returns the node's arguments without ROS's remapping arguments, such as the node's name.
  2. scenario_from_arguments() reads --scenario with Python's argparse. parse_known_args() leaves the other arguments alone, such as the path of the result file that rostest adds. The dictionary SCENARIOS maps the name to the scenario's class.
  3. rospy.init_node() starts the test node, and rostest.rosrun() runs the test method of that class and writes its result.

diffbot_control registers both files with add_rostest() in its CMakeLists.txt, so CI runs them with the workspace's other tests, as Testing and CI describes.

The inputs

A wheel message usually reports WHEEL_INPUT, 0.1 m/s and 0.4 rad/s. A valid IMU sample, VALID_SAMPLE, is in the frame imu_link, is stamped with the step's ROS time and reports 0.5 rad/s. A sample that the validator has to drop is in the same frame and reports 2.0 rad/s. Its stamp is too old, repeats the valid sample's stamp, lies in the future or is older than the last valid one. The messages have no noise. Which inputs a step sends depends on the phase, as the tables of invalid_imu_samples and robot_rates list.

The constants, the wheel message and the IMU samples

The test publishes on the two topics that the nodes read on the robot. The wheel odometry goes to the EKF on the topic /diffbot/mobile_base_controller/odom. The IMU samples go to the validator on the topic /diffbot/imu/data_raw. The test subscribes to the validator's output, the topic /diffbot/imu/data_validated, to the filter's output, the topic /diffbot/odometry/filtered, and to the topic /diagnostics.

The constants of test_ekf_synthetic.py:

The file's timing constants come in four groups:

The excerpt shows the timing, topic and test constants of the file, from test_ekf_synthetic.py, lines 49–132:

test_ekf_synthetic.py: the constants
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
CLOCK_TOPIC = '/clock'
WHEEL_ODOMETRY_TOPIC = '/diffbot/mobile_base_controller/odom'
RAW_IMU_TOPIC = '/diffbot/imu/data_raw'
VALIDATED_IMU_TOPIC = '/diffbot/imu/data_validated'
FILTERED_TOPIC = '/diffbot/odometry/filtered'
DIAGNOSTICS_TOPIC = '/diagnostics'
QUEUE_SIZE = 100  # messages buffered per topic between callbacks
CLOCK_QUEUE_SIZE = 10  # messages

STEP = rospy.Duration(0, 20000000)  # 20 ms
START = rospy.Time(100, 0)
RESTART = rospy.Time(50, 0)  # where the clock goes back to in phase 6
WAIT = 5.0  # s of wall time for one step's messages
DELIVERY = 0.02  # s of wall time for the inputs to arrive before the clock moves on
POLL_INTERVAL = 0.001  # s of wall time between two checks of a condition that wait_for() waits for
NO_OUTPUT_WAIT = 10 * DELIVERY  # s of wall time for an EKF output that may not come
TF_DELIVERY = 1.0  # s of wall time for the EKF's own TF listener to get the latched transform
RESET_DELIVERY = 0.5  # s of wall time for the clock's jump back to reach the nodes
RESET_STEPS = 5  # steps within which the EKF publishes again after the clock went back

# The robot's wheel odometry period: diffbot_base's control loop runs the drive controller at
# 10 Hz, which publishes at most once per run. The IMU's period on the robot is STEP
ROBOT_WHEEL_ODOMETRY_PERIOD = rospy.Duration(0, 100000000)  # 100 ms
STEPS_PER_WHEEL_MESSAGE = round(ROBOT_WHEEL_ODOMETRY_PERIOD.to_sec() / STEP.to_sec())  # 5

# Steps per phase, 20 ms each
FUSION_STEPS = 150
PHASE_STEPS = 50
AFTER_RESET_STEPS = 100
# Wheel periods of STEPS_PER_WHEEL_MESSAGE steps after the IMU stopped. The first step of each
# period publishes a wheel message
IMU_STOPPED_WHEEL_PERIODS = 20
# s of ROS time without IMU samples before phase 7 reads the diagnostics: longer than the 1 s
# period of the validator's diagnostics
DIAGNOSTICS_WAIT = rospy.Duration(1.5)

SPEED = 0.1  # m/s, forward, from the wheels
WHEEL_RATE = 0.4  # rad/s
IMU_RATE = 0.5  # rad/s
BAD_RATE = 2.0  # rad/s, in the samples the validator has to drop
# s, the stamps of the samples to drop relative to the step's time
STALE_OFFSET = -0.5
FUTURE_OFFSET = 0.5
OLDER_OFFSET = -0.01

# The inputs' variances, of the size of the robot's: diffbot_control.yaml's
# twist_covariance_diagonal and diffbot_base_config.h's IMU variances. The test doesn't read
# those files
WHEEL_VARIANCES = [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
IMU_ANGULAR_VELOCITY_VARIANCE = 4.0e-4  # (rad/s)²
IMU_LINEAR_ACCELERATION_VARIANCE = 1.0e-2  # (m/s²)²
# In a 6x6 covariance stored row by row, the diagonal's elements are 7 apart
DIAGONAL_STRIDE = 7
VARIANCE_INDICES = (0, 4, 8)  # the diagonal of a 3x3 covariance stored row by row
ORIENTATION_UNAVAILABLE = -1.0  # REP 145: orientation_covariance[0] = -1
GRAVITY = 9.81  # m/s², on the upward axis at rest

# Tolerances of the estimate
SPEED_TOLERANCE = 0.002  # m/s
YAW_RATE_TOLERANCE = 0.01  # rad/s
# rad/s, for the estimate that approaches the wheels' rate after the IMU samples stop passing
WHEEL_RATE_TOLERANCE = 0.02

# robot_rates' phase 1 changes the wheels' forward speed from SPEED to CHANGED_SPEED with the wheel
# message of this step, the 21st, 2 s into the phase
SPEED_CHANGE_STEP = 100
# m/s, SPEED plus ten times SPEED_TOLERANCE. The filter estimates an acceleration from the change,
# which moves the estimate between two wheel messages: by less than SPEED_TOLERANCE for this change
CHANGED_SPEED = 0.12
# Steps from SPEED_CHANGE_STEP on within which the estimate has to reach CHANGED_SPEED: the second
# wheel message after the change brings it within SPEED_TOLERANCE, one more step allows for a late
# message
SPEED_CHANGE_WINDOW = STEPS_PER_WHEEL_MESSAGE + 1  # 6

# The validator's status on the topic /diagnostics contains this name, and its values have these
# keys (scripts/imu_validator.py). The texts are written out here and not imported from the
# script: the test pins what a user reads on that topic
VALIDATOR_STATUS_NAME = 'IMU sample validator'
FORWARDED_KEY = 'Samples forwarded'
DROPPED_KEY_PREFIX = 'Dropped, '
# The validator's reasons, as its diagnostics name them
TOO_OLD = 'too old'
NOT_NEWER = 'not newer than the last one'
FROM_THE_FUTURE = 'from the future'

What the wheels report in a wheel message is one value of the type WheelInput. The excerpt shows the type and its two values, from test_ekf_synthetic.py, lines 149–158:

test_ekf_synthetic.py: WheelInput, WHEEL_INPUT and CHANGED_SPEED_WHEEL_INPUT
149
150
151
152
153
154
155
156
157
158
class WheelInput(NamedTuple):
    """The wheel odometry that a step publishes: what the wheels report."""

    speed: float  # m/s, forward
    yaw_rate: float  # rad/s


# What the wheels report in every wheel message, apart from robot_rates' phase 1 after its change
WHEEL_INPUT = WheelInput(SPEED, WHEEL_RATE)
CHANGED_SPEED_WHEEL_INPUT = WheelInput(CHANGED_SPEED, WHEEL_RATE)
  • speed: the forward speed \(v_x\) that the wheels report.
  • yaw_rate: the yaw rate \(\omega_z\) that the wheels report.
  • WHEEL_INPUT: SPEED, 0.1 m/s, and WHEEL_RATE, 0.4 rad/s. Every wheel message of the test reports these two values, apart from the second part of phase 1 of robot_rates.
  • CHANGED_SPEED_WHEEL_INPUT: CHANGED_SPEED, 0.12 m/s, and WHEEL_RATE. Phase 1 of robot_rates sends this value from its 21st wheel message on.

Two functions build the messages. The excerpt shows both, from test_ekf_synthetic.py, lines 209–238:

test_ekf_synthetic.py: wheel_odometry() and imu_sample()
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
235
236
237
238
def wheel_odometry(stamp: rospy.Time, wheel_input: WheelInput) -> Odometry:
    """Return the drive controller's odometry at stamp, for wheels that report wheel_input."""
    message = Odometry()
    message.header.stamp = stamp
    message.header.frame_id = 'odom'
    message.child_frame_id = 'base_footprint'
    message.pose.pose.orientation.w = 1.0
    message.twist.twist.linear.x = wheel_input.speed
    message.twist.twist.angular.z = wheel_input.yaw_rate
    for axis, variance in enumerate(WHEEL_VARIANCES):
        message.pose.covariance[DIAGONAL_STRIDE * axis] = variance
        message.twist.covariance[DIAGONAL_STRIDE * axis] = variance
    return message


def imu_sample(stamp: rospy.Time, yaw_rate: float) -> Imu:
    """Return a sample as the firmware publishes it, in DiffBot's imu_link.

    DiffBot's imu_link is aligned with base_footprint.
    """
    message = Imu()
    message.header.stamp = stamp
    message.header.frame_id = 'imu_link'
    message.orientation_covariance[0] = ORIENTATION_UNAVAILABLE
    message.angular_velocity.z = yaw_rate
    message.linear_acceleration.z = GRAVITY
    for index in VARIANCE_INDICES:
        message.angular_velocity_covariance[index] = IMU_ANGULAR_VELOCITY_VARIANCE
        message.linear_acceleration_covariance[index] = IMU_LINEAR_ACCELERATION_VARIANCE
    return message
  • wheel_odometry(): a nav_msgs/Odometry message as the drive controller publishes it, in the frame odom with the child frame base_footprint. The forward speed \(v_x\) and the yaw rate \(\omega_z\) are those of the argument wheel_input. The variances \(\sigma^2\) on the diagonals of both covariances are WHEEL_VARIANCES, with 0.03 (rad/s)² for the yaw rate. These are the numbers of diffbot_control.yaml, which the test writes out and doesn't read from the file.
  • imu_sample(): a sensor_msgs/Imu message as the firmware publishes it, in the frame imu_link. The orientation is marked unavailable, the yaw rate \(\omega_z\) is the function's argument, and the upward acceleration \(f_z\) is GRAVITY. The variance of the angular velocity is IMU_ANGULAR_VELOCITY_VARIANCE, 4 · 10⁻⁴ (rad/s)², the firmware's number, written out in the test too.
  • No noise: the test sets every value of both messages itself, the same in every step of a phase apart from the speed change of robot_rates, and without noise. So what the test sees in the estimate comes from the filter and the validator, not from chance.

A step can publish several IMU samples. The class ImuInput describes one of them. The excerpt shows the class and the sample that most steps use, from test_ekf_synthetic.py, lines 137–146:

test_ekf_synthetic.py: ImuInput and VALID_SAMPLE
137
138
139
140
141
142
143
144
145
146
class ImuInput(NamedTuple):
    """An IMU sample that a step publishes, and what the validator has to do with it."""

    offset: float  # s, the sample's stamp relative to the step's time
    yaw_rate: float  # rad/s
    drop_reason: Optional[str]  # None for a sample the validator has to forward


# A valid sample stamped at the step's time, which the validator has to forward
VALID_SAMPLE = ImuInput(0.0, IMU_RATE, None)
  • offset: the sample's stamp \(t_s\) relative to the step's ROS time \(t\), in seconds.
  • yaw_rate: the yaw rate \(\omega_z\) that the sample reports.
  • drop_reason: None for a sample that the validator has to forward. For a sample that the validator has to drop, the reason as the validator's diagnostics name it, one of the texts of DropReason.
  • VALID_SAMPLE: stamped with the step's time, reporting IMU_RATE, 0.5 rad/s.

Before the first step

Before the first step, setUp() subscribes to the validator's and the EKF's outputs, publishes the first clock message, and waits until every node is connected. wait_until() and wait_for() poll a condition on the wall clock.

setUp(), the arrival log and the waits

The class SteppedClockTestCase holds what both scenarios share: the topics, the steps of the clock and the comparisons. Each scenario is a subclass with one test method. unittest calls setUp() before the test method. In short, setUp() sets the test's ROS time to 100 s, creates the topics' publishers and subscribers, publishes the first clock message, and waits until the validator and the EKF are connected and TF has the IMU's transform.

setUp(), on_validated() and all_connected():

The excerpt shows setUp() and its two helpers on_validated() and all_connected(), from test_ekf_synthetic.py, lines 266–305:

test_ekf_synthetic.py: setUp(), on_validated() and all_connected()
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
def setUp(self) -> None:
    self.now = START
    # The stamps of the samples that the validator forwarded
    self.forwarded_stamp_keys: ArrivalLog[StampKey] = ArrivalLog()
    self.ekf_outputs: ArrivalLog[Odometry] = ArrivalLog()
    self.diagnostics: ArrivalLog[DiagnosticArray] = ArrivalLog()
    self.expected_forward_count = 0
    self.expected_drops: Counter[str] = collections.Counter()
    self.clock_publisher = rospy.Publisher(
        CLOCK_TOPIC, Clock, queue_size=CLOCK_QUEUE_SIZE, latch=True)
    self.wheel_odometry_publisher = rospy.Publisher(
        WHEEL_ODOMETRY_TOPIC, Odometry, queue_size=QUEUE_SIZE)
    self.raw_imu_publisher = rospy.Publisher(RAW_IMU_TOPIC, Imu, queue_size=QUEUE_SIZE)
    self.validated_imu_subscriber = rospy.Subscriber(
        VALIDATED_IMU_TOPIC, Imu, self.on_validated, queue_size=QUEUE_SIZE)
    self.filtered_odometry_subscriber = rospy.Subscriber(
        FILTERED_TOPIC, Odometry, self.ekf_outputs.append, queue_size=QUEUE_SIZE)
    self.diagnostics_subscriber = rospy.Subscriber(
        DIAGNOSTICS_TOPIC, DiagnosticArray, self.diagnostics.append, queue_size=QUEUE_SIZE)
    self.clock_publisher.publish(Clock(self.now))
    buffer = tf2_ros.Buffer()
    self.listener = tf2_ros.TransformListener(buffer)
    # Before the first input: the EKF and the validator listen, and the production URDF's
    # imu_link transform is there. A paused clock doesn't advance ROS-time waits, so these
    # wait on the wall clock
    wait_for(self.all_connected, "the EKF and the validator didn't connect")
    wait_for(lambda: buffer.can_transform('base_footprint', 'imu_link', rospy.Time(0)),
             'no transform from imu_link to base_footprint (robot_state_publisher)')
    time.sleep(TF_DELIVERY)

def on_validated(self, message: Imu) -> None:
    """Note the stamp of a sample that the validator forwarded."""
    self.forwarded_stamp_keys.append(stamp_key(message.header.stamp))

def all_connected(self) -> bool:
    """Return whether the EKF and the validator subscribe and publish to the test."""
    return (self.wheel_odometry_publisher.get_num_connections() > 0
            and self.raw_imu_publisher.get_num_connections() > 0
            and self.validated_imu_subscriber.get_num_connections() > 0
            and self.filtered_odometry_subscriber.get_num_connections() > 0)
  1. setUp() sets the test's own ROS time self.now to START, 100 s, and creates the three logs and the two counters that the phases fill. self.forwarded_stamp_keys holds the stamps of the samples that the validator forwarded, self.ekf_outputs the filter's outputs, and self.diagnostics the messages of the topic /diagnostics. self.expected_forward_count and self.expected_drops count what the validator has to do with each sample that the test sends.
  2. setUp() creates the three publishers and the three subscribers. self.clock_publisher is latched, so a node that connects later still gets the last clock message. Each subscriber's callback appends what arrives to one of the three logs. For the validator's output, the callback is on_validated(), which appends the sample's stamp.
  3. setUp() publishes the clock for the first time, at START.
  4. setUp() waits until the validator and the EKF are connected to the test's topics, with all_connected(), and until TF has the transform from imu_link to base_footprint. Then it sleeps TF_DELIVERY, 1 s of wall time, so that the EKF's own TF listener has the transform too.

The three logs are objects of the class ArrivalLog. rospy calls a subscriber's callback in a thread of its own, while the test's thread reads what arrived. A lock guards every access, so the test's thread never reads a log while a callback changes it.

ArrivalLog:

The excerpt shows the class, from test_ekf_synthetic.py, lines 164–194:

test_ekf_synthetic.py: ArrivalLog
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
class ArrivalLog(Generic[Item]):
    """What arrived on a topic, in the order of arrival.

    rospy calls a subscriber's callback in a thread of its own, which appends to the log, while
    the test's thread reads it. A lock guards every access, so that the test's thread never
    reads the log while the callback changes it.
    """

    def __init__(self) -> None:
        self._lock = threading.Lock()
        self._items: List[Item] = []

    def append(self, item: Item) -> None:
        """Add an item at the end."""
        with self._lock:
            self._items.append(item)

    def count(self) -> int:
        """Return the number of items so far."""
        with self._lock:
            return len(self._items)

    def since(self, count: int) -> List[Item]:
        """Return a copy of the items after the first count."""
        with self._lock:
            return self._items[count:]

    def contains_all(self, items: Sequence[Item]) -> bool:
        """Return whether each of the items has arrived."""
        with self._lock:
            return all(item in self._items for item in items)
  • append(): adds an item at the end. The subscribers of the filter's output and of the topic /diagnostics have it as their callback, and on_validated() calls it.
  • count(): returns the number of items so far. The test takes this number before a step.
  • since(): returns a copy of the items after the first count items. With the number from before a step, the test gets what arrived since the step started.
  • contains_all(): returns whether each of the given items has arrived.

The clock stands still during setUp(). A wait on the ROS time would never end, so the test waits on the wall clock with wait_for(). The excerpt shows wait_for() and the function wait_until() that it calls, from test_ekf_synthetic.py, lines 241–254:

test_ekf_synthetic.py: wait_until() and wait_for()
241
242
243
244
245
246
247
248
249
250
251
252
253
254
def wait_until(condition: Callable[[], bool]) -> bool:
    """Wait at most WAIT s of wall time for condition() to be true, and return whether it is."""
    deadline = time.monotonic() + WAIT
    while not condition():
        if time.monotonic() > deadline:
            return False
        time.sleep(POLL_INTERVAL)
    return True


def wait_for(condition: Callable[[], bool], failure: str) -> None:
    """Wait on the wall clock until condition() is true, at most WAIT s, else fail with failure."""
    if not wait_until(condition):
        raise AssertionError(failure)
  • wait_until(): checks condition() every POLL_INTERVAL, 1 ms, and returns True as soon as the condition holds, or False after WAIT, 5 s.
  • wait_for(): fails the test with the text failure when wait_until() returns False.

The code of one step

step() runs the five parts of one step of the clock and returns the step's last output. Its helpers publish the inputs, wait for the validator and the delivery, advance the clock and collect the outputs. The comparisons of an output are assert_fused_estimate() and its helpers.

step() and its helpers

Every phase is a number of steps, as One step of the clock describes them. 20 ms is the period of the IMU at 50 Hz. The excerpt shows advance(), outputs_since() and step(), from test_ekf_synthetic.py, lines 343–384. The docstring of step() lists the five things that one step does, and advance() does the first four:

test_ekf_synthetic.py: advance(), outputs_since() and step()
343
344
345
346
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
375
376
377
378
379
380
381
382
383
384
def advance(self, wheel_input: Optional[WheelInput],
            imu_inputs: Sequence[ImuInput]) -> StepStart:
    """Do 1 to 4 of a step, as step() lists them, and return where the step started."""
    start = StepStart(self.now, self.ekf_outputs.count())
    valid_stamp_keys = self.publish_inputs(start.stamp, wheel_input, imu_inputs)
    self.wait_for_forwarded(valid_stamp_keys, start.stamp)
    time.sleep(DELIVERY)
    self.advance_clock()
    return start

def outputs_since(self, start: StepStart) -> List[Odometry]:
    """Return the EKF's outputs that arrived after the step's start and belong to the step.

    An output belongs to the step if its stamp lies between the step's time and the ROS time
    that the test has published. An output that arrives late from before the clock went back
    carries a later stamp, and one from an earlier step an earlier stamp.
    """
    return [message for message in self.ekf_outputs.since(start.output_count_before)
            if start.stamp <= message.header.stamp <= self.now]

def step(self, wheel_input: Optional[WheelInput],
         imu_inputs: Sequence[ImuInput] = ()) -> Odometry:
    """Run one step and return the EKF's last output for it, which has to come.

    One step:
    1. Publish the step's wheel odometry, if wheel_input isn't None, stamped with the step's
       ROS time, and its IMU samples, each stamped with that time plus the sample's offset
       (publish_inputs()).
    2. Wait on the wall clock until the IMU sample validator's output for the step's valid
       samples reaches the test (wait_for_forwarded()). The EKF subscribes to the same
       topic.
    3. Wait DELIVERY more, for the inputs to arrive at the EKF.
    4. Advance the clock by STEP (advance_clock()). The EKF's 50 Hz update then processes the
       inputs.
    5. Wait, with the time limit WAIT, for an EKF output that belongs to the step
       (outputs_since()).
    advance() does 1 to 4.
    """
    start = self.advance(wheel_input, imu_inputs)
    wait_for(lambda: bool(self.outputs_since(start)),
             'no EKF output for the step at {:.2f} s'.format(start.stamp.to_sec()))
    return self.outputs_since(start)[-1]

The five things of one step, as the docstring of step() numbers them:

  1. Publish the inputs: publish_inputs() publishes the step's wheel odometry, stamped with the step's ROS time, and its IMU samples, each stamped with that time plus the sample's offset. A step whose wheel_input is None publishes no wheel odometry. The scenario robot_rates uses such steps.
  2. Wait for the validator: wait_for_forwarded() waits on the wall clock until the validator's output for the step's valid samples reached the test. The EKF subscribes to the same topic.
  3. Wait DELIVERY: advance() sleeps DELIVERY, 20 ms of wall time, so that the inputs reach the EKF before the clock moves.
  4. Advance the clock: advance_clock() adds STEP to self.now and publishes the new clock. The EKF's update runs 50 times per second of ROS time, so the new clock lets the filter process the step's inputs.
  5. Wait for the EKF: step() waits until the filter published an output that belongs to the step, at most WAIT, 5 s of wall time. step() returns the last such output.

advance() returns a StepStart, which says where the step started. The excerpt shows the class, from test_ekf_synthetic.py, lines 197–201:

test_ekf_synthetic.py: StepStart
197
198
199
200
201
class StepStart(NamedTuple):
    """Where a step started: what tells the step's EKF outputs from the earlier ones."""

    stamp: rospy.Time  # the step's ROS time
    output_count_before: int  # the number of EKF outputs that the test had before the step
  • stamp: the step's ROS time. advance() reads it from self.now before the clock moves.
  • output_count_before: the number of EKF outputs that the test had before the step, from self.ekf_outputs.count().
  • outputs_since(): takes the StepStart and returns the EKF's outputs that arrived after the first output_count_before and are stamped from the step's time up to the ROS time that the test has published, self.now. So an output of an earlier step doesn't count for this step, with its earlier stamp. Neither does an output from before the clock went back that arrives late: its stamp lies after the new clock.

advance() calls three helpers, and publish_inputs() calls a fourth, count_expected_outcome(). The excerpt shows the four helpers, from test_ekf_synthetic.py, lines 307–341:

test_ekf_synthetic.py: publish_inputs(), count_expected_outcome(), wait_for_forwarded() and advance_clock()
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
def publish_inputs(self, stamp: rospy.Time, wheel_input: Optional[WheelInput],
                   imu_inputs: Sequence[ImuInput]) -> List[StampKey]:
    """Publish one step's wheel odometry and IMU samples, stamped relative to stamp.

    A wheel_input of None publishes no wheel odometry in the step. Return the stamp keys of
    the samples that the validator has to forward.
    """
    if wheel_input is not None:
        self.wheel_odometry_publisher.publish(wheel_odometry(stamp, wheel_input))
    valid_stamp_keys = []
    for imu_input in imu_inputs:
        sample_stamp = stamp + rospy.Duration.from_sec(imu_input.offset)
        self.raw_imu_publisher.publish(imu_sample(sample_stamp, imu_input.yaw_rate))
        self.count_expected_outcome(imu_input)
        if imu_input.drop_reason is None:
            valid_stamp_keys.append(stamp_key(sample_stamp))
    return valid_stamp_keys

def count_expected_outcome(self, imu_input: ImuInput) -> None:
    """Count the sample as one that the validator has to forward or to drop."""
    if imu_input.drop_reason is None:
        self.expected_forward_count += 1
    else:
        self.expected_drops[imu_input.drop_reason] += 1

def wait_for_forwarded(self, stamp_keys: Sequence[StampKey], stamp: rospy.Time) -> None:
    """Wait on the wall clock until the validator forwarded the samples with these stamps."""
    wait_for(lambda: self.forwarded_stamp_keys.contains_all(stamp_keys),
             "the validator didn't forward the IMU samples of the step at {:.2f} s".format(
                 stamp.to_sec()))

def advance_clock(self) -> None:
    """Move the ROS time on by STEP and publish it."""
    self.now += STEP
    self.clock_publisher.publish(Clock(self.now))
  • publish_inputs(): publishes the wheel odometry for wheel_input, stamped with the step's time stamp, unless wheel_input is None. For each ImuInput of the step, it publishes an IMU sample stamped stamp plus the sample's offset, and passes the ImuInput to count_expected_outcome(). It returns valid_stamp_keys, the stamps of the samples that the validator has to forward.
  • count_expected_outcome(): counts what the validator has to do with the sample, the outcome that the test expects. A sample without a drop_reason adds one to self.expected_forward_count. A sample with a drop_reason adds one to that reason in self.expected_drops.
  • wait_for_forwarded(): waits with wait_for() until self.forwarded_stamp_keys contains every stamp of stamp_keys. on_validated() appends to this log the stamp of each sample that arrives on the topic /diffbot/imu/data_validated.
  • advance_clock(): adds STEP to self.now and publishes the new time on the topic /clock.

Two more methods run steps. The excerpt shows step_allowing_no_output() and run_steps(), from test_ekf_synthetic.py, lines 386–397:

test_ekf_synthetic.py: step_allowing_no_output() and run_steps()
386
387
388
389
390
391
392
393
394
395
396
397
def step_allowing_no_output(self, wheel_input: Optional[WheelInput],
                            imu_inputs: Sequence[ImuInput] = ()) -> Optional[Odometry]:
    """Run one step and return the EKF's last output for it, or None if none came."""
    start = self.advance(wheel_input, imu_inputs)
    time.sleep(NO_OUTPUT_WAIT)
    outputs = self.outputs_since(start)
    return outputs[-1] if outputs else None

def run_steps(self, count: int, wheel_input: WheelInput,
              imu_inputs: Sequence[ImuInput] = ()) -> List[Odometry]:
    """Run count steps with the same inputs and return the EKF's output for each."""
    return [self.step(wheel_input, imu_inputs) for _ in range(count)]
  • step_allowing_no_output(): is for steps after which the filter may publish nothing: phase 6 of invalid_imu_samples and phase 2 of robot_rates. It does 1 to 4 with advance(), sleeps NO_OUTPUT_WAIT, 0.2 s of wall time, and returns the step's last output, or None if no output came.
  • run_steps(): runs count steps with the same inputs and returns one output for each step.

The steps use wait_for() for the messages that they need, and wait_for() fails the test if a message is still missing after WAIT, 5 s of wall time. The wait after the measured periods of phase 2 returns what is missing instead, as phase 2 explains. The waits reduce how much the result depends on the computer's speed. They don't prove that the EKF received each input before the clock moved. The checks allow for a late input. In a phase that waits for an output in every step, the filter fuses an input that arrives late one step later, and the checks look at the last output of a phase and at the highest of the outputs that the test keeps in a phase. Phase 2 of robot_rates moves the clock on in every step of its measured wheel periods without waiting for an output, and its check of the outputs allows a late wheel message within its wheel period.

Three helpers hold the checks of the fusion, which both scenarios use in their phase 1. The excerpt shows them, from test_ekf_synthetic.py, lines 412–432:

test_ekf_synthetic.py: assert_forward_speed(), assert_fused_estimate() and print_fusion()
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
def assert_forward_speed(self, output: Odometry, speed: float, what: str) -> None:
    """Assert that the output's forward speed is the wheels' speed. what names the case."""
    self.assertAlmostEqual(output.twist.twist.linear.x, speed, delta=SPEED_TOLERANCE,
                           msg=what + ': the forward speed comes from the wheels')

def assert_fused_estimate(self, output: Odometry, speed: float, what: str) -> None:
    """Assert that output has the EKF's frames, the wheels' speed and the IMU's yaw rate.

    what names the case.
    """
    self.assertEqual(output.header.frame_id, 'odom', what)
    self.assertEqual(output.child_frame_id, 'base_footprint', what)
    self.assert_forward_speed(output, speed, what)
    self.assert_yaw_rate(output, IMU_RATE, YAW_RATE_TOLERANCE,
                         what + ": the IMU's variance is far smaller than the wheels'")

def print_fusion(self, what: str, output: Odometry) -> None:
    """Print the inputs' yaw rates and the output's estimate. what names the case."""
    print('{}: wheels {} rad/s, IMU {} rad/s, estimate {:.4f} rad/s, forward {:.4f} m/s'
          .format(what, WHEEL_RATE, IMU_RATE, output.twist.twist.angular.z,
                  output.twist.twist.linear.x))

The seven phases of invalid_imu_samples

The scenario invalid_imu_samples is the class TestEkfWithInvalidImuSamples, one test on one filter. The figure shows its seven phases from left to right:

The seven phases of the stepped-clock test from left to right: the yaw rate that the wheel odometry and the valid IMU samples report, the samples that the validator has to drop, the ranges that the test accepts for the filter's estimate, and the estimates that a run printed Phase IMU valid IMU to drop 1 Phase 1: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ 2 Phase 2: a sample that reports 2.0 rad/s, in every step. The validator has to drop it.✕ 3 Phase 3: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ Phase 3: a sample that reports 2.0 rad/s, in every step. The validator has to drop it.✕ 4 Phase 4: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ Phase 4: a sample that reports 2.0 rad/s, in every step. The validator has to drop it.✕ 5 Phase 5: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ Phase 5: a sample that reports 2.0 rad/s, in every step. The validator has to drop it.✕ 6 Phase 6: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ 7 none The clock goes back from 107 s to 50 s Yaw rate ωz in rad/s 0.4 0.5 100 103 104 105 52.04 107 50 ROS time t in s Wheel odometry: 0.4 rad/s in every step wheels Phase 1: the valid IMU samples report 0.5 rad/s Phase 3: the valid IMU samples report 0.5 rad/s Phase 4: the valid IMU samples report 0.5 rad/s Phase 5: the valid IMU samples report 0.5 rad/s Phase 6: the valid IMU samples report 0.5 rad/s IMU Phases 2 to 5: no output that the test keeps, one per step, may be above 0.51 rad/s kept outputs at most 0.51 Phase 1: the last output has to be 0.5 ± 0.01 rad/s Phase 2: the last output has to be 0.4 ± 0.02 rad/s Phase 3: the last output has to be 0.5 ± 0.01 rad/s Phase 4: the last output has to be 0.5 ± 0.01 rad/s Phase 5: the last output has to be 0.5 ± 0.01 rad/s Phase 6: the last output has to be 0.5 ± 0.01 rad/s 0.4966 rad/s, the first output of phase 2 in the run at the commit 3882a19 0.4987 rad/s, the last output of phase 1 in the run at the commit 3882a19 0.4005 rad/s, the last output of phase 2 in the run at the commit 3882a19 0.4987 rad/s, the last output of phase 6 in the run at the commit 3882a19 estimate 0.4987 0.4005 0.4987 The seven phases of the stepped-clock test from left to right: the yaw rate that the wheel odometry and the valid IMU samples report, the samples that the validator has to drop, the ranges that the test accepts for the filter's estimate, and the estimates that a run printed Phase IMU valid IMU to drop 1 Phase 1: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ 2 Phase 2: a sample that reports 2.0 rad/s, in every step. The validator has to drop it.✕ 3 Phase 3: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ Phase 3: a sample that reports 2.0 rad/s, in every step. The validator has to drop it.✕ 4 Phase 4: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ Phase 4: a sample that reports 2.0 rad/s, in every step. The validator has to drop it.✕ 5 Phase 5: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ Phase 5: a sample that reports 2.0 rad/s, in every step. The validator has to drop it.✕ 6 Phase 6: a valid sample that reports 0.5 rad/s, in every step. The validator forwards it.✓ 7 none The clock goes back from 107 s to 50 s Yaw rate ωz in rad/s 0.4 0.5 100 103 104 105 52.04 107 50 ROS time t in s Wheel odometry: 0.4 rad/s in every step wheels Phase 1: the valid IMU samples report 0.5 rad/s Phase 3: the valid IMU samples report 0.5 rad/s Phase 4: the valid IMU samples report 0.5 rad/s Phase 5: the valid IMU samples report 0.5 rad/s Phase 6: the valid IMU samples report 0.5 rad/s IMU Phases 2 to 5: no output that the test keeps, one per step, may be above 0.51 rad/s kept outputs at most 0.51 Phase 1: the last output has to be 0.5 ± 0.01 rad/s Phase 2: the last output has to be 0.4 ± 0.02 rad/s Phase 3: the last output has to be 0.5 ± 0.01 rad/s Phase 4: the last output has to be 0.5 ± 0.01 rad/s Phase 5: the last output has to be 0.5 ± 0.01 rad/s Phase 6: the last output has to be 0.5 ± 0.01 rad/s 0.4966 rad/s, the first output of phase 2 in the run at the commit 3882a19 0.4987 rad/s, the last output of phase 1 in the run at the commit 3882a19 0.4005 rad/s, the last output of phase 2 in the run at the commit 3882a19 0.4987 rad/s, the last output of phase 6 in the run at the commit 3882a19 estimate 0.4987 0.4005 0.4987

What each step sends

  • wheel odometry: 0.4 rad/s in every step of every phase
  • IMU: the valid samples report 0.5 rad/s
  • ✓a valid IMU sample in each step of the phase. The validator forwards it
  • ✕an IMU sample that the validator has to drop, in each step of the phase. It reports 2.0 rad/s, which is beyond the chart. Its stamp is 0.5 s before the time of the step in phase 2, the stamp of the valid sample in phase 3, 0.5 s after the time of the step in phase 4, and 0.01 s before the time of the step in phase 5. In phases 3 to 5, the test sends it after the valid sample

What the test asserts

  • the range for the last output of a phase: 0.5 ± 0.01 rad/s, and 0.4 ± 0.02 rad/s in phase 2
  • the limit in phases 2 to 5: no output that the test keeps, one per step, may be above 0.51 rad/s

What the test printed in a run at the diffbot commit 3882a19

  • the filter's estimate in the last output of phase 1, 0.4987 rad/s, of phase 2, 0.4005 rad/s, and of phase 6, 0.4987 rad/s
  • the filter's estimate in the first output of phase 2, 0.4966 rad/s, drawn a little right of the start of phase 2, so that it doesn't hide the dot of phase 1. The run printed only the first and the last estimate of phase 2, so the chart has no line between them
The stepped-clock test from left to right, one step every 20 ms of ROS time. The rows "IMU valid" and "IMU to drop" show the samples of one step of each phase. At the solid vertical line, the clock goes back from 107 s to 50 s.

In every step, the wheel odometry reports 0.1 m/s and 0.4 rad/s. The table lists what changes from phase to phase:

Phase and its steps IMU samples of each step What the test asserts
1: FUSION_STEPS, 150 One valid sample, 0.5 rad/s The last output has the frames odom and base_footprint, \(v_x\) = 0.1 ± 0.002 m/s and \(\omega_z\) = 0.5 ± 0.01 rad/s
2: PHASE_STEPS, 50 One sample stamped 0.5 s before the step, 2.0 rad/s No kept output above 0.51 rad/s. The last output has \(\omega_z\) = 0.4 ± 0.02 rad/s
3: 50 The valid sample, then one with the same stamp, 2.0 rad/s No kept output above 0.51 rad/s. The last output has \(\omega_z\) = 0.5 ± 0.01 rad/s
4: 50 The valid sample, then one stamped 0.5 s after the step, 2.0 rad/s As phase 3
5: 50 The valid sample, then one stamped 0.01 s before the step, 2.0 rad/s As phase 3
6: at most RESET_STEPS, 5, then AFTER_RESET_STEPS, 100 The clock goes back to 50 s. Then one valid sample An output comes within 5 steps. The first of the next 100 outputs is stamped before 100 s. The last output has \(\omega_z\) = 0.5 ± 0.01 rad/s
7: 75 None The validator's counts on the topic /diagnostics are the test's own counts
The test method, how to read the figure, the wheels' input and the two comparisons

The excerpt shows the test method, from test_ekf_synthetic.py, lines 453–460. Each check_...() method is one phase, and each phase needs the state that the phases before it left. The method stops at the first phase that fails:

test_ekf_synthetic.py: the test method of invalid_imu_samples
453
454
455
456
457
458
459
460
def test_ekf_handles_invalid_imu_samples_and_recovers_after_the_clock_goes_back(self) -> None:
    self.check_estimate_follows_the_imu_with_the_smaller_variance()
    self.check_estimate_follows_the_wheels_when_imu_samples_are_stale()
    self.check_validator_drops_samples_with_a_repeated_stamp()
    self.check_validator_drops_samples_from_the_future()
    self.check_validator_drops_samples_older_than_the_last_valid_one()
    self.check_estimate_follows_the_imu_again_after_the_clock_goes_back()
    self.check_validator_diagnostics_count_each_forwarded_and_dropped_sample()

The figure shows the seven phases from left to right, one step every 20 ms of ROS time. The rows "IMU valid" and "IMU to drop" show the samples of one step: ✓ is a valid sample, ✕ is a sample that the validator has to drop. The chart below the rows shows the yaw rate \(\omega_z\) that the wheel odometry and the valid IMU samples report, the range that the test accepts for the last output of each phase, the limit for the outputs that the test keeps in phases 2 to 5, and the estimates that the test printed in a run. The run printed the first and the last estimate of phase 2, so phase 2 has two markers. The three lists below the chart explain each mark and give its numbers.

In every step of every phase, the wheel odometry reports WHEEL_INPUT: SPEED, 0.1 m/s, and WHEEL_RATE, 0.4 rad/s, as with a wheel separation that is too large in the drive controller's configuration.

Two helpers hold the comparisons. The excerpt shows them, from test_ekf_synthetic.py, lines 399–410. assert_yaw_rate() checks one output against expected ± delta. assert_stays_at_most() checks that none of a phase's kept outputs is above limit. The test keeps one output per step, the last one that step() finds when its wait ends:

test_ekf_synthetic.py: the two comparisons
399
400
401
402
403
404
405
406
407
408
409
410
def assert_yaw_rate(self, output: Odometry, expected: float, delta: float, what: str) -> None:
    """Assert that the output's yaw rate is expected ± delta. what names the case."""
    rate = output.twist.twist.angular.z
    self.assertAlmostEqual(rate, expected, delta=delta,
                           msg='{}: estimated {:.4f} rad/s, expected {} ± {}'.format(
                               what, rate, expected, delta))

def assert_stays_at_most(self, outputs: List[Odometry], limit: float, what: str) -> None:
    """Assert that no output's yaw rate exceeds limit. what names the case."""
    highest = max(message.twist.twist.angular.z for message in outputs)
    self.assertLessEqual(highest, limit,
                         '{}: the estimate went up to {:.4f} rad/s'.format(what, highest))

Phase 1: the estimate follows the IMU

The wheels report 0.4 rad/s and the IMU 0.5 rad/s, for 150 steps, 3 s of ROS time.

assert_fused_estimate() checks the last output. The excerpt shows its checks, from test_ekf_synthetic.py, lines 422–426:

test_ekf_synthetic.py: the checks of assert_fused_estimate()
422
423
424
425
426
self.assertEqual(output.header.frame_id, 'odom', what)
self.assertEqual(output.child_frame_id, 'base_footprint', what)
self.assert_forward_speed(output, speed, what)
self.assert_yaw_rate(output, IMU_RATE, YAW_RATE_TOLERANCE,
                     what + ": the IMU's variance is far smaller than the wheels'")
  • The frames: the last output is in the frame odom with the child frame base_footprint, as ekf.yaml sets them.
  • The forward speed \(v_x\): SPEED, 0.1 m/s, within SPEED_TOLERANCE, 0.002 m/s. Only the wheel odometry carries a forward speed, so this check shows that the filter fuses the wheel odometry.
  • The yaw rate \(\omega_z\): IMU_RATE, 0.5 rad/s, within YAW_RATE_TOLERANCE, 0.01 rad/s. The IMU's variance is 75 times smaller than the wheels', so the estimate lies close to the IMU's yaw rate. The wheels' 0.4 rad/s is ten tolerances away, so the check shows that the filter fuses the IMU's samples.
The phase

The excerpt shows the phase, from test_ekf_synthetic.py, lines 462–466:

test_ekf_synthetic.py: phase 1
462
463
464
465
466
def check_estimate_follows_the_imu_with_the_smaller_variance(self) -> None:
    """Check that the estimate follows the IMU's yaw rate when the wheels report another."""
    outputs = self.run_steps(FUSION_STEPS, WHEEL_INPUT, [VALID_SAMPLE])
    self.print_fusion('fusion', outputs[-1])
    self.assert_fused_estimate(outputs[-1], SPEED, 'phase 1')

Phase 2: only samples that are too old

Each step sends one IMU sample that is stamped STALE_OFFSET, 0.5 s, before the step's time and reports BAD_RATE, 2.0 rad/s.

The phase

0.5 s is five times the validator's max_age, so the validator has to drop every one of these samples. The excerpt shows the phase, from test_ekf_synthetic.py, lines 468–477:

test_ekf_synthetic.py: phase 2
468
469
470
471
472
473
474
475
476
477
def check_estimate_follows_the_wheels_when_imu_samples_are_stale(self) -> None:
    """Check that the estimate goes to the wheels' yaw rate when every IMU sample is stale."""
    outputs = self.run_steps(PHASE_STEPS, WHEEL_INPUT,
                             [ImuInput(STALE_OFFSET, BAD_RATE, TOO_OLD)])
    print('stale IMU: estimate from {:.4f} to {:.4f} rad/s'.format(
        outputs[0].twist.twist.angular.z, outputs[-1].twist.twist.angular.z))
    self.assert_stays_at_most(outputs, IMU_RATE + YAW_RATE_TOLERANCE,
                              'phase 2: stale samples reporting 2.0 rad/s')
    self.assert_yaw_rate(outputs[-1], WHEEL_RATE, WHEEL_RATE_TOLERANCE,
                         'phase 2: without IMU samples, the estimate follows the wheels')
  • No kept output above 0.51 rad/s: the limit is IMU_RATE + YAW_RATE_TOLERANCE, and the test compares the one output that it keeps per step. A filter that fused one of the 2.0 rad/s samples would go far above this limit.
  • The last output: WHEEL_RATE, 0.4 rad/s, within WHEEL_RATE_TOLERANCE, 0.02 rad/s. Without valid IMU samples, the wheel odometry is the filter's only source for the yaw rate, as when the IMU stops. The 50 steps, 1 s of ROS time, give the estimate the time to get there.

Phases 3 to 5: a valid sample and one to drop

In these phases, each step sends VALID_SAMPLE first and then a second sample that reports 2.0 rad/s. The second sample repeats the valid stamp in phase 3, lies 0.5 s in the future in phase 4, and lies 0.01 s in the past in phase 5. The validator drops each one, and the estimate stays at the IMU's yaw rate.

The phases

The excerpt shows the three phases, from test_ekf_synthetic.py, lines 479–504:

test_ekf_synthetic.py: phases 3 to 5
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
def check_validator_drops_samples_with_a_repeated_stamp(self) -> None:
    """Check that the estimate stays at the valid samples' rate despite repeated stamps."""
    outputs = self.run_steps(PHASE_STEPS, WHEEL_INPUT,
                             [VALID_SAMPLE,
                              ImuInput(VALID_SAMPLE.offset, BAD_RATE, NOT_NEWER)])
    self.assert_stays_at_most(outputs, IMU_RATE + YAW_RATE_TOLERANCE,
                              'phase 3: samples with a repeated stamp')
    self.assert_yaw_rate(outputs[-1], IMU_RATE, YAW_RATE_TOLERANCE,
                         'phase 3: valid samples again')

def check_validator_drops_samples_from_the_future(self) -> None:
    """Check that the estimate stays at the valid samples' rate despite future stamps."""
    outputs = self.run_steps(PHASE_STEPS, WHEEL_INPUT,
                             [VALID_SAMPLE,
                              ImuInput(FUTURE_OFFSET, BAD_RATE, FROM_THE_FUTURE)])
    self.assert_stays_at_most(outputs, IMU_RATE + YAW_RATE_TOLERANCE,
                              'phase 4: samples from the future')
    self.assert_yaw_rate(outputs[-1], IMU_RATE, YAW_RATE_TOLERANCE, 'phase 4: valid samples')

def check_validator_drops_samples_older_than_the_last_valid_one(self) -> None:
    """Check that the estimate stays at the valid samples' rate despite older stamps."""
    outputs = self.run_steps(PHASE_STEPS, WHEEL_INPUT,
                             [VALID_SAMPLE, ImuInput(OLDER_OFFSET, BAD_RATE, NOT_NEWER)])
    self.assert_stays_at_most(outputs, IMU_RATE + YAW_RATE_TOLERANCE,
                              'phase 5: samples older than the last valid one')
    self.assert_yaw_rate(outputs[-1], IMU_RATE, YAW_RATE_TOLERANCE, 'phase 5: valid samples')
  • Phase 3, a repeated stamp: the second sample has the stamp of the valid one: its offset is the valid sample's, VALID_SAMPLE.offset, 0 s. The validator has to drop it as not newer than the last valid sample, check 5 of the five checks.
  • Phase 4, from the future: the second sample is stamped FUTURE_OFFSET, 0.5 s, after the step's time, 25 times the validator's max_future. Check 4 has to drop it.
  • Phase 5, out of order: the second sample is stamped OLDER_OFFSET, 0.01 s, before the step's time. This stamp is well within max_age, so check 4 passes. Check 5 has to drop the sample, because its stamp is older than the valid sample's of the same step.

The assertions are the same in the three phases. No kept output is above 0.51 rad/s, and the last output is 0.5 rad/s within 0.01 rad/s. So the valid samples keep reaching the filter. The validator's counts in phase 7 show that the validator dropped the 150 samples that report 2.0 rad/s.

Phase 6: the clock goes back

The test sets the clock back from 107 s to RESTART, 50 s, as /gazebo/reset_simulation does. The validator starts its order check over, robot_localization resets the filter, and the estimate follows the IMU again.

The phase

The test sets the clock back from 107 s to RESTART, 50 s, as a call of the service /gazebo/reset_simulation does in the simulation. The validator starts its order check over, as a clock that goes back on the validator's tests page explains, and the EKF starts over from the new time, because ekf.yaml sets reset_on_time_jump. The excerpt shows the phase, from test_ekf_synthetic.py, lines 506–529:

test_ekf_synthetic.py: phase 6
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
def check_estimate_follows_the_imu_again_after_the_clock_goes_back(self) -> None:
    """Check that the estimate follows the IMU again after the clock goes back to RESTART."""
    self.now = RESTART
    self.clock_publisher.publish(Clock(self.now))
    time.sleep(RESET_DELIVERY)
    # The first measurement after the reset initializes the filter, and robot_localization
    # doesn't publish that estimate, because its stamp is older than the last one it
    # published. The measurement that showed the jump is dropped, and so can be the other
    # one of its step: the reset clears the node's callback queue. So the outputs start
    # again within a few steps
    for _ in range(RESET_STEPS):
        if self.step_allowing_no_output(WHEEL_INPUT, [VALID_SAMPLE]) is not None:
            break
    else:
        self.fail('phase 6: no EKF output within {} steps after the clock went back'.format(
            RESET_STEPS))
    outputs = self.run_steps(AFTER_RESET_STEPS, WHEEL_INPUT, [VALID_SAMPLE])
    print('after the clock went back to {:.0f} s: estimate {:.4f} rad/s at {:.2f} s'.format(
        RESTART.to_sec(), outputs[-1].twist.twist.angular.z,
        outputs[-1].header.stamp.to_sec()))
    self.assertLess(outputs[0].header.stamp, START,
                    'phase 6: the outputs should carry the new time')
    self.assert_yaw_rate(outputs[-1], IMU_RATE, YAW_RATE_TOLERANCE,
                         'phase 6: after the clock went back')
  1. The phase sets self.now to RESTART, publishes the clock and sleeps RESET_DELIVERY, 0.5 s of wall time, so that the jump reaches the nodes.
  2. The phase runs up to RESET_STEPS, 5, steps with step_allowing_no_output() and stops at the first step with an output of its own. The filter can stay without an output in the first steps after the jump. The comment in the excerpt says why. An output from before the jump that arrives late doesn't end this wait, because its stamp lies after the new clock and outputs_since() leaves it out. No output within 5 steps fails the test.
  3. The phase runs AFTER_RESET_STEPS, 100, steps with valid samples, 2 s of ROS time.
  4. The first of these outputs has to be stamped before START, 100 s. So the filter works in the new time.
  5. The last output has to be 0.5 rad/s within 0.01 rad/s, as in phase 1.

Phase 7: the validator's counts

The validator's counts on the topic /diagnostics have to equal the test's own counts: the samples that the validator forwarded and those that it dropped, by reason.

The phase

The validator counts every sample that it forwards and every sample that it drops, by reason, and reports the counts on the topic /diagnostics once per second of ROS time, as the validator's diagnostics on the Localization page show. The test kept its own counts with count_expected_outcome(). Phase 7 compares the two. The excerpt shows the phase, from test_ekf_synthetic.py, lines 531–571:

test_ekf_synthetic.py: phase 7
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
def check_validator_diagnostics_count_each_forwarded_and_dropped_sample(self) -> None:
    """Check that the validator's diagnostics count each sample that phases 1 to 6 sent."""
    values = self.wait_for_validator_diagnostics()
    print('validator diagnostics: {}'.format(values))
    self.assert_diagnostic_counts(values)

def validator_values(self, array_count_before: int) -> Optional[Dict[str, str]]:
    """Return the validator's latest status values after the first array_count_before arrays.

    None if no array after them has a status of the validator.
    """
    for array in reversed(self.diagnostics.since(array_count_before)):
        for status in array.status:
            if VALIDATOR_STATUS_NAME in status.name:
                return {pair.key: pair.value for pair in status.values}
    return None

def wait_for_validator_diagnostics(self) -> Dict[str, str]:
    """Advance the clock without IMU samples and return the validator's latest values.

    The validator publishes its diagnostics once per second of ROS time.
    """
    array_count_before = self.diagnostics.count()
    start = self.now
    while self.now < start + DIAGNOSTICS_WAIT:
        self.step(WHEEL_INPUT)
    wait_for(lambda: self.validator_values(array_count_before) is not None,
             'phase 7: no status of the IMU sample validator on the topic /diagnostics')
    values = self.validator_values(array_count_before)
    assert values is not None  # wait_for() returned
    return values

def assert_diagnostic_counts(self, values: Dict[str, str]) -> None:
    """Assert that values has the expected count for forwarded and for each drop reason."""
    self.assertEqual(str(self.expected_forward_count), values.get(FORWARDED_KEY),
                     'phase 7: forwarded samples')
    for reason, count in self.expected_drops.items():
        self.assertEqual(str(count), values.get(DROPPED_KEY_PREFIX + reason),
                         'phase 7: dropped, ' + reason)
    self.assertEqual(len(self.expected_drops) + 1, len(values),
                     'phase 7: other drop reasons: {}'.format(values))
  • wait_for_validator_diagnostics(): notes in array_count_before how many messages of the topic /diagnostics the test has so far. Then it runs steps without IMU samples until the clock is DIAGNOSTICS_WAIT, 1.5 s, further. That is longer than the 1 s between two reports, so a report from after the last IMU sample arrives. The method returns the keys and values of the validator's latest status.
  • validator_values(): looks through the messages that arrived after the first array_count_before, the newest first, and finds the validator's status by its name. The name contains VALIDATOR_STATUS_NAME, "IMU sample validator". The test writes this text and the keys out and doesn't import them from the script, so the test pins what a user reads on the topic.
  • assert_diagnostic_counts(): the value of FORWARDED_KEY, "Samples forwarded", has to be self.expected_forward_count. For each reason, the value of "Dropped, " plus the reason has to be the test's count. The last assertion compares the number of values with the number of expected reasons plus one, so the status has no other reason to drop a sample.

The robot's rates: robot_rates

The robot's nominal rates are 50 Hz for the IMU and 10 Hz for the wheel odometry. The scenario robot_rates uses them, in two phases:

The two phases of the scenario robot_rates from left to right: the wheel messages and the IMU samples, the yaw rate that they report, the ranges that the test accepts for the filter's estimate, and the estimate in the filter's outputs in a recorded run 100 ms: 5 IMU samples, 1 wheel message Phase 1 2 wheels IMU A wheel message every 100 ms Phase 1: a valid IMU sample every 20 ms stopped Yaw rate ωz in rad/s 0.4 0.5 100 103 105 ROS time t in s Wheel odometry: 0.4 rad/s in every message wheels Phase 1: the IMU samples report 0.5 rad/s IMU Phase 1: the last output has to be 0.5 ± 0.01 rad/s Phase 2: the last output has to be 0.4 ± 0.02 rad/s Phase 1: the filter's estimate in each of its 150 outputs, one per step 0.4979 rad/s, the output stamped 103 s 0.49 rad/s, the output stamped 103.1 s 0.49 rad/s, the output stamped 103.2 s 0.4656 rad/s, the output stamped 103.3 s 0.4656 rad/s, the output stamped 103.4 s 0.4426 rad/s, the output stamped 103.5 s 0.4335 rad/s, the output stamped 103.6 s 0.4335 rad/s, the output stamped 103.7 s 0.4204 rad/s, the output stamped 103.8 s 0.4204 rad/s, the output stamped 103.9 s 0.4123 rad/s, the output stamped 104 s 0.4095 rad/s, the output stamped 104.1 s 0.4095 rad/s, the output stamped 104.2 s 0.4057 rad/s, the output stamped 104.3 s 0.4057 rad/s, the output stamped 104.4 s 0.4034 rad/s, the output stamped 104.5 s 0.4026 rad/s, the output stamped 104.6 s 0.4026 rad/s, the output stamped 104.7 s 0.4016 rad/s, the output stamped 104.8 s 0.4016 rad/s, the output stamped 104.9 s 0.5000 estimate 0.4016 The two phases of the scenario robot_rates from left to right: the wheel messages and the IMU samples, the yaw rate that they report, the ranges that the test accepts for the filter's estimate, and the estimate in the filter's outputs in a recorded run 100 ms: 5 IMU samples, 1 wheel message Phase 1 2 wheels IMU A wheel message every 100 ms Phase 1: a valid IMU sample every 20 ms stopped Yaw rate ωz in rad/s 0.4 0.5 100 103 105 ROS time t in s Wheel odometry: 0.4 rad/s in every message wheels Phase 1: the IMU samples report 0.5 rad/s IMU Phase 1: the last output has to be 0.5 ± 0.01 rad/s Phase 2: the last output has to be 0.4 ± 0.02 rad/s Phase 1: the filter's estimate in each of its 150 outputs, one per step 0.4979 rad/s, the output stamped 103 s 0.49 rad/s, the output stamped 103.1 s 0.49 rad/s, the output stamped 103.2 s 0.4656 rad/s, the output stamped 103.3 s 0.4656 rad/s, the output stamped 103.4 s 0.4426 rad/s, the output stamped 103.5 s 0.4335 rad/s, the output stamped 103.6 s 0.4335 rad/s, the output stamped 103.7 s 0.4204 rad/s, the output stamped 103.8 s 0.4204 rad/s, the output stamped 103.9 s 0.4123 rad/s, the output stamped 104 s 0.4095 rad/s, the output stamped 104.1 s 0.4095 rad/s, the output stamped 104.2 s 0.4057 rad/s, the output stamped 104.3 s 0.4057 rad/s, the output stamped 104.4 s 0.4034 rad/s, the output stamped 104.5 s 0.4026 rad/s, the output stamped 104.6 s 0.4026 rad/s, the output stamped 104.7 s 0.4016 rad/s, the output stamped 104.8 s 0.4016 rad/s, the output stamped 104.9 s 0.5000 estimate 0.4016

What the steps send

  • a wheel message in every fifth step, 100 ms of ROS time, in both phases, up to the end of the 20 measured wheel periods. It reports 0.4 rad/s, the blue line, and a forward speed of 0.1 m/s in the first 20 wheel messages of phase 1, 0.12 m/s in its last 10, and 0.1 m/s again in the 20 of phase 2. The chart shows the yaw rates only
  • a valid IMU sample in every step of phase 1, 20 ms. It reports 0.5 rad/s, the orange line. In phase 2, the IMU sends nothing

What the test asserts

  • the range for the last output of a phase: 0.5 ± 0.01 rad/s in phase 1, 0.4 ± 0.02 rad/s in phase 2
  • =in phase 2: the header stamp of one output in each wheel period, the 100 ms from one wheel message to the next

What the filter published in a recorded run at the diffbot commit 3882a19

  • phase 1: the filter's estimate in each of its 150 outputs, one per step. It stays between 0.4987 and 0.5000 rad/s, and the last output has 0.5000 rad/s
  • phase 2: the filter's 20 outputs in the measured periods, each stamped with a wheel message's time. The estimate goes from 0.4979 to 0.4016 rad/s
The scenario robot_rates from left to right, one step every 20 ms of ROS time. At the dotted vertical line, the IMU stops.
Phase and its steps Inputs What the test asserts
1: FUSION_STEPS, 150 One valid IMU sample in every step, a wheel message in every fifth step: 0.1 m/s, from step 100 on 0.12 m/s An output in every step. The output of step 99 has the frames, \(v_x\) = 0.1 ± 0.002 m/s and \(\omega_z\) = 0.5 ± 0.01 rad/s. From step 106 on, the output that the test keeps for each step has \(v_x\) = 0.12 ± 0.002 m/s. The last output has the frames, \(v_x\) = 0.12 ± 0.002 m/s and \(\omega_z\) = 0.5 ± 0.01 rad/s
2: IMU_STOPPED_WHEEL_PERIODS, 20 measured wheel periods of 5 steps, then 1 extra period No IMU sample, a wheel message of 0.1 m/s in the first step of each period One wheel message on the topic in each measured wheel period. The header stamp of exactly one output in each measured wheel period. The last output has \(v_x\) = 0.1 ± 0.002 m/s and \(\omega_z\) = 0.4 ± 0.02 rad/s
The class, its setUp(), the test method and how to read the figure

On the robot, the inputs arrive at different rates. The IMU is configured to publish at 50 Hz. The wheel odometry arrives at most 10 times per second, because the control loop of the node diffbot_base runs the drive controller at a nominal 10 Hz, as the box under frequency on the Localization page explains. The scenario robot_rates feeds the filter at these rates: an IMU sample in every step, every 20 ms of ROS time, and a wheel message in every fifth step, every 100 ms. Then the IMU stops, and the wheel messages go on.

The scenario is the class TestEkfAtTheRobotRates. The excerpt shows the class with its docstring, its setUp() and the test method, from test_ekf_synthetic.py, lines 574–604. Phase 2 needs a filter that fused the IMU's samples before, so the two phases are one test:

test_ekf_synthetic.py: the class, setUp() and the test method of robot_rates
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
class TestEkfAtTheRobotRates(SteppedClockTestCase):
    """The scenario robot_rates: the IMU at 50 Hz and the wheel odometry at 10 Hz, as on the robot.

    Its phases run in this order, and the second needs the state that the first left:
    1. Fusion at the robot's rates: the IMU reports 0.5 rad/s in every step, the wheels 0.4 rad/s
       in every fifth step. The filter publishes in every step, and the estimate follows the
       IMU, with the wheels' forward speed also between two wheel messages. From the 21st wheel
       message on, the wheels report 0.12 m/s instead of 0.1 m/s, and the estimate's forward
       speed follows within 6 steps.
    2. The IMU stops, the wheel odometry goes on in every fifth step, at 0.1 m/s again, and the
       clock moves on in every step. The filter publishes one output per wheel period, and the
       estimate goes to the wheels' 0.4 rad/s and 0.1 m/s.
    """

    def setUp(self) -> None:
        super().setUp()
        # The stamps of the wheel messages on their topic, which phase 2 counts
        self.wheel_message_stamps: ArrivalLog[rospy.Time] = ArrivalLog()
        self.wheel_odometry_subscriber = rospy.Subscriber(
            WHEEL_ODOMETRY_TOPIC, Odometry, self.on_wheel_odometry, queue_size=QUEUE_SIZE)
        wait_for(lambda: self.wheel_odometry_subscriber.get_num_connections() > 0,
                 "the test's subscriber to the wheel odometry didn't connect")

    def on_wheel_odometry(self, message: Odometry) -> None:
        """Note the stamp of a wheel message on the topic."""
        self.wheel_message_stamps.append(message.header.stamp)

    def test_ekf_follows_the_imu_at_the_robot_rates_and_the_wheels_after_the_imu_stops(
            self) -> None:
        self.check_estimate_follows_the_imu_and_a_wheel_speed_change_at_the_robot_rates()
        self.check_ekf_publishes_at_the_wheel_rate_and_follows_the_wheels_when_the_imu_stops()

setUp() first runs the setUp() of SteppedClockTestCase, through super(). Then setUp() subscribes to the wheel odometry's topic itself, with the callback on_wheel_odometry(), which notes the stamp of each wheel message in self.wheel_message_stamps. So phase 2 counts the wheel messages that the topic carried, not only those that the test meant to send.

The figure shows the two phases from left to right, over the ROS time \(t\). The rows "wheels" and "IMU" mark each wheel message and each IMU sample. On a narrow screen, the ticks of the IMU's samples, 20 ms apart, merge into a band. So the bracket at the top marks one wheel period of 100 ms: 5 IMU samples and 1 wheel message. The chart below the rows shows the yaw rate \(\omega_z\) that the inputs report, the range that the test accepts for the last output of each phase, and the filter's estimate in each of its outputs, recorded in a run of the scenario. The chart ends with the last of the 20 wheel periods that phase 2 measures, at 105.00 s, so the messages after these periods lie beyond the chart. The lists below the chart explain each mark and give its numbers. The figure in phase 2 shows three wheel messages of each phase in close-up.

The IMU samples report IMU_RATE, 0.5 rad/s, as in invalid_imu_samples. The wheel odometry reports WHEEL_RATE, 0.4 rad/s, in each of its messages, and a forward speed that changes once in phase 1.

Phase 1 of robot_rates: the estimate follows the IMU and the wheels' speed

The phase runs 150 steps, 3 s of ROS time, with a valid IMU sample in each step and a wheel message in every fifth step.

The phase and its checks

With the wheel message of step 100, 2 s into the phase, the wheels' forward speed changes from SPEED, 0.1 m/s, to CHANGED_SPEED, 0.12 m/s. Only the wheel odometry carries a forward speed. So the estimate's forward speed \(v_x\) reaches 0.12 m/s only if the filter fuses the wheel messages after the change. The excerpt shows the phase, from test_ekf_synthetic.py, lines 606–624:

test_ekf_synthetic.py: phase 1 of robot_rates
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
def check_estimate_follows_the_imu_and_a_wheel_speed_change_at_the_robot_rates(
        self) -> None:
    """Check that the estimate follows the IMU's yaw rate and the change of the wheels' speed.

    The wheel messages report SPEED before SPEED_CHANGE_STEP and CHANGED_SPEED from it on.
    The forward speed comes only from the wheels, so the estimate reaches CHANGED_SPEED only
    if the filter fuses wheel messages after the change.
    """
    # The step before SPEED_CHANGE_STEP and the last of the FUSION_STEPS steps are each the
    # fourth after a wheel message, so the checks of the fused estimate look at outputs between
    # two wheel messages
    outputs = self.run_steps_at_the_robot_rates(FUSION_STEPS, [VALID_SAMPLE])
    self.print_fusion("fusion at the robot's rates", outputs[-1])
    self.print_speed_change(outputs)
    self.assert_fused_estimate(outputs[SPEED_CHANGE_STEP - 1], SPEED,
                               'phase 1, before the speed change')
    self.assert_forward_speeds(outputs[SPEED_CHANGE_STEP + SPEED_CHANGE_WINDOW:],
                               CHANGED_SPEED, 'phase 1, after the speed change')
    self.assert_fused_estimate(outputs[-1], CHANGED_SPEED, 'phase 1')

The phase runs its steps with run_steps_at_the_robot_rates(), which asks wheel_input_at_the_robot_rates() for each step's wheel odometry. The excerpt shows the method, from test_ekf_synthetic.py, lines 668–676:

test_ekf_synthetic.py: run_steps_at_the_robot_rates()
668
669
670
671
672
673
674
675
676
def run_steps_at_the_robot_rates(self, count: int,
                                 imu_inputs: Sequence[ImuInput]) -> List[Odometry]:
    """Run count steps with the wheel odometry in every fifth and return each step's output.

    Every step publishes imu_inputs, and the steps that wheel_input_at_the_robot_rates()
    gives one publish the wheel odometry.
    """
    return [self.step(wheel_input_at_the_robot_rates(index), imu_inputs)
            for index in range(count)]

The next excerpt shows the function that decides each step's wheel odometry, from test_ekf_synthetic.py, lines 777–785. The step with the step_index 0 publishes a wheel message, and so does every step whose step_index is a multiple of STEPS_PER_WHEEL_MESSAGE, 5. The other steps get None and publish no wheel odometry:

test_ekf_synthetic.py: wheel_input_at_the_robot_rates()
777
778
779
780
781
782
783
784
785
def wheel_input_at_the_robot_rates(step_index: int) -> Optional[WheelInput]:
    """Return the wheel odometry of phase 1's step at step_index in robot_rates, None for none.

    The first step publishes the wheel odometry, then every STEPS_PER_WHEEL_MESSAGE-th step:
    WHEEL_INPUT before SPEED_CHANGE_STEP, CHANGED_SPEED_WHEEL_INPUT from it on.
    """
    if step_index % STEPS_PER_WHEEL_MESSAGE != 0:
        return None
    return WHEEL_INPUT if step_index < SPEED_CHANGE_STEP else CHANGED_SPEED_WHEEL_INPUT

The phase checks four things:

  • An output in every step: each step goes through step(), which fails when the filter publishes no output for the step. Between two wheel messages, each IMU sample gives the filter a measurement to fuse, so the filter publishes 50 times per second of ROS time, as in invalid_imu_samples.
  • Before the change: assert_fused_estimate() checks the output of the step with the index 99, SPEED_CHANGE_STEP - 1, against SPEED and IMU_RATE, with the tolerances of phase 1 of invalid_imu_samples. Step 99 is the fourth step after the wheel message of step 95, so this check looks at an output 80 ms after the last wheel message. Between two wheel messages, the filter's prediction keeps \(v_x\), and the IMU's samples carry only the yaw rate \(\omega_z\).
  • After the change: assert_forward_speeds() checks the output that the test keeps for each step, the last one that step() returns, from the step with the index 106 on, SPEED_CHANGE_STEP + SPEED_CHANGE_WINDOW, against CHANGED_SPEED, 0.12 m/s, within 0.002 m/s. SPEED_CHANGE_WINDOW, 6 steps, gives the estimate time for two wheel messages, in the steps 100 and 105, and one more step for a message that arrives late. The constants' comments say why the change is 0.02 m/s: the filter estimates an acceleration from the change, which moves the estimate between two wheel messages, by less than the tolerance for this small change.
  • The last output: assert_fused_estimate() checks the output of the step with the index 149, again the fourth after a wheel message, against CHANGED_SPEED and IMU_RATE.

The excerpt shows the helper of the check after the change, from test_ekf_synthetic.py, lines 724–729:

test_ekf_synthetic.py: assert_forward_speeds()
724
725
726
727
728
729
def assert_forward_speeds(self, outputs: Sequence[Odometry], speed: float,
                          what: str) -> None:
    """Assert that each output's forward speed is the wheels' speed. what names the case."""
    for output in outputs:
        self.assert_forward_speed(
            output, speed, '{} at {:.2f} s'.format(what, output.header.stamp.to_sec()))

Phase 2 of robot_rates: the IMU stops

The phase sends no IMU sample. The wheel messages go on, one in every fifth step, for IMU_STOPPED_WHEEL_PERIODS, 20 measured wheel periods of 100 ms, and one extra period, whose messages show the test that those of the measured periods have arrived. The figure shows three wheel messages of each phase, with each output's header stamp and the ROS time at which the filter published it:

Phase 1: the IMU running

Phase 1, IMU running: an IMU sample in every step, a wheel message in every fifth, and an EKF output in every step wheels IMU EKF 102.7 102.8 102.9 0.5 rad/s, stamped 102.66 s, published at 102.68 s 0.5 rad/s, stamped 102.68 s, published at 102.7 s 0.4992 rad/s, stamped 102.7 s, published at 102.72 s 0.4997 rad/s, stamped 102.72 s, published at 102.74 s 0.4999 rad/s, stamped 102.74 s, published at 102.76 s 0.5 rad/s, stamped 102.76 s, published at 102.78 s 0.5 rad/s, stamped 102.78 s, published at 102.8 s 0.4992 rad/s, stamped 102.8 s, published at 102.82 s 0.4997 rad/s, stamped 102.82 s, published at 102.84 s 0.4999 rad/s, stamped 102.84 s, published at 102.86 s 0.5 rad/s, stamped 102.86 s, published at 102.88 s 0.5 rad/s, stamped 102.88 s, published at 102.9 s 0.4992 rad/s, stamped 102.9 s, published at 102.92 s 0.4997 rad/s, stamped 102.92 s, published at 102.94 s Phase 1, IMU running: an IMU sample in every step, a wheel message in every fifth, and an EKF output in every step wheels IMU EKF 102.7 102.8 102.9 0.5 rad/s, stamped 102.66 s, published at 102.68 s 0.5 rad/s, stamped 102.68 s, published at 102.7 s 0.4992 rad/s, stamped 102.7 s, published at 102.72 s 0.4997 rad/s, stamped 102.72 s, published at 102.74 s 0.4999 rad/s, stamped 102.74 s, published at 102.76 s 0.5 rad/s, stamped 102.76 s, published at 102.78 s 0.5 rad/s, stamped 102.78 s, published at 102.8 s 0.4992 rad/s, stamped 102.8 s, published at 102.82 s 0.4997 rad/s, stamped 102.82 s, published at 102.84 s 0.4999 rad/s, stamped 102.84 s, published at 102.86 s 0.5 rad/s, stamped 102.86 s, published at 102.88 s 0.5 rad/s, stamped 102.88 s, published at 102.9 s 0.4992 rad/s, stamped 102.9 s, published at 102.92 s 0.4997 rad/s, stamped 102.92 s, published at 102.94 s

Phase 2: the IMU stopped

Phase 2, IMU stopped: a wheel message in every fifth step, and one EKF output per wheel message, stamped with the message's time: fused in the next update, or predicted at the time of the message wheels IMU EKF 103.1 103.2 103.3 stopped 0.49 rad/s, stamped 103.1 s, published at 103.12 s fused 0.49 rad/s, stamped 103.2 s, published at 103.2 s predicted 0.4656 rad/s, stamped 103.3 s, published at 103.32 s fused sensor_timeout 0.1 s Phase 2, IMU stopped: a wheel message in every fifth step, and one EKF output per wheel message, stamped with the message's time: fused in the next update, or predicted at the time of the message wheels IMU EKF 103.1 103.2 103.3 stopped 0.49 rad/s, stamped 103.1 s, published at 103.12 s fused 0.49 rad/s, stamped 103.2 s, published at 103.2 s predicted 0.4656 rad/s, stamped 103.3 s, published at 103.32 s fused sensor_timeout 0.1 s
  • a wheel message, every 100 ms of ROS time, published at its stamp. The axis labels its stamps in s
  • a valid IMU sample, every 20 ms, published at its stamp
  • an EKF output, at the ROS time of the filter's update that published it, from robot_localization's debug trace. The filter fuses the inputs of one step in its update 20 ms later
  • a tick on the axis: one of the filter's updates, 50 per second of ROS time
  • in phase 2: from an output's header stamp, the time of its wheel message, to the ROS time at which the filter published it, 20 ms later for a fused output. A predicted output is published at its stamp
  • fused: in the trace, the filter fused the wheel message and published the result, stamped with the message's time
  • predicted: in the trace, the filter predicted to the time of the wheel message, without a measurement, because the ROS time was 0.1 s or more, its sensor_timeout, past its last measurement. It published the prediction, stamped with that time. It fuses the wheel message in the next update, but publishes no second output with the same stamp. The next output contains the message
Each drawing spans 3 wheel messages of a phase of robot_rates, about 260 ms, in a recorded run at the diffbot commit 3882a19, with robot_localization's debug trace, ROS time to the right.

The table follows the first two wheel messages of the lower drawing, one column for each. All times are ROS times:

Wheel message's stamp 103.10 s 103.20 s
What the EKF did fused the wheel message predicted at the timeout
Output's header stamp 103.10 s 103.20 s
Published at 103.12 s 103.20 s

assert_one_output_per_period() checks the outputs' header stamps, not when the filter published them. The excerpt shows it, from test_ekf_synthetic.py, lines 741–747:

test_ekf_synthetic.py: one output stamp per wheel period
741
742
743
744
745
746
747
def assert_one_output_per_period(self, outputs: Sequence[Odometry],
                                 period_starts: Sequence[rospy.Time]) -> None:
    """Assert that each wheel period holds one output's stamp, in the order of the periods."""
    self.assertEqual([wheel_period_index(message.header.stamp, period_starts)
                      for message in outputs],
                     list(range(len(period_starts))),
                     'phase 2: one EKF output per wheel period, in the order of the periods')

The method turns each output's header stamp into the index of its wheel period. The indices have to be 0, 1, 2 and so on: one output stamp in each measured period, in the order of the periods, at any time within the period. So a wheel message that reaches the filter late can still pass. In the recorded run, each of the 20 outputs is stamped with its wheel message's time: 12 published 20 ms later by the update that fused the message, 8 at their stamp by a prediction at the sensor timeout.

The phase in detail: the code, how the filter stamps and publishes, and the checks

The phase sends no IMU sample. The wheel messages go on, one in every fifth step, for IMU_STOPPED_WHEEL_PERIODS, 20 measured wheel periods and one extra period, and report 0.1 m/s again. A wheel period runs from one wheel message's stamp to the next one's, 100 ms. The phase checks that each measured wheel period contains the header stamp of exactly one output, and that the estimate goes back to the wheels' forward speed and to their yaw rate.

An output's message gives the time of its estimate in its header stamp, which the filter writes into the message. robot_localization's debug trace tells when the filter published the output, and that ROS time can be later. The figure shows both times, for three wheel messages at the end of phase 1 and three in phase 2 of a recorded run. In each drawing, the row "wheels" marks the wheel messages at their stamps, the row "IMU" the IMU samples, and the row "EKF" each output at the ROS time of the update that published it. In the lower drawing, a thin line leads down from each wheel message's stamp, and an arrow leads from there to an output that was published later.

What the test asserts and what the recorded run shows, side by side:

  • The test asserts: the header stamp of exactly one output in each measured wheel period, at any time within the period. So a wheel message that reaches the filter late can still pass. The test doesn't look at the times at which the outputs are published.
  • The recorded run shows: each of the 20 outputs is stamped exactly with its wheel message's time. 12 of them were published 20 ms after their stamp, by the update that fused the wheel message. 8 were published at their stamp, by a prediction at the sensor timeout. So a fused output is published between two wheel messages, while its header stamp lies at the start of its period.

The excerpt shows the phase, from test_ekf_synthetic.py, lines 626–666. Its docstring says what the filter does between two wheel messages:

test_ekf_synthetic.py: phase 2 of robot_rates
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
def check_ekf_publishes_at_the_wheel_rate_and_follows_the_wheels_when_the_imu_stops(
        self) -> None:
    """Check that the EKF follows the wheels, one output per wheel period, once the IMU stops.

    Without the IMU, the filter has a measurement to fuse only when a wheel message arrives.
    Without one, the filter predicts only when the ROS time is sensor_timeout (ekf.yaml, 0.1 s)
    past its last measurement, and that is the wheel odometry's period. The filter publishes no
    second output with the same stamp. So it publishes one output per wheel period, as long as
    each wheel message reaches the filter, and the filter's updates run, before the clock
    reaches the next wheel message's stamp. The steps' sleeps give a message about 0.9 s of
    wall time for that, 5 DELIVERY and 4 NO_OUTPUT_WAIT, but they don't prove that the filter's
    updates ran. When the message reaches the filter before the update STEP after its stamp,
    the output is stamped with the message's time: the prediction to that time, published
    before the filter fused the message, or the fused message. A message that arrives later can
    leave an update of its period to predict, with that update's time, or is fused in a later
    update. So the check allows any stamp within the period. The test also takes the wheel
    messages from their topic and checks one at the start of each period. It picks outputs
    and wheel messages by their stamps, so that one whose callback runs late counts in its
    own phase. It reads them after one more wheel period, whose wheel message and an output
    from its start on show that every callback of the measured periods has run
    (wait_for_messages_from()). The wheel messages report SPEED again, so the estimate's
    forward speed goes back from phase 1's CHANGED_SPEED only if the filter fuses them.
    """
    period_starts = self.run_wheel_periods_without_imu(IMU_STOPPED_WHEEL_PERIODS)
    [next_period_start] = self.run_wheel_periods_without_imu(1)
    missing_after_the_periods = self.wait_for_messages_from(next_period_start)
    outputs = self.outputs_within_the_periods(period_starts)
    wheel_message_stamps = self.wheel_message_stamps_within_the_periods(period_starts)
    if not outputs:
        self.fail('phase 2: no EKF output in the {} wheel periods without the IMU'.format(
            len(period_starts)))
    self.print_imu_stopped(period_starts, wheel_message_stamps, outputs)
    self.assertFalse(missing_after_the_periods,
                     'phase 2: after the wheel periods, the test got no {}, so it may lack '
                     'messages of the periods too'.format(
                         ' and no '.join(missing_after_the_periods)))
    self.assert_one_wheel_message_per_period(wheel_message_stamps, period_starts)
    self.assert_one_output_per_period(outputs, period_starts)
    self.assert_forward_speed(outputs[-1], SPEED, 'phase 2')
    self.assert_yaw_rate(outputs[-1], WHEEL_RATE, WHEEL_RATE_TOLERANCE,
                         'phase 2: without IMU samples, the estimate follows the wheels')

The phase runs its steps with run_wheel_periods_without_imu(). The excerpt shows the method, from test_ekf_synthetic.py, lines 678–692:

test_ekf_synthetic.py: run_wheel_periods_without_imu()
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
def run_wheel_periods_without_imu(self, count: int) -> List[rospy.Time]:
    """Run count wheel periods without IMU samples and return the times when they start.

    A wheel period is STEPS_PER_WHEEL_MESSAGE steps. Its first step publishes a wheel
    message, stamped with the period's start, the others publish nothing. Every step allows
    no output: the clock moves on whether the filter published or not, after NO_OUTPUT_WAIT
    for the filter's update.
    """
    period_starts: List[rospy.Time] = []
    for _ in range(count):
        period_starts.append(self.now)
        self.step_allowing_no_output(WHEEL_INPUT)
        for _ in range(STEPS_PER_WHEEL_MESSAGE - 1):
            self.step_allowing_no_output(None)
    return period_starts
  1. A wheel period is STEPS_PER_WHEEL_MESSAGE, 5, steps. The first step of a period notes the clock self.now in period_starts and publishes a wheel message stamped with this time, which reports WHEEL_INPUT.
  2. The other 4 steps of the period publish nothing.
  3. Every step runs with step_allowing_no_output(): the clock moves on whether the filter published or not, and each step waits NO_OUTPUT_WAIT, 0.2 s of wall time, so that the filter's update for the new clock very likely runs before the next step.
  4. After IMU_STOPPED_WHEEL_PERIODS, 20, periods, the clock stands at the end of the last period, 2 s of ROS time after the phase's start.

robot_localization decides in each update what to publish, in four places of its code:

  • When the filter predicts without a measurement: in each update, the filter fuses the measurements that have arrived up to the ROS time. With no measurement in its queue, the filter predicts to the ROS time only when the ROS time is at least sensor_timeout, 0.1 s, past its last measurement. 0.1 s is also the wheel odometry's period.
  • The stamp of an output: the filter's last measurement time, which a prediction moves to the ROS time.
  • No second output with the same stamp: the filter publishes an output only when its stamp is later than the stamp of the last output it published. ekf.yaml doesn't set permit_corrected_publication, which would allow it.
  • A measurement older than the filter's time: the filter corrects its state with it but doesn't predict and doesn't move its time back.

The code in robot_localization 2.7.7:

The function integrateMeasurements() predicts without a measurement only when lastUpdateDelta, the ROS time minus the filter's last measurement time, is at least the sensor timeout. The prediction moves the last measurement time to the ROS time. The excerpt shows this part, from lines 680–711 of ros_filter.cpp:

ros_filter.cpp: integrateMeasurements() with an empty queue
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
else if (filter_.getInitializedStatus())
{
  // In the event that we don't get any measurements for a long time,
  // we still need to continue to estimate our state. Therefore, we
  // should project the state forward here.
  double lastUpdateDelta = currentTimeSec - filter_.getLastMeasurementTime();

  // If we get a large delta, then continuously predict until
  if (lastUpdateDelta >= filter_.getSensorTimeout())
  {
    predictToCurrentTime = true;

    RF_DEBUG("Sensor timeout! Last measurement time was " << filter_.getLastMeasurementTime() <<
             ", current time is " << currentTimeSec <<
             ", delta is " << lastUpdateDelta << "\n");
  }
}
else
{
  RF_DEBUG("Filter not yet initialized.\n");
}

if (filter_.getInitializedStatus() && predictToCurrentTime)
{
  double lastUpdateDelta = currentTimeSec - filter_.getLastMeasurementTime();

  filter_.validateDelta(lastUpdateDelta);
  filter_.predict(currentTimeSec, lastUpdateDelta);

  // Update the last measurement time and last update time
  filter_.setLastMeasurementTime(filter_.getLastMeasurementTime() + lastUpdateDelta);
}

The function getFilteredOdometryMessage() stamps the output with the filter's last measurement time, as lines 427–429 of ros_filter.cpp show:

ros_filter.cpp: the output's stamp and frames
427
428
429
message.header.stamp = ros::Time(filter_.getLastMeasurementTime());
message.header.frame_id = worldFrameId_;
message.child_frame_id = baseLinkOutputFrameId_;

The function periodicUpdate() marks an output as corrected_data when its stamp isn't later than lastPublishedStamp_, as lines 1975–1980 of ros_filter.cpp show. permitCorrectedPublication_ is the parameter permit_corrected_publication, whose default is false:

ros_filter.cpp: an output with the stamp of the last one
1975
1976
1977
1978
1979
1980
// If we're trying to publish with the same time stamp, it means that we had a measurement get inserted into the
// filter history, and our state estimate was updated after it was already published. As of ROS Noetic, TF2 will
// issue warnings whenever this occurs, so we make this behavior optional.
// Just for safety, we also check for the condition where the last published stamp is *later* than this stamp.
// This should never happen, but we should handle the case anyway.
corrected_data = (!permitCorrectedPublication_ && lastPublishedStamp_ >= filteredPosition.header.stamp);

The function publishes only an output that isn't corrected_data, as lines 2048–2055 of ros_filter.cpp show:

ros_filter.cpp: publishing the output
2048
2049
2050
2051
2052
2053
2054
2055
// Fire off the position and the transform
if (!corrected_data)
{
  positionPub_.publish(filteredPosition);
}

// Retain the last published stamp so we can detect repeated transforms in future cycles
lastPublishedStamp_ = filteredPosition.header.stamp;

The function processMeasurement() of the filter predicts to a measurement's time only when delta, the measurement's time minus the last measurement time, is positive, and corrects in every case, as lines 214–235 of filter_base.cpp show:

filter_base.cpp: predict, then correct
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
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);
}

The same function moves the last measurement time only for a delta that isn't negative, as lines 261–264 of filter_base.cpp show:

filter_base.cpp: the last measurement time
261
262
263
264
if (delta >= 0.0)
{
  lastMeasurementTime_ = measurement.time_;
}

sensor_timeout in ekf.yaml is 0.1 s, the same as the wheel period. So the setting sits exactly at the limit: the ROS time at which the filter may predict without a measurement is the stamp of the next wheel message.

The three recordings have their wheel messages on time. In them, with the IMU stopped, the updates 40, 60 and 80 ms after a wheel message's stamp publish nothing. In these updates, the filter has nothing to fuse, and its last measurement is less than 0.1 s old. In the test, the filter's update at the stamp \(t_\text{w}\) of the next wheel message runs when the clock reaches \(t_\text{w}\), before the test publishes the message. In the first period, that update at 103.00 s still has the last IMU sample, stamped 102.98 s, in its queue and fuses it, and the update 20 ms later fuses the wheel message. In the other 19 periods, the queue of the update at \(t_\text{w}\) is empty, and the update decides between two cases:

  • Fused: the ROS time is less than 0.1 s past the filter's last measurement, so the update does nothing. The update 20 ms later fuses the message and publishes the result, stamped \(t_\text{w}\), at the ROS time \(t_\text{w}\) + 20 ms.
  • Predicted: the ROS time is 0.1 s or more past the filter's last measurement, so the update predicts to \(t_\text{w}\) and publishes the prediction, stamped \(t_\text{w}\), at the ROS time \(t_\text{w}\), with the yaw rate \(\omega_z\) of the output before. The update 20 ms later fuses the message, but its output has the same stamp and isn't published. The next output, stamped with the next wheel message's time, contains the message.

Which of the two happens depends on the last digits of the difference between two stamps. The filter computes with times in seconds as double-precision floating-point numbers, and the difference of two stamps 0.1 s apart comes out a little above or a little below 0.1. For example, 103.2 s minus 103.1 s gives 0.1000000000000085, and the filter predicts. 103.3 s minus 103.2 s gives 0.0999999999999943, and the filter waits for the message. In each of three recorded runs, robot_localization's debug trace shows 8 of these 19 outputs as predicted and 11 as fused, at the same wheel messages: the predicted ones where the difference of two stamps comes out at 0.1 or above. With the first period, 12 of the 20 outputs of phase 2 are fused. In both cases, the output is stamped with the wheel message's time.

A wheel message can also reach the filter late, for example on a busy computer. The test doesn't wait for an output in the measured periods, so the clock moves on, and the filter fuses the message in a later update of its period. That late update either publishes the message, stamped with its time, or an earlier update of the period has already predicted and published, stamped with that update's time. Then the late message is older than the filter's time: the filter corrects with it but publishes no output for it. So each period still gets one output stamp, as long as its wheel message reaches the filter, and the filter's updates run, before the clock reaches the next wheel message's stamp. The steps' sleeps give a message about 0.9 s of wall time for that, five times DELIVERY and four times NO_OUTPUT_WAIT, but they don't prove that the filter's updates ran. The check of the outputs allows any stamp within a wheel period. Two functions put stamps into wheel periods. The excerpt shows them, from test_ekf_synthetic.py, lines 788–798:

test_ekf_synthetic.py: within_wheel_periods() and wheel_period_index()
788
789
790
791
792
793
794
795
796
797
798
def within_wheel_periods(stamp: rospy.Time, period_starts: Sequence[rospy.Time]) -> bool:
    """Return whether stamp lies from the first period's start to before the last period's end."""
    return period_starts[0] <= stamp < period_starts[-1] + ROBOT_WHEEL_ODOMETRY_PERIOD


def wheel_period_index(stamp: rospy.Time, period_starts: Sequence[rospy.Time]) -> int:
    """Return the index of the wheel period that stamp lies in, -1 before the first.

    A wheel period starts at its wheel message's stamp and ends where the next one starts.
    """
    return sum(1 for period_start in period_starts if period_start <= stamp) - 1
  • within_wheel_periods(): returns whether a stamp lies from the first period's start to before the end of the last period. The phase takes the outputs and the wheel messages by their stamps with this function, from all that the test has received after one more wheel period, which the next excerpt shows. So the stamps, not the order of arrival, decide the period of each message.
  • wheel_period_index(): returns for a stamp the index of its wheel period, the number of periods that started at or before the stamp, minus one. A period starts at a wheel message's stamp and ends at the next one's.

The phase reads the outputs and the wheel messages only after one more wheel period, which it doesn't measure. run_wheel_periods_without_imu(1) runs this period like the others, from the end of the last measured period at 105.00 s, and returns its start. Then wait_for_messages_from() waits for the messages of this extra period. The excerpt shows this method and the two methods that take the outputs and the wheel messages of the measured periods, from test_ekf_synthetic.py, lines 694–722:

test_ekf_synthetic.py: the wait after the measured periods, and what the phase reads
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
def wait_for_messages_from(self, stamp: rospy.Time) -> List[str]:
    """Wait for a wheel message stamped stamp and an output from stamp on, return the missing.

    The wait lasts at most WAIT s and doesn't fail. Each subscription gets the messages of its
    one publisher in order and runs their callbacks in that order. So when both messages have
    arrived, the callbacks of the earlier wheel messages and outputs have run. The caller moves
    the clock on past stamp first, through a whole wheel period, so that the filter publishes
    an output even when the wheel message reaches it late. Return the names of the messages
    that didn't arrive.
    """
    arrivals: Dict[str, Callable[[], bool]] = {
        'wheel message at {:.2f} s'.format(stamp.to_sec()):
            lambda: self.wheel_message_stamps.contains_all([stamp]),
        'EKF output stamped at {:.2f} s or later'.format(stamp.to_sec()):
            lambda: any(output.header.stamp >= stamp for output in self.ekf_outputs.since(0)),
    }
    wait_until(lambda: all(arrived() for arrived in arrivals.values()))
    return [name for name, arrived in arrivals.items() if not arrived()]

def outputs_within_the_periods(self, period_starts: Sequence[rospy.Time]) -> List[Odometry]:
    """Return the EKF's outputs stamped within the wheel periods, in the order of arrival."""
    return [message for message in self.ekf_outputs.since(0)
            if within_wheel_periods(message.header.stamp, period_starts)]

def wheel_message_stamps_within_the_periods(
        self, period_starts: Sequence[rospy.Time]) -> List[rospy.Time]:
    """Return the stamps of the wheel messages within the periods, in the order of arrival."""
    return [stamp for stamp in self.wheel_message_stamps.since(0)
            if within_wheel_periods(stamp, period_starts)]
  • wait_for_messages_from(): waits with wait_until(), at most WAIT, until the test's subscriber has the extra period's wheel message, stamped at its start, and an output stamped at that start or later. Each subscription receives the messages of its one publisher in their order and runs their callbacks in that order. So when these two messages have arrived, the callbacks of all earlier wheel messages and outputs have run. The extra period moves the clock on by a whole wheel period first, so that the filter publishes an output even when the extra wheel message reaches the filter late. The method doesn't fail. It returns the names of the messages that didn't arrive.
  • outputs_within_the_periods() and wheel_message_stamps_within_the_periods(): take the outputs and the wheel messages' stamps that lie within the measured periods, by within_wheel_periods(), from all that the test received, in the order of arrival. The extra period's messages lie after the last measured period, so they don't count.

The excerpt shows the assertion of the wheel messages, from test_ekf_synthetic.py, lines 731–739:

test_ekf_synthetic.py: one wheel message per wheel period
731
732
733
734
735
736
737
738
739
def assert_one_wheel_message_per_period(self, wheel_message_stamps: Sequence[rospy.Time],
                                        period_starts: Sequence[rospy.Time]) -> None:
    """Assert that one wheel message arrived, stamped with each period's start, in order."""
    received = [stamp.to_sec() for stamp in wheel_message_stamps]
    expected = [start.to_sec() for start in period_starts]
    missing = [start for start in expected if start not in received]
    self.assertEqual(received, expected,
                     'phase 2: one wheel message at the start of each wheel period'
                     + (', none at {} s'.format(missing) if missing else ''))

In assert_one_wheel_message_per_period(), the stamps of the received wheel messages have to be exactly the periods' starts, in their order, one message at the start of each period. A failure names the starts without a wheel message.

The phase checks these things, in this order:

  • Outputs at all: without any output within the measured periods, the phase fails with the message that it got no EKF output in the 20 wheel periods.
  • The extra period's messages: its wheel message and an output from its start on arrived. Otherwise messages of the measured periods might still be missing, and the phase fails with a message that names what didn't arrive.
  • One wheel message per wheel period: assert_one_wheel_message_per_period() checks the wheel messages that the test's own subscriber received on the topic. So the wheel topic carried the 20 wheel messages, one at the start of each period.
  • One output stamp per wheel period: the phase takes the outputs stamped within the periods, before the end of the last period. The extra period's outputs are stamped at that end or later, so the phase leaves them out. assert_one_output_per_period() checks that each of the 20 periods contains the header stamp of one output, in their order. A filter that put two output stamps into one period would fail, for example one that predicted again before the next wheel message. In runs of the scenario with changed settings and the wheel messages on time, a sensor_timeout of 0.08 s of ROS time gave 24 outputs for the 20 periods, and predict_to_current_time, which makes the filter predict in every update, gave 100, five in each period. Both runs failed this check. The check doesn't prove the setting itself. In a run in which each of the 20 wheel messages of the measured periods was published 0.5 s of wall time late, a sensor_timeout of 0.08 s gave one output stamp per period, and the run passed. The steps moved the clock on by only 60 ms of ROS time during those 0.5 s, so the test published each message 60 ms of ROS time after its stamp, for example the message stamped 103.00 s at the ROS time 103.06 s. Both sensor_timeout and this delay are ROS times. The test prints how many outputs carry a wheel message's stamp.
  • The forward speed \(v_x\): the last output's, SPEED, 0.1 m/s within 0.002 m/s, with assert_forward_speed(). The estimate comes from phase 1's 0.12 m/s, so the estimate gets back to 0.1 m/s only if the filter fuses the wheel messages of phase 2.
  • The yaw rate \(\omega_z\): the last output's, WHEEL_RATE, 0.4 rad/s, within WHEEL_RATE_TOLERANCE, 0.02 rad/s. The wheel odometry is the filter's only source of the yaw rate in this phase. In the recorded run, the estimate was within the tolerance from the eleventh output on, and 0.4016 rad/s at the end.

Runs

The runs on this page are runs of the commit 3882a19 of diffbot#220, whose files are those of its merge commit 7d428c2, in the dev container's image. Each scenario passed in 12 runs of 12, with the same printed values in each run. The rostest command took 17 s of wall time for invalid_imu_samples and 31 s for robot_rates.

invalid_imu_samples

What a run printed

In a run, the scenario printed these lines:

1
2
3
4
fusion: wheels 0.4 rad/s, IMU 0.5 rad/s, estimate 0.4987 rad/s, forward 0.1000 m/s
stale IMU: estimate from 0.4966 to 0.4005 rad/s
after the clock went back to 50 s: estimate 0.4987 rad/s at 52.02 s
validator diagnostics: {'Samples forwarded': '402', 'Dropped, from the future': '50', 'Dropped, not newer than the last one': '100', 'Dropped, too old': '50'}
  • The estimate: 0.4987 rad/s at the end of phase 1 and of phase 6, and 0.4005 rad/s at the end of phase 2. These are the three dots in the figure of the seven phases. 0.4966 rad/s is the first estimate of phase 2, the diamond in the figure.
  • The forwarded samples: 402 are the 150 of phase 1, the 3 · 50 of phases 3 to 5, and the 102 of phase 6. In this run, the filter published again in the second step after the clock went back, so phase 6 had 2 + 100 steps.
  • The dropped samples: 50 too old from phase 2, 100 not newer from phases 3 and 5, and 50 from the future from phase 4.

robot_rates

What a run printed

In a run, the scenario printed these lines:

1
2
3
fusion at the robot's rates: wheels 0.4 rad/s, IMU 0.5 rad/s, estimate 0.5000 rad/s, forward 0.1207 m/s
speed change at 102.00 s, wheels 0.1 to 0.12 m/s: estimate 0.1000 m/s before, 0.1155 m/s in the step of the change, 0.1194 to 0.1209 m/s from 6 steps after it
IMU stopped: 20 outputs for 20 wheel periods with 20 wheel messages, 20 of the outputs stamped at a wheel message, estimate from 0.4979 to 0.4016 rad/s, forward from 0.1046 to 0.0997 m/s
  • Phase 1, the yaw rate: the last output has 0.5000 rad/s. In the recorded run, the filter's first output, at the first wheel message, has 0.4987 rad/s. From the second wheel message on, each wheel message pulled the estimate down to 0.4992 rad/s, and the IMU's samples brought it back to 0.5000 rad/s within three steps. In invalid_imu_samples, with a wheel message in every step, phase 1 ends at 0.4987 rad/s.
  • Phase 1, the forward speed: 0.1000 m/s in the output of step 99, before the change. 0.1155 m/s in the output of step 100, the first that contains a wheel message of 0.12 m/s. From step 106 on, the outputs that the test keeps lie between 0.1194 and 0.1209 m/s, within 0.002 m/s of 0.12 m/s. 0.1207 m/s at the end of the phase.
  • Phase 2: 20 outputs for 20 wheel periods, with 20 wheel messages on the topic, each output stamped with its wheel message's time. The estimate goes from 0.4979 to 0.4016 rad/s, and the forward speed from 0.1046 to 0.0997 m/s.

The figures of robot_rates come from a recording run of the scenario at the same commit, in a copy of ekf_synthetic_robot_rates.test with two additions. rosbag records the filter's outputs and the inputs, and robot_localization writes its debug trace, a log of each update, which the filter's parameter debug turns on. The outputs' stamps, yaw rates and forward speeds come from the recording. The ROS time of the update that published each output, and whether that update predicted at the sensor timeout, come from the debug trace. The scenario passed in each of three such runs, with the same outputs. Neither the recording nor the trace is part of the test. Their values are in localization-stepped-clock-test-runs.jsonl, which localization_control_test_figures.py reads to draw the figures.

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.