# ROS 2 robot_localization: select measurements, then check the estimate

This exercise runs the real Jazzy `robot_localization` `ekf_node` twice against
the same supplied wheel odometry and IMU streams. The reference travels straight
at 1 m/s for five seconds. The wheel twist agrees with that motion; the IMU gyro
has a deliberate +0.2 rad/s bias. Enabling that gyro's yaw-rate field makes the
estimate turn even though the reference remains straight.

The inputs are synthetic. There is no robot, recorded dataset, sensor calibration,
or measured hardware performance here. The result demonstrates a configuration
mistake against a known reference; it does not establish that IMUs hurt estimation.

## Build and run

Save these files together: `Dockerfile`, `localization_workflow.py`, `verify.sh`,
`baseline.yaml`, and `fuse-biased-gyro.yaml`. The README, environment record, and
captured output provide the explanation and verification context.

```bash
docker build --platform linux/arm64 -t dustin-robot-localization-lesson:jazzy .

mkdir -p localization-output

docker run --rm --network none --platform linux/arm64 \
  --mount "type=bind,source=$(pwd),target=/lesson,readonly" \
  --mount "type=bind,source=$(pwd)/localization-output,target=/output" \
  dustin-robot-localization-lesson:jazzy \
  bash /lesson/verify.sh --output /output/run
```

Building requires internet access and disk space. The base image digest and the
ARM64 `ros-jazzy-robot-localization` Debian package version are pinned. Apt resolves
other dependencies at build time, so this is not a complete dependency snapshot.
If the pinned package leaves the repository, the build fails instead of silently
switching versions. `environment.txt` identifies the tested environment.

The run uses no external network, installs nothing, and removes its container afterward.
The source is mounted read-only; outputs remain in `localization-output/run`.
Use a new output directory for each run. An existing directory causes the script
to exit before writing. Omit the output mount and `--output /output/run` to use a
temporary directory that is removed when the run finishes.

`verify.sh` selects ROS domain 74 and Fast DDS and sources Jazzy. The script rejects
an unexpected estimator package version. Both filters, the input publisher, and
the output subscribers run inside the same container. No ROS daemon is required.

## Measurements and frames

Each run publishes 51 messages per input topic, with header times from 100 through
105 seconds in 0.1-second steps. These are supplied counts, not proof that every
message was processed by the estimator. The reference starts at the origin with
zero heading and ends at `(5, 0)` metres, still facing the positive x direction.

| Input | Supplied values | Selected in baseline | Selected in changed run |
| --- | --- | --- | --- |
| `/fixture/odom`, `nav_msgs/msg/Odometry` | Body velocity `(1, 0, 0)` m/s; angular velocity zero | `vx`, `vy`, `vyaw` | Same |
| `/fixture/imu`, `sensor_msgs/msg/Imu` | Angular velocity `(0, 0, 0.2)` rad/s | None | `vyaw` |

Odometry pose uses `header.frame_id=odom`, while its twist uses
`child_frame_id=base_link`. The supplied odometry pose follows the reference but
is deliberately excluded from both filters. Combining pose and velocity from the
same wheel calculation as independent evidence would count related information
twice. Here the estimator obtains its position by integrating selected velocity.

The IMU frame is also `base_link`: its axes and origin are already aligned with
the body in this fixture, so no mounting transform is needed. A real sensor in
`imu_link` needs the appropriate mounting transform. There is no `map` frame or
global correction in this example. The filter publishes `/odometry/filtered`
with parent `odom` and child `base_link`, plus the matching transform on `/tf`.

## Masks and covariance

Both selection masks contain fifteen booleans, in this order:

```text
x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
```

The odometry mask enables indices 6, 7, and 11. The baseline IMU mask is all false;
the changed configuration enables only index 11. The script compares the two YAML
files and reads parameters back from each running node to check that it loaded
the intended configuration. The all-false IMU mask creates no IMU subscription.
The baseline log reports that condition, with a warning printed at error level.

`two_d_mode=true` constrains the non-planar state components. The supplied zero
`vy` is also selected as a no-sideways-motion constraint. That assumption belongs
to this straight-line fixture; it would be wrong for a robot that can move sideways
or is slipping laterally.

Each selected wheel twist variance is 0.01: `(m/s)^2` for linear velocity and
`(rad/s)^2` for yaw rate. The selected IMU yaw-rate variance is 0.0001 `(rad/s)^2`.
Off-diagonal entries are zero. These are variances, not standard deviations.
The smaller reported IMU variance gives its biased reading greater influence;
it does not remove bias. These supplied covariances are teaching choices, not
tuning recommendations or calibrated uncertainty estimates.

