Tier 2 Specification — Linear Kalman Filter for Pitch with Gyro-Bias State

Status: FINAL. Approved by the owner on 2026-10-31, including the §0 decisions and the §9 thresholds. Audience: EXECUTOR (implements §4–§8) and TESTER (writes tests from §2–§10 without seeing code). Where this document says “must”, it is a testable requirement. Prerequisite: the Tier 1 specification (tier1_spec.md, FINAL) stays binding. This document only lists what is added or changed. Tier 1 sections are cited as T1-§x.


0. Decisions in this tier (for owner review)

# Decision Reason
0.1 The Kalman filter (KF) uses the trapezoidal rate (ω[k−1] + ω[k]) / 2 in its prediction step. Tier 1 functions keep the backward-rectangle rule unchanged. The Tier 1 rule ω[k]·dt effectively leads the angle by half a sample, giving an error of about (dt/2)·ω(t). For the default trajectory that is ≈0.14° RMS. The KF’s steady-state noise floor is ≈0.03°, so with the rectangle rule this artefact would dominate the KF error and hide its advantage over the CF. The trapezoidal rule reduces it to below 0.002°. Test KD4 detects which rule is used.
0.2 The filter’s default bias_rw_std = 1e-4 rad/s/√s. The random-walk scenarios S3 and S5 use the same value in the simulator. Over 600 s this lets the bias wander by about ±0.0025 rad/s (1σ), about 25 % of the nominal bias. That is large enough to require tracking, and it is plausible for a phone-grade gyro.
0.3 The Tier 1 regression check uses a reference fixture. The TESTER writes a generator script, and the owner runs it on the current Tier 1 code before any Tier 2 change is installed. The tester cannot run code. This lets the “bit-identical Tier 1 output” requirement be verified as a black box without a git tag.
0.4 Expected values in §9 are planner estimates from a continuous-time steady-state Riccati analysis (§5.4). Every threshold is at least 1.5× away from its expected value. Response to the Tier 1 margin feedback.

1. Scope

In scope for Tier 2:

Out of scope: roll estimation, linear acceleration or vibration, EKF, quaternions, adaptive or innovation-based tuning, CSV loading, angle wrapping, streaming APIs. These are unchanged from Tier 1 except that the bias may now vary over time.


2. Conventions

All of T1-§2 applies unchanged. In addition:

2.1 Bias random-walk unit (binding for simulator and filter)

σ_b (the parameter gyro_bias_rw_std in the simulator and bias_rw_std in the filter) has the unit rad/s/√s. A bias random walk with intensity σ_b, sampled at step dt, has independent increments:

b[k] − b[k−1] ~ N(0, σ_b² · dt)

so after time T the bias has drifted with standard deviation σ_b · √T. The filter’s process noise for the bias state, Q[1,1] = σ_b² · dt, is the same quantity.

2.2 Reporting units


3. File layout

Unchanged from T1-§3, with these additions:

project_root/
  tier2.ipynb
  TIER2_RESULTS.md                          # EXECUTOR
  tests/make_tier1_reference.py             # TESTER writes; owner runs once (see §9.3, KS1)
  tests/data/tier1_reference.npz            # produced by the script above, run by the owner

make_tier1_reference.py is a standalone script. Its name does not start with test_, so pytest does not collect it. imu_filters.py must still not import imu_sim, imu_metrics or imu_plots.

Test-only dependencies are unchanged from Tier 1: pytest, jupyter/nbconvert and ipykernel.


4. Changes to imu_sim.py

4.1 New signature

simulate_pitch_imu(
    duration: float = 60.0,
    fs: float = 100.0,
    trajectory: Callable | None = None,
    gyro_bias = (0.01, 0.01, 0.01),
    gyro_noise_std: float = 0.005,
    accel_noise_std: float = 0.05,
    seed: int | None = None,
    gyro_bias_rw_std: float = 0.0,             # NEW, rad/s/√s, applies to all 3 axes
    bias_step: tuple[float, float] | None = None,  # NEW, (t_step [s], delta [rad/s]), y axis only
) -> SimulatedIMU

The first seven parameters keep their order and defaults. Tier 1 test I2 must be updated to this signature. No other Tier 1 test changes.

4.2 Bias model

Let b0 = gyro_bias (shape (3,)). For each axis i independently:

The measurement model is that of T1-§4.4, with the constant bias replaced by bias[k, :]:

gyro[k, i] = true_rate_i[k] + bias[k, i] + n_g[k, i]

4.3 New field in SimulatedIMU

Appended after fs:

Field Type / shape Meaning
gyro_bias_true (N, 3) float64 the bias actually applied at each sample, rad/s

The field gyro_bias keeps its meaning: the (3,) initial bias b0. Therefore gyro_bias_true[0] == gyro_bias always.

