From 646a3871e8731be2fd7ac7f61588afca0dbdd066 Mon Sep 17 00:00:00 2001
From: Orange <2314753575@qq.com>
Date: Mon, 20 Jul 2026 17:32:25 +0800
Subject: [PATCH] docs: design lightweight Kalman obstacle tracker

---
 ...07-20-lightweight-kalman-tracker-design.md | 153 ++++++++++++++++++
 1 file changed, 153 insertions(+)
 create mode 100644 docs/superpowers/specs/2026-07-20-lightweight-kalman-tracker-design.md

diff --git a/docs/superpowers/specs/2026-07-20-lightweight-kalman-tracker-design.md b/docs/superpowers/specs/2026-07-20-lightweight-kalman-tracker-design.md
new file mode 100644
index 0000000..a73c586
--- /dev/null
+++ b/docs/superpowers/specs/2026-07-20-lightweight-kalman-tracker-design.md
@@ -0,0 +1,153 @@
+# Lightweight Kalman Obstacle Tracker Design
+
+## Goal
+
+Replace the current exponential-smoothing tracker with a true lightweight
+constant-velocity Kalman filter. The tracker must improve association and
+short dropout handling for a robot moving at up to 2 m/s, without using TF and
+without materially increasing RDKx5 CPU load.
+
+## Operating Conditions
+
+- ROS2 Humble on RDKx5.
+- LaserScan rate is approximately 12 Hz.
+- Robot speed is at most 2 m/s, corresponding to about 0.167 m translation per
+  scan.
+- The environment is simple: walls and two or three target signboards.
+- Two-point observations may update an existing track but may not create one.
+- Track coordinates remain in the laser frame. No TF or odometry dependency is
+  introduced in this version.
+
+## Alternatives Considered
+
+1. Constant-velocity linear Kalman filter: selected. It provides velocity-based
+   prediction and covariance-aware association using fixed-size matrices.
+2. Alpha-beta filter: rejected because association would still depend mainly on
+   manually tuned fixed distance gates.
+3. TF/odometry compensated filter: deferred because transform data can have
+   significant latency on this robot.
+
+## State And Prediction
+
+Each track contains the state `[x, y, vx, vy]` and a fixed-size 4x4 covariance
+matrix. The transition model is constant velocity and uses `dt` calculated from
+consecutive LaserScan timestamps. Invalid, non-positive, or unusually large
+time deltas are clamped to a safe range around the nominal scan period so that
+velocity and covariance cannot diverge after clock discontinuities.
+
+The process model uses configurable acceleration noise. Initial velocity is
+zero, but initial velocity covariance is intentionally broad enough to associate
+the second observation after a 0.167 m frame-to-frame displacement.
+
+## Measurements
+
+Circle fitting produces a two-dimensional center measurement and can initialize
+a tentative track. Its measurement covariance is relatively small.
+
+A two-point chord uses the track's predicted radius to construct the unique
+center farther from the laser origin. It has a larger measurement covariance,
+does not change track radius, and can only update an existing track.
+
+Circle measurements update radius with conservative smoothing. Chord
+measurements never update radius.
+
+## Association
+
+All tracks are predicted before association. Circle measurements are associated
+first, followed by chord measurements. Candidate pairs must pass both:
+
+- squared Mahalanobis distance at or below `9.21`;
+- Euclidean center distance at or below `0.35 m`.
+
+Pair selection is global greedy assignment over all valid observation-track
+pairs sorted by innovation score. Each observation and each track can be used at
+most once per source pass. This removes the current observation-order bias while
+remaining trivial for the expected three or four tracks.
+
+An unmatched circle measurement creates a tentative track. Unmatched chord
+measurements are discarded.
+
+## Track Lifecycle
+
+A new track starts as tentative and is not published. It becomes confirmed
+after either:
+
+- two circle-fit updates; or
+- one circle-fit initialization followed by enough reliable chord updates to
+  reach three total updates.
+
+A tentative track is deleted after one missed frame. A confirmed track remains
+internally available for association for five missed frames.
+
+A confirmed track may publish predicted coordinates for at most three missed
+frames, approximately 0.25 seconds at 12 Hz. Predicted output stops earlier when
+the largest position standard deviation exceeds `0.15 m`. This separates
+reassociation lifetime from externally visible stale-data lifetime.
+
+When an observation falls outside the association gates, the old track is not
+silently reassigned. A new circle observation may form a tentative track, but
+neither the old prediction nor the new tentative track creates an immediate
+duplicate published obstacle.
+
+## Initial Parameters
+
+```yaml
+process_accel_noise: 3.0
+initial_velocity_stddev: 2.5
+fit_position_stddev: 0.02
+chord_position_stddev: 0.06
+mahalanobis_gate: 9.21
+max_association_distance: 0.35
+max_position_stddev: 0.15
+track_confirm_fit_hits: 2
+track_confirm_total_hits: 3
+track_publish_misses: 3
+track_delete_misses: 5
+```
+
+Every parameter will be documented in `config/params.yaml`. Obsolete EMA and
+fixed nearest-neighbor parameters will be removed rather than retained as dead
+configuration.
+
+## Debug Information
+
+The existing compact debug topic remains controlled by `debug` as the master
+switch and `debug_info` as the textual-output switch. It gains these counters:
+
+- tracks created, confirmed, and deleted;
+- fit and chord Kalman updates;
+- association rejections;
+- predicted tracks currently published;
+- mean and maximum accepted innovation.
+
+The debug path must not alter tracker decisions. Production operation continues
+to use `debug: false` when visualization and diagnostic output are unnecessary.
+
+## Testing
+
+Unit tests will cover:
+
+- constant-velocity prediction and association at 2 m/s and 12 Hz;
+- suppression of a one-frame false circle;
+- confirmation by two circle fits;
+- confirmation by one fit followed by two valid chord updates;
+- predicted publication for three misses and suppression on the fourth;
+- internal deletion after five misses;
+- covariance-based early publication suppression;
+- rejection of distant fit and chord observations;
+- invalid and discontinuous timestamps;
+- preservation of existing circle-fit and chord geometry behavior.
+
+Tests are written and observed failing before production implementation. Final
+verification uses a Release build with `colcon build --symlink-install`, package
+tests, and a short ROS2 launch/topic smoke test on domain 22 without replacing
+or terminating unrelated running nodes.
+
+## Scope Exclusions
+
+- TF, odometry, IMU, or command-velocity compensation;
+- nonlinear turn-rate motion models;
+- Hungarian assignment;
+- creation of obstacles from two points alone;
+- browser visualization changes;
+- unrelated navigation or workspace cleanup.