Odometry's unused pose covariance has diagonal entries 1.0. Its complete twist
covariance diagonal is 0.01. The IMU angular-velocity covariance diagonal is
`[0.01, 0.01, 0.0001]`. Orientation and linear acceleration are unavailable, marked
with `covariance[0]=-1` in their respective IMU blocks. Their mask entries remain
false. An all-zero IMU covariance block means unknown covariance, not perfect data.

The process-noise and initial-estimate covariance parameters are left unset in
both YAML files. The package's 3.8.3 defaults therefore apply. Parameter readback
checks the configured values; the optional unset covariance parameters are not
queried because this version rejects requests for uninitialized parameters.
In that version's `filter_base.cpp`, initial covariance is `1e-9 I`;
process noise is diagonal with values `[0.05, 0.05, 0.06, 0.03, 0.03, 0.06, 0.025,
0.025, 0.04, 0.01, 0.01, 0.02, 0.01, 0.01, 0.015]` in the same state order.
The first selected measurement then initializes its corresponding state and
covariance entries. These defaults identify this exercise, not a robot setup.

## Clock and process control

The fixture first supplies a repeating clock at 99.9 seconds. Once discovery and
parameter checks succeed, it queues the next stamped messages and advances the
clock by 0.1 seconds. The estimator's simulation-time timer can then process them.
The clock holds while callbacks settle and the script checks the output stamp.
Holding the initial clock without ever advancing it would leave that timer waiting.

`frequency=20.0` is a configured target. This exercise advances time in 0.1-second
steps and does not measure a 20 Hz output rate. The supplied clock, input
timestamps, and wall time are separate. Wall-time waits allow callbacks to run;
they are not latency guarantees. Timeouts use a monotonic clock so a stalled ROS
clock cannot make the script wait forever. Discovery and output waits allow 15
seconds, followed by bounded process cleanup on failure.

Each case starts a fresh estimator process group. It is signalled and checked for
shutdown before the next case resets time. The supplied messages, covariance,
frame choices, and common filter settings stay the same between cases.

## Inspect the results

| File | Evidence |
| --- | --- |
| `supplied-inputs.csv` | Reference motion, timestamps, and supplied measurement values |
| `baseline-filtered.csv`, `fuse-biased-gyro-filtered.csv` | Received filtered estimates, quaternion, twist, and selected output variances |
| `baseline-tf.csv`, `fuse-biased-gyro-tf.csv` | Received `odom` to `base_link` transforms |
| `baseline-parameters.json`, `fuse-biased-gyro-parameters.json` | Parameter values read back from the actual nodes |
| `baseline.log`, `fuse-biased-gyro.log` | Estimator diagnostics and shutdown messages |
| `report.json` | Configuration comparison, final reference errors, and passed checks |

The checks require finite planar states and unit quaternions, confirm the final
TF matches the filtered pose at the same stamp, and compare the final position
with the supplied reference. The baseline must have less than 0.05 m of position
error. The changed run must show positive turning and more than 1.5 m of position
error. These broad bounds test the intended effect without treating asynchronous
callback timing as an exact numerical contract. Equal-timestamp inputs can
initialize the filter in a different order. Wheel-first initialization sets forward
velocity to 1 m/s immediately; IMU-first initialization leaves it near zero, and
subsequent wheel updates raise it gradually because of the small default initial
covariance. Two verified runs therefore gave 2.362 m and 2.448 m final position
error in the biased-gyro case, while reaching the same rounded yaw. The captured
output is one run, and exact endpoint reproduction would need controlled
initialization. Keep the CSV files to inspect
the full received sequence; output-message counts can differ from input counts.

## Try next

1. Compare the two parameter JSON files. Confirm that the only configured
   difference is the IMU yaw-rate mask bit. Find the final filtered row and its
   matching transform, then recompute position error with `sqrt((x-5)^2+y^2)`.
2. Predict what happens if the supplied gyro bias is removed. For an exploratory
   copy, set `imu.angular_velocity.z=0.0` in `samples()`, update `supplied_row()`
   to record that new input, and replace the turning/error checks with straight-line
   expectations. Do not keep the original expected-failure assertions: this is a
   different experiment. A calibrated gyro can be useful; disabling all IMUs is
   not the lesson.

## Primary references

- [robot_localization 3.8.3 configuration documentation](https://github.com/cra-ros-pkg/robot_localization/blob/3.8.3/doc/configuring_robot_localization.rst)
- [State-estimation parameters](https://github.com/cra-ros-pkg/robot_localization/blob/3.8.3/doc/state_estimation_nodes.rst)
- [Default covariance and initialization implementation](https://github.com/cra-ros-pkg/robot_localization/blob/3.8.3/src/filter_base.cpp)
- [ROS 2 clock and time design](https://design.ros2.org/articles/clock_and_time.html)