4.4 Backward compatibility and randomness

4.5 New errors (ValueError)

A t_step outside [t[0], t[−1]] is allowed. It means the step either applies to every sample or to none.


5. Additions to imu_filters.py

5.1 kalman_noise_matrices(dt, gyro_noise_std, accel_noise_std, bias_rw_std, gravity=9.81) -> tuple[ndarray, float]

Returns (Q, R):

Errors (ValueError):

5.2 KalmanResult (frozen dataclass, defined in imu_filters)

| Field | Shape | Meaning | |—|—|—| | angle | (N,) float64 | posterior pitch estimate, rad | | bias | (N,) float64 | posterior gyro-bias estimate for the axis passed in, rad/s | | cov | (N, 2, 2) float64 | posterior covariance; cov[k,0,0] in rad², cov[k,1,1] in (rad/s)², cov[k,0,1] in rad²/s |

5.3 kalman_pitch

kalman_pitch(
    t, gyro_rate, accel_angle,
    gyro_noise_std: float = 0.005,     # rad/s per sample (sensor spec)
    accel_noise_std: float = 0.05,     # m/s² per axis (sensor spec)
    bias_rw_std: float = 1e-4,         # rad/s/√s (sensor spec, §2.1)
    gravity: float = 9.81,
    initial_angle: float | None = None,
    initial_bias: float = 0.0,
    initial_cov = None,                # array-like (2, 2) or None
) -> KalmanResult

Inputs:

Initialization (k = 0):

No measurement update is applied at k = 0.

Recurrence for k ≥ 1, with x = [angle, bias]. This form is binding, and test KD2 reproduces it:

dt   = t[k] − t[k−1]
ω̄    = (gyro_rate[k−1] + gyro_rate[k]) / 2
Q    = kalman_noise_matrices(dt, gyro_noise_std, accel_noise_std, bias_rw_std, gravity)[0]
F    = [[1, −dt], [0, 1]]
x⁻   = [ angle[k−1] + (ω̄ − bias[k−1]) · dt ,  bias[k−1] ]
P⁻   = F · cov[k−1] · Fᵀ + Q
S    = P⁻[0,0] + R
K    = P⁻[:, 0] / S                  # shape (2,)
y    = accel_angle[k] − x⁻[0]
x    = x⁻ + K · y
cov[k] = P⁻ − S · (K Kᵀ)             # equals (I − K H) P⁻ with H = [1, 0]; symmetric by construction

Errors (ValueError):

Performance: for N = 60 000 the call must complete in under 2 s on a typical laptop.

Tier 2 usage: kalman_pitch(t, gyro[:,1], accel_tilt(accel)[1]). The returned bias estimates gyro_bias_true[:, 1].

5.4 Justification of Q and R (binding derivation; reproduce it in the notebook)


6. Additions to imu_metrics.py

The input rules and errors are as in T1-§6, plus the cases listed below.

6.1 convergence_time(t, estimate, truth, tol, t_start=None) -> float

6.2 coverage_fraction(estimate, truth, std, n_sigma=2.0) -> float


7. Additions to imu_plots.py

plot_comparison (T1-§7.1) is unchanged.

7.1 plot_error_comparison(t, truth, estimates, exclude=("gyro",), t_window=None, title="") -> Figure

7.2 plot_bias_estimate(t, bias_est, bias_std, bias_true=None, n_sigma=2.0, title="") -> Figure


8. tier2.ipynb

The notebook must:

It must produce these sections, using seed 0 for every scenario:

Section Scenario Content
Standard S1 Table of RMSE and max abs error, in °, per method. plot_comparison with all four methods. plot_error_comparison excluding gyro, with t_window=(30, 40). plot_bias_estimate with truth.
Long run S2 Table of drift_rate per method in rad/s and °/s. plot_error_comparison excluding gyro.
Random-walk bias S3 KF angle and bias RMSE. The bias coverage fraction (KC1). plot_bias_estimate with truth.
Bias step S4 KF bias convergence time after the step. KF and CF max angle error in [60, 90] s. plot_bias_estimate and plot_error_comparison with t_window=(55, 95).
Tuning sweep S5 Seed 0 only. A heatmap or table of KF RMSE over the §9.4 grid, with the (1, 1) cell marked.
Discussion — Short markdown discussion, including the KF-vs-CF behavior at the bias step.

9. Scenarios and success criteria

Scenarios (all use simulate_pitch_imu defaults unless listed):

ID Simulator arguments Seeds
S1 defaults (60 s) — identical to Tier 1 0–9
S2 duration=600 0–4
S3 duration=600, gyro_bias_rw_std=1e-4 0–4
S4 duration=120, bias_step=(60.0, 0.01) 0–4
S5 duration=120, gyro_bias_rw_std=1e-4 0–2

Estimators:

