diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml index 01c5bdb..a777e1a 100644 --- a/.github/workflows/clang-tidy.yml +++ b/.github/workflows/clang-tidy.yml @@ -37,4 +37,5 @@ jobs: BirdsEye/dovex_header.cpp \ BirdsEye/filename_validator.cpp \ BirdsEye/sd_access_policy.cpp \ - BirdsEye/lap_format.cpp + BirdsEye/lap_format.cpp \ + BirdsEye/tach_filter.cpp diff --git a/BirdsEye/BirdsEye.ino b/BirdsEye/BirdsEye.ino index c9a526b..4db0b5d 100644 --- a/BirdsEye/BirdsEye.ino +++ b/BirdsEye/BirdsEye.ino @@ -258,12 +258,17 @@ volatile uint32_t tachLastPulseUs = 0; volatile bool tachHavePeriod = false; // Ring buffer: ISR writes pulse timestamps, TACH_LOOP reads and computes periods. -// Single-producer (ISR writes head), single-consumer (TACH_LOOP reads tail). -// 16 entries handles up to 20k RPM with 48ms of main-loop stall margin. +// Single-producer (ISR writes head), single-consumer (TACH_LOOP writes tail). +// The ISR checks full before publishing (one slot sacrificed so head==tail +// means empty) and drops + flags instead of lapping the consumer: SD GC +// stalls can block the main loop for 100 ms–2 s, far past what any sane +// ring size covers at racing RPM. tachRingTail is volatile because the ISR +// reads it for the full check. static const uint8_t TACH_RING_SIZE = 16; volatile uint32_t tachRingBuf[TACH_RING_SIZE]; volatile uint8_t tachRingHead = 0; // ISR write index (only ISR writes) -static uint8_t tachRingTail = 0; // Main-loop read index (only TACH_LOOP writes) +volatile uint8_t tachRingTail = 0; // Main-loop read index (only TACH_LOOP writes) +volatile bool tachRingOverflow = false; // ISR sets on drop; TACH_LOOP clears // Tunable constants static const float tachRevsPerPulse = 1.0f; // Wasted spark = 1 pulse/rev @@ -770,6 +775,16 @@ bool activeTimerSectorsConfigured() { void trackDetectionLoop() { if (trackDetected || !gpsData.fix || trackManifestCount == 0) return; + // Throttle the scan to 1 Hz. Each haversineDistanceMiles() is several + // software-emulated double libm calls (the M4F FPU is single-precision + // only); at the 200-entry manifest ceiling a full scan costs multiple + // milliseconds. gpsData.fix stays true BETWEEN PVT updates, so without + // this gate the scan ran every ~250 Hz loop iteration — collapsing the + // loop rate — for an answer that changes at driving pace. + static unsigned long lastManifestScan = 0; + if (millis() - lastManifestScan < 1000) return; + lastManifestScan = millis(); + double bestDist = 999999.0; int bestIndex = -1; diff --git a/BirdsEye/gps_functions.ino b/BirdsEye/gps_functions.ino index 334367a..d25d272 100644 --- a/BirdsEye/gps_functions.ino +++ b/BirdsEye/gps_functions.ino @@ -515,6 +515,15 @@ void GPS_RECONFIGURE() { bool GPS_BAUD_RECOVERY() { debugln(F("GPS baud recovery: attempting...")); + // This path runs from GPS_LOOP() under the armed ~4 s hardware watchdog, + // and it is the slowest thing in the firmware short of the OTA apply: + // up to three myGNSS.begin() probes (~1.1 s of ping timeouts each) plus + // ~500 ms of explicit delay(). Against a genuinely hung module that + // out-waits the WDT → reset → re-setup → recovery → reset boot loop — + // the one path built to revive a sick GPS must not trip the watchdog. + // Pet before every blocking probe (same rationale as fwStageToFlash()). + wdtPet(); + // Stop timer so GpsBufferedStream passes through to Serial1 directly. // The SparkFun library needs direct serial access for begin()/setSerialRate(). stopGpsSerialTimer(); @@ -532,6 +541,7 @@ bool GPS_BAUD_RECOVERY() { // Module not responding at 57600 — try 9600 (factory default) debugln(F("GPS baud recovery: trying 9600...")); + wdtPet(); // the first begin() just burned ~1 s of the 4 s budget GPS_SERIAL.end(); delay(50); GPS_SERIAL.begin(9600); @@ -540,12 +550,14 @@ bool GPS_BAUD_RECOVERY() { if (myGNSS.begin(gpsStream)) { // Found at 9600 — switch it back to 57600 debugln(F("GPS baud recovery: found at 9600, switching to 57600")); + wdtPet(); // second begin() done; baud switch + final probe still ahead myGNSS.setSerialRate(GPS_BAUD_RATE); delay(100); GPS_SERIAL.end(); delay(50); GPS_SERIAL.begin(GPS_BAUD_RATE); delay(100); + wdtPet(); if (myGNSS.begin(gpsStream)) { debugln(F("GPS baud recovery: reconnected at 57600")); @@ -562,6 +574,7 @@ bool GPS_BAUD_RECOVERY() { } // Restore 57600 even on failure so other code doesn't break + wdtPet(); GPS_SERIAL.end(); delay(50); GPS_SERIAL.begin(GPS_BAUD_RATE); diff --git a/BirdsEye/tach_filter.cpp b/BirdsEye/tach_filter.cpp new file mode 100644 index 0000000..d6f55c7 --- /dev/null +++ b/BirdsEye/tach_filter.cpp @@ -0,0 +1,33 @@ +#include "tach_filter.h" + +namespace tach_filter { + +void reset(Kalman& k) { + k.x = 0.0f; + k.p = kInitialUncertaintyP; +} + +void update(Kalman& k, float rpmMeasured, int periodCount) { + if (periodCount <= 0) return; + + // Predict step: constant-RPM model, uncertainty grows + k.p += kProcessNoiseQ; + + // Measurement noise scales inversely with number of periods + const float r = kMeasurementNoiseRBase / (float)periodCount; + + // Update step + const float gain = k.p / (k.p + r); + k.x += gain * (rpmMeasured - k.x); + k.p *= (1.0f - gain); + + // Uncertainty floor to prevent numerical collapse + if (k.p < kUncertaintyFloorP) k.p = kUncertaintyFloorP; +} + +float rpmFromMeanPeriodUs(float meanPeriodUs, float revsPerPulse) { + if (meanPeriodUs <= 0.0f || revsPerPulse <= 0.0f) return 0.0f; + return (60.0e6f * revsPerPulse) / meanPeriodUs; +} + +} // namespace tach_filter diff --git a/BirdsEye/tach_filter.h b/BirdsEye/tach_filter.h new file mode 100644 index 0000000..ac6e82e --- /dev/null +++ b/BirdsEye/tach_filter.h @@ -0,0 +1,53 @@ +#pragma once + +/////////////////////////////////////////// +// TACHOMETER KALMAN FILTER +// The 1-D Kalman filter that turns mean inter-pulse periods into a +// smoothed RPM estimate, extracted from tachometer.ino so the math is +// host-testable. The ISR/ring-buffer plumbing stays in the sketch; this +// unit owns the predict/update equations and the tuning constants. +// +// Pure logic — no Arduino headers — so it is exercised by host tests. +/////////////////////////////////////////// + +namespace tach_filter { + +// Process noise Q: how much RPM^2 the true value can change between +// updates. A kart engine with a light flywheel can shift ~200 RPM per +// pulse at 5k RPM. 800 is conservative-smooth; raise to 1500–2000 if +// tracking feels sluggish. +constexpr float kProcessNoiseQ = 800.0f; + +// Measurement noise R base: variance of a single-period RPM measurement +// (~50 RPM std dev from combustion variation + ISR latency jitter). +// Scales inversely with the number of periods in the measurement — +// more pulses, more confidence. +constexpr float kMeasurementNoiseRBase = 2500.0f; + +// Uncertainty assigned at reset (engine stop / sleep / boot): high, so +// the first real measurement dominates the stale estimate. +constexpr float kInitialUncertaintyP = 10000.0f; + +// Floor that keeps the uncertainty from collapsing numerically to zero +// (which would make the filter stop tracking). +constexpr float kUncertaintyFloorP = 1.0f; + +struct Kalman { + float x = 0.0f; // RPM estimate + float p = kInitialUncertaintyP; // estimate uncertainty (RPM^2) +}; + +// Return the estimate to the rest state (RPM 0, high uncertainty). +void reset(Kalman& k); + +// Fold one measurement into the estimate: `rpmMeasured` is the RPM +// implied by the mean of `periodCount` inter-pulse periods. periodCount +// <= 0 is a no-op. +void update(Kalman& k, float rpmMeasured, int periodCount); + +// Convert a mean inter-pulse period (microseconds) to RPM for a pickup +// producing `revsPerPulse` revolutions per pulse (wasted spark = 1.0). +// Non-positive inputs return 0. +float rpmFromMeanPeriodUs(float meanPeriodUs, float revsPerPulse); + +} // namespace tach_filter diff --git a/BirdsEye/tachometer.ino b/BirdsEye/tachometer.ino index c4422f3..dc15360 100644 --- a/BirdsEye/tachometer.ino +++ b/BirdsEye/tachometer.ino @@ -15,20 +15,12 @@ /////////////////////////////////////////// #include "tachometer.h" +#include "tach_filter.h" // ---- Kalman filter state ---- -static float kalmanX = 0.0f; // RPM estimate -static float kalmanP = 10000.0f; // Estimate uncertainty (RPM^2) - -// Process noise Q: how much RPM^2 can change between Kalman updates. -// A kart engine with light flywheel can shift ~200 RPM per pulse at 5k RPM. -// Q=800 is conservative-smooth. Increase to 1500-2000 if tracking feels sluggish. -static const float KALMAN_Q = 800.0f; - -// Measurement noise R_BASE: uncertainty of a single-period RPM measurement. -// ~50 RPM std dev from combustion variation + ISR latency jitter = variance 2500. -// Scales inversely with number of periods: more pulses → lower noise. -static const float KALMAN_R_BASE = 2500.0f; +// The predict/update math and the tuning constants (Q, R_BASE, the +// uncertainty floor) live in the host-tested tach_filter pure unit. +static tach_filter::Kalman tachKalman; // After engine-stopped timeout, the first period is garbage (it spans the // entire stopped duration). This flag discards it. @@ -55,15 +47,29 @@ void TACH_COUNT_PULSE() { // Time-based debounce: reject ignition ringing within 3ms of last valid pulse if (dt < tachMinPulseGapUs) return; - // Record timestamp tachLastPulseUs = now; - tachRingBuf[tachRingHead] = now; - // ARM Cortex-M4: single-byte write is atomic. Data is visible before head - // advances because stores are observed in program order on same processor. - tachRingHead = (tachRingHead + 1) % TACH_RING_SIZE; - // Wake trigger for sleep mode (BirdsEye.ino reads this) + // Wake trigger for sleep mode (BirdsEye.ino reads this). Set even when + // the ring is full below — a dropped timestamp is still a real pulse. tachHavePeriod = true; + + // SPSC full check (one slot is sacrificed so head==tail is unambiguously + // "empty"). TACH_LOOP can be blocked for 100 ms–2 s by the same SD GC + // stalls the GPS serial ring exists for; at 6000 RPM a 200 ms stall + // overruns 16 entries, and advancing head unconditionally would lap the + // consumer and corrupt every period it computes. Dropping the pulse and + // flagging the gap is safe — the consumer discards the one period that + // spans the gap and the Kalman estimate coasts until fresh data arrives. + uint8_t nextHead = (tachRingHead + 1) % TACH_RING_SIZE; + if (nextHead == tachRingTail) { + tachRingOverflow = true; + return; + } + + // Data is visible before head advances because stores are observed in + // program order on the same processor (single-byte head write is atomic). + tachRingBuf[tachRingHead] = now; + tachRingHead = nextHead; } /** @@ -132,37 +138,35 @@ void TACH_LOOP() { periodSum += periods[i]; } float meanPeriodUs = (float)periodSum / (float)periodCount; - float rpmMeasured = (60.0e6f * tachRevsPerPulse) / meanPeriodUs; - - // Predict step: constant-RPM model, uncertainty grows - kalmanP += KALMAN_Q; - - // Measurement noise scales inversely with number of periods - float R = KALMAN_R_BASE / (float)periodCount; - - // Update step - float K = kalmanP / (kalmanP + R); - kalmanX += K * (rpmMeasured - kalmanX); - kalmanP *= (1.0f - K); - - // Uncertainty floor to prevent numerical collapse - if (kalmanP < 1.0f) kalmanP = 1.0f; + float rpmMeasured = + tach_filter::rpmFromMeanPeriodUs(meanPeriodUs, tachRevsPerPulse); + tach_filter::update(tachKalman, rpmMeasured, periodCount); } } + // ---- Step 3.5: Ring overflow recovery ---- + // The ISR dropped pulses while the ring was full (main loop stalled). + // The period between the last retained timestamp and the next retained + // pulse spans the whole gap and is NOT a real measurement — drop the + // carried prev-timestamp so that period is never computed. The estimate + // simply coasts until consecutive post-gap pulses arrive. + if (tachRingOverflow) { + tachRingOverflow = false; + tachHavePrevTimestamp = false; + } + // ---- Step 4: Engine-stopped timeout ---- // 32-bit reads are atomic on ARM Cortex-M4, no noInterrupts() needed uint32_t lastPulseUs = tachLastPulseUs; if ((uint32_t)(micros() - lastPulseUs) > tachStopTimeoutUs) { - kalmanX = 0.0f; - kalmanP = 10000.0f; // High uncertainty for next startup + tach_filter::reset(tachKalman); // High uncertainty for next startup tachNeedFirstPulseDiscard = true; tachHavePrevTimestamp = false; tachRingTail = tachRingHead; // Flush ring buffer } // ---- Step 5: Update reported value ---- - tachLastReported = (int)(kalmanX + 0.5f); + tachLastReported = (int)(tachKalman.x + 0.5f); } /** @@ -184,9 +188,9 @@ void TACH_SLEEP() { // computation starts clean instead of chewing on stale timestamps (the // first post-wake period would otherwise span the whole sleep). tachRingTail = tachRingHead; + tachRingOverflow = false; tachHavePrevTimestamp = false; tachNeedFirstPulseDiscard = true; - kalmanX = 0.0f; - kalmanP = 10000.0f; + tach_filter::reset(tachKalman); tachLastReported = 0; } diff --git a/CHANGELOG.md b/CHANGELOG.md index 7ca972e..0bca209 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,27 @@ and this project aims to follow [Semantic Versioning](https://semver.org/spec/v2 library release is validated. ### Fixed +- **Track detection no longer throttles the whole main loop.** The manifest + scan (O(N) software-double haversine, several ms at the 200-entry ceiling) + was gated only on `gpsData.fix`, which stays true between PVT updates — so + it ran every ~250 Hz loop iteration instead of "every GPS fix" as + documented, dragging the loop rate down while hunting for a track. The + scan is now throttled to 1 Hz, which is still instant at driving pace. +- **Tach ring buffer can no longer be lapped during SD stalls.** The pulse + ISR advanced the ring head unconditionally; an SD GC stall (the documented + 100 ms–2 s, the same reason the GPS serial ring exists) at racing RPM + overruns the 16-entry buffer, breaking the ring invariant and producing a + confident-but-wrong RPM that was logged to CSV and fed the >500 RPM + auto-race trigger. The ISR now checks full and drops the pulse instead, + flagging the gap so `TACH_LOOP()` discards the one period spanning it — + the Kalman estimate coasts briefly rather than going silently wrong. The + filter math itself moved to the host-tested `tach_filter` pure unit. +- **GPS baud recovery no longer risks tripping the 4 s hardware watchdog.** + `GPS_BAUD_RECOVERY()` can block for up to three ~1.1 s module probes plus + ~500 ms of delays — against a genuinely hung GPS that out-waited the WDT, + causing a reset → re-setup → recovery → reset boot loop. It now pets the + watchdog before each blocking probe (same treatment `fwStageToFlash()` + already had). - **Sleep mode actually sleeps now.** The tach ISR latched `tachHavePeriod` on the first engine pulse since boot and nothing ever cleared it, so every sleep entry (long-press, 5-min menu idle, USB) instantly bounced through diff --git a/CLAUDE.md b/CLAUDE.md index 5985706..7d669da 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -102,6 +102,7 @@ desktop toolchain. This is where logic worth unit-testing lives. | `crc32.{h,cpp}` | CRC-32/IEEE-802.3 (zlib) incremental + hex; pins firmware-OTA CRC to the web client | | `sd_access_policy.{h,cpp}` | SD access arbitration decision table (mode values + grant/deny rules) | | `lap_format.{h,cpp}` | ms → `M:SS.mmm` lap-time rendering (three zero-minutes styles), used by all display pages | +| `tach_filter.{h,cpp}` | Tachometer 1-D Kalman filter (predict/update math + Q/R tuning constants) | ### Non-Source @@ -201,11 +202,16 @@ loop() ~250 Hz - ISR `TACH_COUNT_PULSE()` fires on falling edge of D0. - 3 ms minimum pulse gap (supports up to ~20 000 RPM). - **Ring buffer architecture**: ISR timestamps every valid pulse into a - 16-entry ring buffer (`tachRingBuf`). `TACH_LOOP()` drains the buffer + 16-entry ring buffer (`tachRingBuf`). The ISR checks full before + publishing (SPSC, one slot sacrificed) and drops + sets + `tachRingOverflow` instead of lapping the consumer during SD GC stalls; + `TACH_LOOP()` then discards the one period spanning the gap. `TACH_LOOP()` drains the buffer each main-loop iteration, computes mean inter-pulse period from ALL accumulated pulses, and feeds the result through a 1D Kalman filter. - **Kalman filter** replaces the old median-of-3 + EMA. Two floats of - state (RPM estimate `kalmanX` + uncertainty `kalmanP`). Process noise + state (estimate + uncertainty in `tach_filter::Kalman`); the + predict/update math and tuning constants live in the host-tested + `tach_filter` pure unit. Process noise Q = 800 (tuned for kart engine inertia). Measurement noise R scales inversely with pulse count (more pulses = more confident). - Time-based debounce only (3 ms). Old volatile flag gate removed — ISR @@ -378,7 +384,9 @@ loop() ~250 Hz immediate Lap Anything activation. - **Track detection flow** (`trackDetectionLoop()`): 1. Valid GPS time lock acquired → DOVEX log file created (see GPS section). - 2. Every GPS fix, scans `trackManifest[]` via haversine. + 2. Scans `trackManifest[]` via haversine, throttled to 1 Hz (the scan + is O(N) software-double math; `gpsData.fix` stays true between PVT + updates, so an unthrottled scan ran every ~250 Hz loop iteration). 3. Closest match within 5 miles → parse full JSON, build `TrackConfig`. 4. Create `CourseManager` with settings-configurable thresholds. 5. CourseManager handles course detection + Lap Anything fallback. @@ -583,8 +591,9 @@ Stored in `trackLayouts[MAX_LAYOUTS]` (max 10 per track). | Track detect radius | 5 miles | `BirdsEye.ino` | | Tach min pulse gap | 3 ms | `BirdsEye.ino` | | Tach ring buffer | 16 entries | `BirdsEye.ino` | -| Tach Kalman Q | 800 RPM² | `tachometer.ino` | -| Tach Kalman R_BASE | 2500 RPM² | `tachometer.ino` | +| Tach Kalman Q | 800 RPM² | `tach_filter.h` | +| Tach Kalman R_BASE | 2500 RPM² | `tach_filter.h` | +| Track manifest scan throttle | 1 Hz | `BirdsEye.ino` | | Tach stop timeout | 500 ms | `BirdsEye.ino` | | Display refresh | 3 Hz | `display_ui.ino` | | Button debounce | 200 ms | `display_ui.ino` | diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 816730a..3a1bb5c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -19,6 +19,7 @@ add_executable(birdseye_tests crc32_test.cpp sd_access_policy_test.cpp lap_format_test.cpp + tach_filter_test.cpp ${BIRDSEYE_DIR}/haversine.cpp ${BIRDSEYE_DIR}/gps_time.cpp ${BIRDSEYE_DIR}/gps_validation.cpp @@ -27,6 +28,7 @@ add_executable(birdseye_tests ${BIRDSEYE_DIR}/crc32.cpp ${BIRDSEYE_DIR}/sd_access_policy.cpp ${BIRDSEYE_DIR}/lap_format.cpp + ${BIRDSEYE_DIR}/tach_filter.cpp ) target_include_directories(birdseye_tests PRIVATE diff --git a/tests/tach_filter_test.cpp b/tests/tach_filter_test.cpp new file mode 100644 index 0000000..f4c6ab3 --- /dev/null +++ b/tests/tach_filter_test.cpp @@ -0,0 +1,115 @@ +#include "doctest.h" +#include "tach_filter.h" + +#include + +using namespace tach_filter; + +// --------------------------------------------------------------------------- +// rpmFromMeanPeriodUs — period → RPM conversion +// --------------------------------------------------------------------------- + +TEST_CASE("rpmFromMeanPeriodUs - real engine speeds at wasted spark") { + // 12 ms per rev = 5000 RPM (the comment in tachometer.ino's header) + CHECK(rpmFromMeanPeriodUs(12000.0f, 1.0f) == doctest::Approx(5000.0f)); + // 3 ms = the debounce floor = 20k RPM ceiling + CHECK(rpmFromMeanPeriodUs(3000.0f, 1.0f) == doctest::Approx(20000.0f)); + // 2 s = the sanity-bound ceiling = 30 RPM floor + CHECK(rpmFromMeanPeriodUs(2000000.0f, 1.0f) == doctest::Approx(30.0f)); +} + +TEST_CASE("rpmFromMeanPeriodUs - revsPerPulse scales linearly") { + // Half a rev per pulse (e.g. 2 pulses/rev pickup) halves the RPM. + CHECK(rpmFromMeanPeriodUs(12000.0f, 0.5f) == doctest::Approx(2500.0f)); + CHECK(rpmFromMeanPeriodUs(12000.0f, 2.0f) == doctest::Approx(10000.0f)); +} + +TEST_CASE("rpmFromMeanPeriodUs - non-positive inputs return 0") { + CHECK(rpmFromMeanPeriodUs(0.0f, 1.0f) == 0.0f); + CHECK(rpmFromMeanPeriodUs(-100.0f, 1.0f) == 0.0f); + CHECK(rpmFromMeanPeriodUs(12000.0f, 0.0f) == 0.0f); +} + +// --------------------------------------------------------------------------- +// Kalman update behavior +// --------------------------------------------------------------------------- + +TEST_CASE("Kalman - reset state is rest with high uncertainty") { + Kalman k; + k.x = 4321.0f; + k.p = 5.0f; + reset(k); + CHECK(k.x == 0.0f); + CHECK(k.p == kInitialUncertaintyP); +} + +TEST_CASE("Kalman - first update after reset jumps most of the way") { + // High post-reset uncertainty means the first measurement dominates: + // gain = (10000+800)/(10000+800+2500) ≈ 0.81. + Kalman k; + update(k, 5000.0f, 1); + CHECK(k.x > 4000.0f); + CHECK(k.x < 5000.0f); +} + +TEST_CASE("Kalman - converges to a constant measurement") { + Kalman k; + for (int i = 0; i < 50; i++) { + update(k, 5000.0f, 3); + } + CHECK(k.x == doctest::Approx(5000.0f).epsilon(0.001)); +} + +TEST_CASE("Kalman - approach to a constant input is monotonic (no overshoot)") { + Kalman k; + float prev = k.x; + for (int i = 0; i < 20; i++) { + update(k, 6000.0f, 2); + CHECK(k.x > prev); // climbing toward the measurement… + CHECK(k.x <= 6000.0f); // …without ever passing it + prev = k.x; + } +} + +TEST_CASE("Kalman - more periods per measurement means faster convergence") { + // R scales as R_BASE/periodCount, so a batch of 8 periods pulls the + // estimate harder than a single period does. + Kalman one, eight; + update(one, 5000.0f, 1); + update(eight, 5000.0f, 8); + CHECK(eight.x > one.x); +} + +TEST_CASE("Kalman - uncertainty never collapses below the floor") { + Kalman k; + for (int i = 0; i < 1000; i++) { + update(k, 5000.0f, 8); + } + CHECK(k.p >= kUncertaintyFloorP); +} + +TEST_CASE("Kalman - non-positive period count is a no-op") { + Kalman k; + update(k, 5000.0f, 3); + const float x = k.x; + const float p = k.p; + update(k, 9999.0f, 0); + update(k, 9999.0f, -1); + CHECK(k.x == x); + CHECK(k.p == p); +} + +TEST_CASE("Kalman - tracks a ramp like a real engine pull") { + // Feed an accelerating input (25 Hz batches, +100 RPM per batch) and + // require the estimate to lag but follow within the process noise's + // ability to track crankshaft inertia. + Kalman k; + float rpm = 3000.0f; + for (int i = 0; i < 40; i++) { + update(k, rpm, 3); + rpm += 100.0f; + } + // input is now 6900 (last fed 6900-100); estimate must be close behind + CHECK(k.x > 6000.0f); + CHECK(k.x < rpm); +}