# 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.