Bias errors are computed against gyro_bias_true[:,1]. Angle errors are computed against pitch_true. “RMSE over t ≥ X” means the RMSE over the samples with t ≥ X. Each criterion must hold for every listed seed unless stated otherwise.

Expected values in brackets are planner estimates. They are not test values.

9.1 Statistical performance

| ID | Scen. | Criterion | (expected) | |—|—|—|—| | KP1 | S1 | KF angle RMSE over t ≥ 20 s ≤ 0.06° | (≈0.03°) | | KP2 | S1 | KF angle RMSE (full run) ≤ 0.6 × CF RMSE (full run, same seed) | (ratio ≈0.3) | | KP3 | S1 | |KF bias − b_y| ≤ 1e-3 rad/s (0.057 °/s) for all t ≥ 20 s | (max ≈5e-4) | | KP4 | S1 | KF max abs angle error over t ≥ 20 s ≤ 0.2° | (≈0.11°) | | KP5 | S2 | |KF drift_rate| ≤ 1e-5 rad/s (5.7e-4 °/s), and KF angle RMSE over t ≥ 540 s ≤ 0.06° | | | KP6 | S3 | KF angle RMSE over t ≥ 30 s ≤ 0.06° | (≈0.032°) | | KP7 | S3 | KF bias RMSE over t ≥ 30 s ≤ 5e-4 rad/s (0.029 °/s) | (≈2.4e-4) | | KP8 | S4 | convergence_time(t, KF bias, bias_true, tol=1e-3, t_start=60.0) ≤ 90 s, so the bias has settled within 30 s of the step | (≈73 s) | | KP9 | S4 | KF max abs angle error over 60 ≤ t ≤ 90 s ≤ 0.7° | (peak ≈0.4°) |

9.2 Consistency

| ID | Scen. | Criterion | (expected) | |—|—|—|—| | KC1 | S3 | coverage_fraction(KF bias, bias_true, sqrt(cov[:,1,1]), 2.0) over samples with t ≥ 30 s ≥ 0.90 | (≈0.95) |

9.3 Simulator extension

| ID | Criterion | |—|—| | KS1 | Regression. With all new options at their defaults, simulate_pitch_imu(seed=s) for s ∈ {0, 3} returns arrays identical (array_equal) to tests/data/tier1_reference.npz in the Tier 1 fields t, gyro, accel, roll_true, pitch_true and gyro_bias. Also, gyro_bias_true has every row equal to gyro_bias.
Fixture generation. The TESTER provides tests/make_tier1_reference.py, which must:
• import imu_sim from project_root and call simulate_pitch_imu(seed=s) for s ∈ {0, 3}, using only the 7 Tier 1 parameters
• save those six fields per seed to tests/data/tier1_reference.npz with keys seed{s}_{field} (e.g. seed0_gyro), using numpy.savez
• create tests/data/ if it is missing, refuse to overwrite an existing file unless --force is given, and print the path written
The owner runs it once on the Tier 1 code before the Tier 2 changes are installed. The KS1 test fails with a clear message (it does not skip) if the file is missing. | | KS2 | Random-walk statistics. With S3 settings and seed 0: gyro_bias_true[0] == gyro_bias. For each axis, diff(gyro_bias_true[:, i]) has std within ±5 % of 1e-4·sqrt(0.01) = 1e-5 and |mean| ≤ 4·1e-5/√(N−1). The pairwise |Pearson r| between the three diff columns is < 0.1. The residual gyro − true_rate − gyro_bias_true has std within ±5 % of 0.005 per axis. | | KS3 | Step. With bias_step=(30.0, 0.01) and no random walk: gyro_bias_true[:,1] == b_y for t < 30 and == b_y + 0.01 for t ≥ 30 (atol 1e-15). Columns 0 and 2 are constant. | | KS4 | Noise-free. With both noise stds 0, gyro_bias_rw_std=1e-4 and bias_step=(30.0, 0.01): gyro[:,1] == pitch_rate + gyro_bias_true[:,1] and gyro[:,i] == gyro_bias_true[:,i] for i = 0, 2 (1e-12). | | KS5 | Determinism. With S3 settings, the same seed gives identical gyro_bias_true and gyro. Seeds 0 and 1 give different gyro_bias_true. | | KS6 | Each ValueError case in §4.5 raises ValueError. |

9.4 Deterministic filter correctness and tuning

