5.9 KiB
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
- Constant-velocity linear Kalman filter: selected. It provides velocity-based prediction and covariance-aware association using fixed-size matrices.
- Alpha-beta filter: rejected because association would still depend mainly on manually tuned fixed distance gates.
- 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
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.