Test: The EKF in Gazebo
The test proves one thing in the simulation: when the wheel odometry reports a wrong turn, the EKF's heading \(\theta\) still follows the robot's true turn, because the filter takes the yaw rate \(\omega_z\) from the IMU. The test makes the wheel odometry wrong on purpose. It gives the drive controller a wheel separation \(b_c\) that is 25 % larger than the simulated robot's wheel separation \(b\). The same simulation also checks that only the EKF publishes the transform odom → base_footprint. Both simulated robots run the test, DiffBot and Remo. The test doesn't show how the filter behaves on the real robot. What the test doesn't show lists the differences.
test_ekf.py has two tests:
| Test | What it checks | Limit |
|---|---|---|
test_ekf_follows_the_true_turn_with_a_wrong_wheel_separation |
In a left turn of 3 s: the IMU sample validator forwards the IMU's samples, the wheel odometry's turn falls behind the true turn, and the EKF's turn is the true turn | more than 45 Hz, more than 0.15 rad behind, within 0.05 rad |
test_ekf_is_the_only_odom_tf_publisher |
Only the node /diffbot/ekf_localization publishes the transform odom → base_footprint on the topic /tf |
exactly one node |
The turn from above
The figure shows the turn test from above, with the values of a saved run and one drawing for each simulated robot. In a drawing, the robot starts with its heading along the dashed line to the right and turns left, counterclockwise. Each arc is one turn from this start: the robot's true turn \(\Delta\theta_\text{true}\) from Gazebo, the turn \(\Delta\theta_\text{EKF}\) of the EKF's estimate and the turn \(\Delta\theta_\text{odom}\) of the wheel odometry. The label at the end of an arc is the arc's name in the line that the test prints, true, EKF or odometry, with the measured turn. The robot is drawn as it stands after the turn, and the line on its body points forward. The small green sector at the end of the EKF's arc is where this arc has to end: 0.05 rad to each side of the true turn, as the label "allowed ±0.05 rad" in the drawing's upper left says. The line above a drawing gives the robot's two wheel separations \(b\) and \(b_c\), and the list under the drawing gives the results of the two checks on the turn.
DiffBot
Wheel separation: b = 0.138 m on the simulated robot, bc = 0.1725 m in the drive controller
- ✓The EKF follows: its turn is 0.005 rad from the true turn. Allowed: 0.05 rad
- ✓The wheel odometry falls behind: by 0.370 rad. Required: more than 0.15 rad
Remo
Wheel separation: b = 0.144 m on the simulated robot, bc = 0.18 m in the drive controller
- ✓The EKF follows: its turn is 0.003 rad from the true turn. Allowed: 0.05 rad
- ✓The wheel odometry falls behind: by 0.374 rad. Required: more than 0.15 rad
- true: the robot's turn in Gazebo, Δθtrue
- EKF: the turn of the EKF's estimate, ΔθEKF
- odometry: the turn of the wheel odometry, Δθodom
- where the EKF's arc has to end:
EKF_TOLERANCE, 0.05 rad, to each side of the true turn
test_ekf.py from above, in a run at the commit 3ef4a91. Each arc is one turn, from the dashed start heading.The table lists the values of the figure, one column for each simulated robot. The run was in the dev container's image, at the commit 3ef4a91 of diffbot#154:
| Measurement | DiffBot | Remo |
|---|---|---|
| Wheel separation \(b\) of the simulated robot | 0.138 m | 0.144 m |
| Controller's wheel separation \(b_c\) | 0.1725 m | 0.18 m |
| True turn \(\Delta\theta_\text{true}\) | 1.849 rad | 1.873 rad |
| Wheel odometry's turn \(\Delta\theta_\text{odom}\) | 1.479 rad | 1.499 rad |
| EKF's turn \(\Delta\theta_\text{EKF}\) | 1.844 rad | 1.870 rad |
| Rate of the validated IMU samples | 49.7 Hz | 50.0 Hz |
The two checks on the turn compare these turns. For DiffBot, they work out as follows:
- The EKF follows the true turn: \(|\Delta\theta_\text{EKF} - \Delta\theta_\text{true}| = |1.844\ \text{rad} - 1.849\ \text{rad}| = 0.005\ \text{rad}\). The test allows at most 0.05 rad, the green sector.
- The wheel odometry falls behind: \(\Delta\theta_\text{true} - \Delta\theta_\text{odom} = 1.849\ \text{rad} - 1.479\ \text{rad} = 0.370\ \text{rad}\). The test requires more than 0.15 rad.
Run it
In the dev container, in the built workspace, run the test for each simulated robot:
1 2 | |
A pass ends with * RESULT: SUCCESS under SUMMARY, after one [passed] line for each of the two tests. CI runs both files with the repository's other tests, as Testing and CI describes.
The output of a passing run, and the line that the test prints
Run it and Read the results on Simulation Test explain rostest's options, its output and its result files. A passing run of sim_ekf_diffbot.test ends with:
1 2 3 4 5 6 7 8 | |
The test prints the measured turn in one line, which rostest copies into the result file rosunit-ekf_diffbot.xml. In the saved run, the line was:
1 | |
What a pass proves and what a failure means
The test gives the controller a wheel separation 25 % too large, so the wheel odometry reports about 20 % less than the true turn. A pass proves that in the simulation the EKF's turn lies within 0.05 rad of the true turn, the wheel odometry's turn falls more than 0.15 rad behind, the validator forwards the IMU's samples at more than 45 Hz, and only the EKF publishes the transform odom → base_footprint.
A failure names the test and the check. The turn test checks four things in a fixed order, from the setup to the EKF's turn, so the first failing check says how far the run got. A missing source of a heading fails before the turn, with the names of the silent sources.
What the test doesn't show
- The real gyroscope: the simulated IMU has noise but no bias and no mounting error.
- The real robot's rates and delays: in the simulation, both inputs are stamped with the simulation's time. The rate check counts the validated IMU samples over the turn, so it doesn't show that each message arrived on time.
- Other errors of the wheels: one constant factor on a turn on the spot, no varying slip, no driving forward.
- The position and turns of more than \(\pi\): the test compares the three turns only.
The details
- The real gyroscope: the simulated IMU adds random noise to the true yaw rate and reports the matching variance \(\sigma^2\). It has no bias and no mounting error. The simulated IMU describes the sensor.
- The real robot's rates and delays: in the simulation, the wheel odometry and the IMU are both configured for 50 Hz and stamped with simulation time. The saved DiffBot run measured 49.7 Hz of validated IMU samples. On the robot, the wheel odometry arrives at most 10 times per second, as The other settings on the Localization page explains. How it's tested on the same page lists what the tests don't show about the robot's rates and delays.
- Other errors of the wheels: the test's error is one constant factor on a turn on the spot. The test has no slip that comes and goes and no driving forward.
- The position: the test compares the three turns only, not the positions \(x\) and \(y\).
- Turns of more than \(\pi\):
turned()returns a change of the heading between \(-\pi\) and \(\pi\).
What the simulation starts
sim_ekf_diffbot.test and sim_ekf_remo.test start the simulation with the EKF on and the drive controller's wheel separation set to 1.25 times the robot's, and run test_ekf.py.
The rostest file and the launch files
Two rostest files run the test, sim_ekf_diffbot.test for the simulated DiffBot and sim_ekf_remo.test for the simulated Remo. A rostest file is a launch file with a <test> element. rostest starts the file's nodes, runs the test node and stops everything when the test node ends, as the test file on Simulation Test explains. The excerpt shows what DiffBot's file starts, from sim_ekf_diffbot.test, lines 9–21:
| sim_ekf_diffbot.test: the simulation with the EKF, the wheel separation and the test node | |
|---|---|
9 10 11 12 13 14 15 16 17 18 19 20 21 | |
sim_drive.launch: starts the simulation that the other simulation tests use too.ekfset totrueswitches the EKF on.gazebo_port: is the port of this test file's Gazebo server. Each simulation test file has a port of its own, because catkin runs the test files at the same time.wheel_separation: the<param>line comes after the include, so its value replaces the value that the included files loaded from the robot'sgeometry.yaml. 0.1725 m is 1.25 times DiffBot's 0.138 m.sim_ekf_remo.testsets 0.18 m, 1.25 times Remo's 0.144 m.- The test node:
ekf_diffbotrunstest_ekf.py. The node's private parametermodelnames the robot whosegeometry.yamlthe test reads.
The next excerpt shows how ekf reaches the drive controller's launch file, from sim_drive.launch, lines 15–25:
| sim_drive.launch: Gazebo without a window, and the controllers with the ekf argument | |
|---|---|
15 16 17 18 19 20 21 22 23 24 25 | |
diffbot_view.launch starts Gazebo's server and spawns the robot from its URDF model. diffbot_control.launch loads the drive controller's configuration, with the robot's geometry.yaml, and starts the controllers. With ekf true, diffbot_control.launch also sets the controller's enable_odom_tf to false and includes ekf.launch, which starts the IMU sample validator and the EKF, as Which launch file starts what shows.
Why a wrong wheel separation
With a wheel separation \(b_c\) 1.25 times the robot's \(b\), the drive controller turns the robot 25 % more than its wheel odometry reports. The IMU measures the true turn, so the EKF's turn follows the robot and the wheel odometry's doesn't.
The wheel separation in the drive controller, with the numbers of the test
The drive controller, diff_drive_controller, uses its wheel_separation parameter twice. It turns a commanded yaw rate into two wheel speeds, and it turns the wheels' movement back into the odometry's yaw rate. \(b\) is the wheel separation of the simulated robot, the distance between its two wheels in the URDF model. \(b_c = 1.25\,b\) is the controller's value. The factor 1.25 is the test's SEPARATION_SCALE. For a commanded yaw rate \(\omega_\text{cmd}\), the controller makes the right wheel's speed \(v_r\) larger than the left wheel's speed \(v_l\) by
The wheels of the simulated robot are \(b\) apart, so the robot turns with the yaw rate
The wheel odometry divides the same speed difference by \(b_c\) and reports \(\omega_\text{cmd}\). So the robot turns 25 % more than its wheel odometry says. This constant error stands in for the errors of a real robot's wheels, slip and a wheel separation that is a little off. The simulated IMU measures the robot's true yaw rate \(\omega_z\). A filter that follows the IMU ends at the true heading, and a filter that follows the wheels ends where the wheel odometry ends.
The test commands \(\omega_\text{cmd}\) = 0.5 rad/s (TURN_VELOCITY) for 3 s (DURATION). For these round numbers, the wheel odometry reports 1.5 rad and the robot turns 1.875 rad.
In a run, the measured turns are a little smaller than these round numbers, as the figure shows. The true turn stays 1.25 times the wheel odometry's turn.
The constants
The constants of test_ekf.py
The excerpt shows the topics, the names and the numbers of the test, from test_ekf.py, lines 41–69. STARTUP_TIMEOUT and POLL_INTERVAL are seconds of wall time, the computer's clock. DURATION and SETTLE are seconds of simulation time, Gazebo's clock.
| test_ekf.py: the constants | |
|---|---|
41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 | |
Reading the three headings
The class HeadingObserver subscribes to the true pose from Gazebo, the wheel odometry and the EKF's estimate, and keeps the latest heading \(\theta\) of each. It also counts the IMU samples that the validator forwards.
yaw(), turned() and HeadingObserver
The test compares three headings \(\theta\) of the robot: the true heading from Gazebo, the heading of the wheel odometry and the heading of the EKF's estimate. Each of these three sources publishes on a topic. The class HeadingObserver subscribes to the topics and keeps the latest heading of each source. Waiting until everything runs and The turn show where the test reads them.
A message of these topics carries a pose, not a heading. The excerpt shows the two functions that turn a pose into a heading and two headings into a turn, from test_ekf.py, lines 86–94:
| test_ekf.py: yaw() and turned() | |
|---|---|
86 87 88 89 90 91 92 93 94 | |
yaw(): a pose stores its orientation as a quaternion.euler_from_quaternion()returns roll, pitch and yaw, andyaw()returns the third, the heading \(\theta\).turned(): returns the change of the heading fromstarttoend, between \(-\pi\) and \(\pi\). The turn of this test is less than 2 rad, so the result is the turn itself.
The next excerpt shows the type Headings and how a HeadingObserver starts, from test_ekf.py, lines 106–134:
| test_ekf.py: Headings and HeadingObserver.__init__() | |
|---|---|
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 133 134 | |
Headings: the robot's heading at one moment by each source, in rad.trueis from Gazebo,odometryfrom the drive controller's wheel odometry andekffrom the EKF's estimate.true_heading,odometry_headingandekf_heading: the observer's latest heading of each source. Each isNoneuntil the first message of its source arrives.sample_count: the number of samples that the IMU sample validator forwarded since the observer was created.- The four subscribers: they read the drive controller's wheel odometry (
WHEEL_ODOMETRY_TOPIC), the EKF's estimate (FILTERED_TOPIC), Gazebo's model states (MODEL_STATES_TOPIC), which carry the robot's true pose, and the validator's output (VALIDATED_IMU_TOPIC). The subscriber of the model states keeps one message,MODEL_STATES_QUEUE_SIZE, because only the latest true pose matters. The subscriber of the validated samples keeps up to 100 messages,IMU_QUEUE_SIZE, because the test counts every sample. lock: several threads use the observer's four values, the three headings andsample_count. rospy has one receiving thread for each publisher connection of a subscribed topic, and calls the subscriber's callback in that thread. Each of the four topics has one publisher, so the four callbacks run in four threads. The test itself runs in the test node's main thread, which callssetUpClass()and the tests and reads the observer. The lock lets one thread at a time read or change the four values. So the test gets the three headings as one copy, and no callback changes a heading while the test copies them. The three headings are the latest of each source, not headings of the same instant.
Each callback stores what the test needs from one message. The excerpt shows the four callbacks, from test_ekf.py, lines 136–155:
| test_ekf.py: the callbacks of HeadingObserver | |
|---|---|
136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 | |
on_wheel_odometry()andon_filtered_odometry(): both topics carrynav_msgs/Odometrymessages. Each callback stores the heading of the message's pose,yaw(message.pose.pose).on_model_states(): Gazebo publishes the poses of all models in one message. The callback takes the pose of the model namedMODEL_NAME. Gazebo's model of the robot is nameddiffbotfor DiffBot and for Remo.on_validated_imu(): counts a sample that the IMU sample validator forwarded.with self.lock: each callback takes the lock before it changes its value and gives the lock back at the end of thewithblock. While another thread holds the lock, the callback waits.
The test reads the observer through three methods, and each holds the lock while it reads. The excerpt shows them, from test_ekf.py, lines 157–175:
| test_ekf.py: latest(), silent_sources() and validated_sample_count() | |
|---|---|
157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 | |
latest(): returns the latest heading of each source as oneHeadings. It returnsNoneuntil each of the three sources has sent a heading.silent_sources(): returns the names of the sources that haven't sent a heading yet, out ofGazebo,odometryandEKF.setUpClass()puts them into its failure message.validated_sample_count(): returns the number of validated IMU samples so far.
Waiting until everything runs
setUpClass() starts the observers and the command publisher and waits, at most STARTUP_TIMEOUT, 120 s of wall time, until each of the three sources has sent a heading and the drive controller listens to the commands.
setUpClass() and ready()
unittest calls setUpClass() once, before the first test of the class. setUpClass() creates the test's node, a HeadingObserver, the observer of the topic /tf and the publisher of the velocity commands. Then it waits until ready() returns true. The excerpt shows both methods, from test_ekf.py, lines 185–204:
| test_ekf.py: setUpClass() and ready() | |
|---|---|
185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 | |
cls.heading_observer: from its creation on, theHeadingObserverkeeps the latest headings and counts the validated IMU samples.cls.odom_tf_observer:OdomTfObserver()starts to count who publishes the transformodom → base_footprint, for the second test. The Transform's Publisher explains this observer.cls.command: publishes the velocity commands onCMD_VEL_TOPIC, the drive controller's command topic.ready(): is true whenlatest()returns headings, so a heading arrived from each of the three sources, and the drive controller has connected to the command topic. ROS loses a command that is published before this connection exists.STARTUP_TIMEOUT: after 120 s of wall time,setUpClass()raises anAssertionError. Its message names the sources that sent no heading, fromsilent_sources(), and the number of subscribers to the commands.
The turn
measure_turn() turns the robot on the spot at 0.5 rad/s for 3 s of simulation time, stops it, and returns the turn that each source saw and the rate of the validated IMU samples.
measure_turn() and TurnMeasurement
measure_turn() turns the robot on the spot and returns what each source saw. It reads the headings with latest_headings(). The excerpt shows both methods, from test_ekf.py, lines 206–233:
| test_ekf.py: latest_headings() and measure_turn() | |
|---|---|
206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 | |
latest_headings() returns what latest() of the HeadingObserver returns. Its assert stops the test if that is None. After setUpClass(), which waited for a heading of each source, latest() returns a Headings.
measure_turn()reads the three headings withlatest_headings()intostart, notes the count of validated IMU samples and reads the simulation time.- For
DURATION, 3 s of simulation time, it publishes aTwistcommand withangular.zset toTURN_VELOCITY, 0.5 rad/s, atCOMMAND_RATE, 20 times per second. A positive yaw rate is a left turn. The command has no forward speed, so the robot turns on the spot. - It computes the rate of the validated IMU samples: the number of samples that
validated_sample_count()counted during the turn, divided by the time that the turn took. - In the
finallyblock, it publishes a command of zero. The robot stops also when an error ends the loop early. - It waits
SETTLE, 1 s of simulation time, until the robot stands. - It reads the three headings again into
stoppedand returns aTurnMeasurement. For each source,turned()gives the turn from the heading instartto the heading instopped, for exampleturned(start.true, stopped.true).
The excerpt shows the result's type, from test_ekf.py, lines 72–83:
| test_ekf.py: TurnMeasurement | |
|---|---|
72 73 74 75 76 77 78 79 80 81 82 83 | |
report() returns the three turns as one line of text. The test prints this line and puts it into the message of a failed check.
The checks on the turn
The excerpt shows the test method, from test_ekf.py, lines 271–281. The method first reads the controller's wheel separation from the parameter server and checks it, so a wrong setup fails before the robot is commanded to turn. Then the method measures one turn and makes the other three assertions:
| test_ekf.py: the turn test | |
|---|---|
271 272 273 274 275 276 277 278 279 280 281 | |
In the list, \(\Delta\theta_\text{true}\) is the robot's true turn, \(\Delta\theta_\text{odom}\) the wheel odometry's turn and \(\Delta\theta_\text{EKF}\) the turn of the EKF's estimate, as in the figure.
- The wheel separation is the scaled one,
assert_wheel_separation_is_scaled(): the parameter/diffbot/mobile_base_controller/wheel_separationisSEPARATION_SCALEtimes thewheel_separationin the robot'sgeometry.yaml, withinSEPARATION_TOLERANCE, 10⁻⁶ m. This check shows that the rostest file's value reached the controller's parameter and still fits the robot's geometry. - The validator forwards the IMU,
assert_validated_imu_rate(): during the turn, the samples on the topic/diffbot/imu/data_validatedarrived at more thanMIN_IMU_RATE_SHAREtimesIMU_RATE, 0.9 · 50 Hz = 45 Hz. Without the validated samples, the EKF would have only the wheel odometry. - The wheel odometry falls behind,
assert_odometry_falls_behind(): \(\Delta\theta_\text{true} - \Delta\theta_\text{odom}\) is more thanMIN_ODOMETRY_ERROR, 0.15 rad. This check shows that the error that the test needs is there. With the scaled separation, the wheel odometry reports 1 / 1.25 of the true turn, so the difference is a fifth of the true turn. - The EKF follows the true turn,
assert_ekf_follows_the_true_turn(): \(|\Delta\theta_\text{EKF} - \Delta\theta_\text{true}|\) is at mostEKF_TOLERANCE, 0.05 rad.
Checks 3 and 4 compare the turns. The excerpt shows them, from test_ekf.py, lines 250–259:
| test_ekf.py: the checks of the turns | |
|---|---|
250 251 252 253 254 255 256 257 258 259 | |
Both messages end with measurement.report(), the three turns, so a failure shows the measured values.
The first two assertion methods and the robot's own wheel separation
The next excerpt shows the first two assertion methods, from test_ekf.py, lines 235–248:
| test_ekf.py: the first two assertions | |
|---|---|
235 236 237 238 239 240 241 242 243 244 245 246 247 248 | |
assert_wheel_separation_is_scaled() gets the robot's own value from nominal_wheel_separation(). The excerpt shows this function, from test_ekf.py, lines 97–103:
| test_ekf.py: nominal_wheel_separation() | |
|---|---|
97 98 99 100 101 102 103 | |
The turn from above lists what the test measured in a run, and works out checks 3 and 4 with these values.
Who publishes the transform
With the EKF on, the EKF has to publish the transform odom → base_footprint, and the drive controller must not. The test counts the transform's publishers on /tf for 3 s and expects only /diffbot/ekf_localization, and the controller's enable_odom_tf false.
The test
With the EKF on, the EKF has to publish the transform odom → base_footprint, and the drive controller must not. The excerpt shows the second test, from test_ekf.py, lines 261–269:
| test_ekf.py: only the EKF publishes odom → base_footprint | |
|---|---|
261 262 263 264 265 266 267 268 269 | |
- The test waits until
self.odom_tf_observerhas seen a first transformodom → base_footprint, at mostSTARTUP_TIMEOUT. - It waits
OBSERVE, 3 s of simulation time, while the observer keeps counting. - It expects exactly one publishing node in the observer's counts,
EKF_NODE, which is/diffbot/ekf_localization. - It expects the controller's parameter
enable_odom_tf, whichODOM_TF_PARAMETERnames, to befalse.
This observer counts from setUpClass() on. unittest runs a class's test methods in alphabetical order, so the turn test runs first, and the count covers the turn too. In the saved run, the observer counted 375 transforms for DiffBot and 371 for Remo, all from /diffbot/ekf_localization. The Transform's Publisher explains the observer and the same check without the EKF.
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.