| ID | Criterion | |—|—| | KD1 | kalman_noise_matrices(0.01, 0.005, 0.05, 1e-4) returns Q = diag(2.5e-9, 1e-10) (rtol 1e-12, off-diagonals exactly 0) and R = (0.05/9.81)² (rtol 1e-12). | | KD2 | Recurrence. On random inputs with non-uniform t (N ≥ 50), non-default noise parameters, a given initial_angle, a non-zero initial_bias and a custom symmetric positive-definite initial_cov, the output matches the §5.3 recurrence. Angle and bias match to atol 1e-10. cov matches to rtol 1e-9 and atol 1e-20. | | KD3 | Constant bias, noise-free. constant_pitch_trajectory(0.3), both noise stds 0, 60 s, default bias, KF defaults: |angle[−1] − 0.3| ≤ 1e-6 rad and |bias[−1] − 0.01| ≤ 1e-6 rad/s. | | KD4 | Integration rule. Default trajectory, both noise stds 0, gyro_bias=(0,0,0), 60 s, KF defaults: max |angle − pitch_true| ≤ 1e-4 rad and max |bias| ≤ 1e-4 rad/s, both over t ≥ 10 s. A rectangle-rule implementation would give an angle error of about 2e-3 rad. | | KD5 | N = 1. The result is angle == [accel_angle[0]] (or initial_angle), bias == [initial_bias], and cov[0] == diag(R, 4e-4) with defaults (rtol 1e-12). | | KD6 | Covariance sanity. On S1 with seed 0, for all k: cov[k] is symmetric (atol 1e-18), both eigenvalues are > 0, and for k ≥ 1, cov[k,0,0] < R. | | KT1 | Tuning sweep. For S5 and seeds 0–2, scale factors sQ, sR ∈ {0.1, 0.3, 1, 3, 10} (25 grid points). Run KF with gyro_noise_std = 0.005·√sQ, bias_rw_std = 1e-4·√sQ and accel_noise_std = 0.05·√sR, other arguments at their defaults. The metric is the KF angle RMSE over t ≥ 30 s, averaged over the seeds. The value at (1, 1) must be ≤ 1.10 × the grid minimum. | | KT2 | Sweep sensitivity. On the KT1 grid, the grid maximum must be ≥ 1.25 × the value at (1, 1), which shows the sweep can discriminate between settings. KT1 and KT2 may share one computation, and together they must run in under 60 s. |

9.5 Metrics, plots, interface

| ID | Criterion | |—|—| | KM1 | convergence_time with t=[0,1,2,3,4,5], e=[0.5,0.2,0.05,0.2,0.01,0.01] (as estimate, with truth = 0) and tol=0.1 returns 4.0. With all |e| ≤ tol it returns t[0]. With the last sample violating it returns inf. With t_start=2.5, the first example returns 4.0, and with e[3]=0.05 it returns 3.0. The §6.1 errors raise ValueError. | | KM2 | coverage_fraction([0,1,3],[0,0,0],[1,1,1]) returns 2/3. coverage_fraction([2],[0],[1]) returns 1.0 (inclusive). The §6.2 errors raise ValueError. | | KG1 | plot_error_comparison with keys gyro/accel/CF/KF and the default exclude, without a window, gives 1 axes with 3 lines labelled accel, CF, KF, and y-data rad2deg(est − truth) (1e-9). With t_window=(30,40) it gives 2 axes, where axes 1 has x-limits (30, 40) (1e-9) and line x-data within [30, 40]. On S1 data, the y-limit rule of §7.1 holds on both axes. With exclude=(), axes 0 has 4 lines. | | KG2 | plot_bias_estimate with bias_true gives 1 axes, lines labelled “estimate” and “truth” with y-data in °/s (1e-9), and exactly one PolyCollection whose vertex y-range equals the §7.2 band (1e-9). Without bias_true it has only the “estimate” line. | | KI1 | T1 test I1 still passes. | | KI2 | inspect.signature names and defaults of simulate_pitch_imu (§4.1), kalman_noise_matrices, kalman_pitch, convergence_time, coverage_fraction, plot_error_comparison and plot_bias_estimate match the spec exactly. | | KI3 | Every ValueError case in §4.5, §5, §6 and §7 raises ValueError. | | KI4 | No new function modifies its input arrays. | | KI5 | KalmanResult fields have the §5.2 shapes and float64 dtype. The new metrics return Python or NumPy floats. | | KI6 | kalman_pitch on N = 60 000 (S3 seed 0) completes in under 2 s (wall clock, best of 3 runs). | | KR1 | The full Tier 1 test suite passes. Its I2 is updated to the §4.1 signature; no other Tier 1 test is changed. | | KG3 | tier2.ipynb executes via nbconvert without error in under 180 s. |


10. Deliverables and definition of done

EXECUTOR delivers:

TESTER delivers:

Owner runs python tests/make_tier1_reference.py on the Tier 1 code and keeps the resulting tests/data/tier1_reference.npz in the repository. Only after that are the EXECUTOR’s Tier 2 changes installed.

The whole tests/ suite must run in under 5 minutes.

Tier 2 is done when all Tier 1 and Tier 2 tests pass and the owner has reviewed the notebook plots.