Localization Data Caching State Machine

Context: During percep/nav kill–rerun cycles the ZED IMU often stops publishing, so the TF stream stalls and percep/nav can’t proceed. The IEKF, unlike the pose filter, does keep “running” in the sense that it can still accept sporadic position/heading corrections. Still, without regular IMU propagation/corrections it updates far less. It becomes prone to drift/divergence, and in our current code it also stops publishing TF because TF is pushed inside the IMU callback.

Proposed Solution: Cache the last “valid” IEKF state while all sensors are reliable, store X (pose), P (covariance), timestamp, and a confidence flag. Add a small TF keepalive node that republishes the most recent valid IEKF TF from a ring buffer during TF gaps so percep/nav never lose transforms. When IMU (and normal IEKF operation) returns, re-seed the IEKF by setting X to the cached last‑valid state and resetting P to the default nominal covariance so it behaves like a clean initial guess, then resume corrections. On recovery, compare the first live IEKF TF to the last keepalive TF; if Δpos/Δyaw are small, snap; if they’re large, blend back over ~1–2 s to avoid jumps. For yaw, prioritize dual‑antenna RTK heading (live, else fresh cached), then mag, then last yaw. This keeps percep/nav online through outages and stabilizes heading without relying on drive‑forward.