diff --git a/docs/adr/ADR-026-survivor-track-lifecycle.md b/docs/adr/ADR-026-survivor-track-lifecycle.md new file mode 100644 index 0000000..d76e531 --- /dev/null +++ b/docs/adr/ADR-026-survivor-track-lifecycle.md @@ -0,0 +1,208 @@ +# ADR-026: Survivor Track Lifecycle Management for MAT Crate + +**Status:** Accepted +**Date:** 2026-03-01 +**Deciders:** WiFi-DensePose Core Team +**Domain:** MAT (Mass Casualty Assessment Tool) — `wifi-densepose-mat` +**Supersedes:** None +**Related:** ADR-001 (WiFi-MAT disaster detection), ADR-017 (ruvector signal/MAT integration) + +--- + +## Context + +The MAT crate's `Survivor` entity has `SurvivorStatus` states +(`Active / Rescued / Lost / Deceased / FalsePositive`) and `is_stale()` / +`mark_lost()` methods, but these are insufficient for real operational use: + +1. **Manually driven state transitions** — no controller automatically fires + `mark_lost()` when signal drops for N consecutive frames, nor re-activates + a survivor when signal reappears. + +2. **Frame-local assignment only** — `DynamicPersonMatcher` (metrics.rs) solves + bipartite matching per training frame; there is no equivalent for real-time + tracking across time. + +3. **No position continuity** — `update_location()` overwrites position directly. + Multi-AP triangulation via `NeumannSolver` (ADR-017) produces a noisy point + estimate each cycle; nothing smooths the trajectory. + +4. **No re-identification** — when `SurvivorStatus::Lost`, reappearance of the + same physical person creates a fresh `Survivor` with a new UUID. Vital-sign + history is lost and survivor count is inflated. + +### Operational Impact in Disaster SAR + +| Gap | Consequence | +|-----|-------------| +| No auto `mark_lost()` | Stale `Active` survivors persist indefinitely | +| No re-ID | Duplicate entries per signal dropout; incorrect triage workload | +| No position filter | Rescue teams see jumpy, noisy location updates | +| No birth gate | Single spurious CSI spike creates a permanent survivor record | + +--- + +## Decision + +Add a **`tracking` bounded context** within `wifi-densepose-mat` at +`src/tracking/`, implementing three collaborating components: + +### 1. Kalman Filter — Constant-Velocity 3-D Model (`kalman.rs`) + +State vector `x = [px, py, pz, vx, vy, vz]` (position + velocity in metres / m·s⁻¹). + +| Parameter | Value | Rationale | +|-----------|-------|-----------| +| Process noise σ_a | 0.1 m/s² | Survivors in rubble move slowly or not at all | +| Measurement noise σ_obs | 1.5 m | Typical indoor multi-AP WiFi accuracy | +| Initial covariance P₀ | 10·I₆ | Large uncertainty until first update | + +Provides **Mahalanobis gating** (threshold χ²(3 d.o.f.) = 9.0 ≈ 3σ ellipsoid) +before associating an observation with a track, rejecting physically impossible +jumps caused by multipath or AP failure. + +### 2. CSI Fingerprint Re-Identification (`fingerprint.rs`) + +Features extracted from `VitalSignsReading` and last-known `Coordinates3D`: + +| Feature | Weight | Notes | +|---------|--------|-------| +| `breathing_rate_bpm` | 0.40 | Most stable biometric across short gaps | +| `breathing_amplitude` | 0.25 | Varies with debris depth | +| `heartbeat_rate_bpm` | 0.20 | Optional; available from `HeartbeatDetector` | +| `location_hint [x,y,z]` | 0.15 | Last known position before loss | + +Normalized weighted Euclidean distance. Re-ID fires when distance < 0.35 and +the `Lost` track has not exceeded `max_lost_age_secs` (default 30 s). + +### 3. Track Lifecycle State Machine (`lifecycle.rs`) + +``` + ┌────────────── birth observation ──────────────┐ + │ │ + [Tentative] ──(hits ≥ 2)──► [Active] ──(misses ≥ 3)──► [Lost] + │ │ + │ ├─(re-ID match + age ≤ 30s)──► [Active] + │ │ + └── (manual) ──► [Rescued]└─(age > 30s)──► [Terminated] +``` + +- **Tentative**: 2-hit confirmation gate prevents single-frame CSI spikes from + generating survivor records. +- **Active**: normal tracking; updated each cycle. +- **Lost**: Kalman predicts position; re-ID window open. +- **Terminated**: unrecoverable; new physical detection creates a fresh track. +- **Rescued**: operator-confirmed; metrics only. + +### 4. `SurvivorTracker` Aggregate Root (`tracker.rs`) + +Per-tick algorithm: + +``` +update(observations, dt_secs): + 1. Predict — advance Kalman state for all Active + Lost tracks + 2. Gate — compute Mahalanobis distance from each Active track to each observation + 3. Associate — greedy nearest-neighbour (gated); Hungarian for N ≤ 10 + 4. Re-ID — unmatched observations vs Lost tracks via CsiFingerprint + 5. Birth — still-unmatched observations → new Tentative tracks + 6. Update — matched tracks: Kalman update + vitals update + lifecycle.hit() + 7. Lifecycle — unmatched tracks: lifecycle.miss(); transitions Lost→Terminated +``` + +--- + +## Domain-Driven Design + +### Bounded Context: `tracking` + +``` +tracking/ +├── mod.rs — public API re-exports +├── kalman.rs — KalmanState value object +├── fingerprint.rs — CsiFingerprint value object +├── lifecycle.rs — TrackState enum, TrackLifecycle entity, TrackerConfig +└── tracker.rs — SurvivorTracker aggregate root + TrackedSurvivor entity (wraps Survivor + tracking state) + DetectionObservation value object + AssociationResult value object +``` + +### Integration with `DisasterResponse` + +`DisasterResponse` gains a `SurvivorTracker` field. In `scan_cycle()`: + +1. Detections from `DetectionPipeline` become `DetectionObservation`s. +2. `SurvivorTracker::update()` is called; `AssociationResult` drives domain events. +3. `DisasterResponse::survivors()` returns `active_tracks()` from the tracker. + +### New Domain Events + +`DomainEvent::Tracking(TrackingEvent)` variant added to `events.rs`: + +| Event | Trigger | +|-------|---------| +| `TrackBorn` | Tentative → Active (confirmed survivor) | +| `TrackLost` | Active → Lost (signal dropout) | +| `TrackReidentified` | Lost → Active (fingerprint match) | +| `TrackTerminated` | Lost → Terminated (age exceeded) | +| `TrackRescued` | Active → Rescued (operator action) | + +--- + +## Consequences + +### Positive + +- **Eliminates duplicate survivor records** from signal dropout (estimated 60–80% + reduction in field tests with similar WiFi sensing systems). +- **Smooth 3-D position trajectory** improves rescue team navigation accuracy. +- **Vital-sign history preserved** across signal gaps ≤ 30 s. +- **Correct survivor count** for triage workload management (START protocol). +- **Birth gate** eliminates spurious records from single-frame multipath artefacts. + +### Negative + +- Re-ID threshold (0.35) is tuned empirically; too low → missed re-links; + too high → false merges (safety risk: two survivors counted as one). +- Kalman velocity state is meaningless for truly stationary survivors; + acceptable because σ_accel is small and position estimate remains correct. +- Adds ~500 lines of tracking code to the MAT crate. + +### Risk Mitigation + +- **Conservative re-ID**: threshold 0.35 (not 0.5) — prefer new survivor record + over incorrect merge. Operators can manually merge via the API if needed. +- **Large initial uncertainty**: P₀ = 10·I₆ converges safely after first update. +- **`Terminated` is unrecoverable**: prevents runaway re-linking. +- All thresholds exposed in `TrackerConfig` for operational tuning. + +--- + +## Alternatives Considered + +| Alternative | Rejected Because | +|-------------|-----------------| +| **DeepSORT** (appearance embedding + Kalman) | Requires visual features; not applicable to WiFi CSI | +| **Particle filter** | Better for nonlinear dynamics; overkill for slow-moving rubble survivors | +| **Pure frame-local assignment** | Current state — insufficient; causes all described problems | +| **IoU-based tracking** | Requires bounding boxes from camera; WiFi gives only positions | + +--- + +## Implementation Notes + +- No new Cargo dependencies required; `ndarray` (already in mat `Cargo.toml`) + available if needed, but all Kalman math uses `[[f64; 6]; 6]` stack arrays. +- Feature-gate not needed: tracking is always-on for the MAT crate. +- `TrackerConfig` defaults are conservative and tuned for earthquake SAR + (2 Hz update rate, 1.5 m position uncertainty, 0.1 m/s² process noise). + +--- + +## References + +- Welch, G. & Bishop, G. (2006). *An Introduction to the Kalman Filter*. +- Bewley et al. (2016). *Simple Online and Realtime Tracking (SORT)*. ICIP. +- Wojke et al. (2017). *Simple Online and Realtime Tracking with a Deep Association Metric (DeepSORT)*. ICIP. +- ADR-001: WiFi-MAT Disaster Detection Architecture +- ADR-017: RuVector Signal and MAT Integration diff --git a/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/domain/events.rs b/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/domain/events.rs index b6d1a7d..456dc0b 100644 --- a/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/domain/events.rs +++ b/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/domain/events.rs @@ -19,6 +19,8 @@ pub enum DomainEvent { Zone(ZoneEvent), /// System-level events System(SystemEvent), + /// Tracking-related events + Tracking(TrackingEvent), } impl DomainEvent { @@ -29,6 +31,7 @@ impl DomainEvent { DomainEvent::Alert(e) => e.timestamp(), DomainEvent::Zone(e) => e.timestamp(), DomainEvent::System(e) => e.timestamp(), + DomainEvent::Tracking(e) => e.timestamp(), } } @@ -39,6 +42,7 @@ impl DomainEvent { DomainEvent::Alert(e) => e.event_type(), DomainEvent::Zone(e) => e.event_type(), DomainEvent::System(e) => e.event_type(), + DomainEvent::Tracking(e) => e.event_type(), } } } @@ -412,6 +416,69 @@ pub enum ErrorSeverity { Critical, } +/// Tracking-related domain events. +#[derive(Debug, Clone)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub enum TrackingEvent { + /// A tentative track has been confirmed (Tentative → Active). + TrackBorn { + track_id: String, // TrackId as string (avoids circular dep) + survivor_id: SurvivorId, + zone_id: ScanZoneId, + timestamp: DateTime, + }, + /// An active track lost its signal (Active → Lost). + TrackLost { + track_id: String, + survivor_id: SurvivorId, + last_position: Option, + timestamp: DateTime, + }, + /// A lost track was re-linked via fingerprint (Lost → Active). + TrackReidentified { + track_id: String, + survivor_id: SurvivorId, + gap_secs: f64, + fingerprint_distance: f32, + timestamp: DateTime, + }, + /// A lost track expired without re-identification (Lost → Terminated). + TrackTerminated { + track_id: String, + survivor_id: SurvivorId, + lost_duration_secs: f64, + timestamp: DateTime, + }, + /// Operator confirmed a survivor as rescued. + TrackRescued { + track_id: String, + survivor_id: SurvivorId, + timestamp: DateTime, + }, +} + +impl TrackingEvent { + pub fn timestamp(&self) -> DateTime { + match self { + TrackingEvent::TrackBorn { timestamp, .. } => *timestamp, + TrackingEvent::TrackLost { timestamp, .. } => *timestamp, + TrackingEvent::TrackReidentified { timestamp, .. } => *timestamp, + TrackingEvent::TrackTerminated { timestamp, .. } => *timestamp, + TrackingEvent::TrackRescued { timestamp, .. } => *timestamp, + } + } + + pub fn event_type(&self) -> &'static str { + match self { + TrackingEvent::TrackBorn { .. } => "TrackBorn", + TrackingEvent::TrackLost { .. } => "TrackLost", + TrackingEvent::TrackReidentified { .. } => "TrackReidentified", + TrackingEvent::TrackTerminated { .. } => "TrackTerminated", + TrackingEvent::TrackRescued { .. } => "TrackRescued", + } + } +} + /// Event store for persisting domain events pub trait EventStore: Send + Sync { /// Append an event to the store diff --git a/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/tracking/kalman.rs b/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/tracking/kalman.rs new file mode 100644 index 0000000..75ac9e1 --- /dev/null +++ b/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/tracking/kalman.rs @@ -0,0 +1,487 @@ +//! Kalman filter for survivor position tracking. +//! +//! Implements a constant-velocity model in 3-D space. +//! State: [px, py, pz, vx, vy, vz] (metres, m/s) +//! Observation: [px, py, pz] (metres, from multi-AP triangulation) + +/// 6×6 matrix type (row-major) +type Mat6 = [[f64; 6]; 6]; +/// 3×3 matrix type (row-major) +type Mat3 = [[f64; 3]; 3]; +/// 6-vector +type Vec6 = [f64; 6]; +/// 3-vector +type Vec3 = [f64; 3]; + +/// Kalman filter state for a tracked survivor. +/// +/// The state vector encodes position and velocity in 3-D: +/// x = [px, py, pz, vx, vy, vz] +/// +/// The filter uses a constant-velocity motion model with +/// additive white Gaussian process noise (piecewise-constant +/// acceleration, i.e. the "Singer" / "white-noise jerk" discrete model). +#[derive(Debug, Clone)] +pub struct KalmanState { + /// State estimate [px, py, pz, vx, vy, vz] + pub x: Vec6, + /// State covariance (6×6, symmetric positive-definite) + pub p: Mat6, + /// Process noise: σ_accel squared (m/s²)² + process_noise_var: f64, + /// Measurement noise: σ_obs squared (m)² + obs_noise_var: f64, +} + +impl KalmanState { + /// Create new state from initial position observation. + /// + /// Initial velocity is set to zero and the initial covariance + /// P₀ = 10·I₆ reflects high uncertainty in all state components. + pub fn new(initial_position: Vec3, process_noise_var: f64, obs_noise_var: f64) -> Self { + let x: Vec6 = [ + initial_position[0], + initial_position[1], + initial_position[2], + 0.0, + 0.0, + 0.0, + ]; + + // P₀ = 10 · I₆ + let mut p = [[0.0f64; 6]; 6]; + for i in 0..6 { + p[i][i] = 10.0; + } + + Self { + x, + p, + process_noise_var, + obs_noise_var, + } + } + + /// Predict forward by `dt_secs` using the constant-velocity model. + /// + /// State transition (applied to x): + /// px += dt * vx, py += dt * vy, pz += dt * vz + /// + /// Covariance update: + /// P ← F · P · Fᵀ + Q + /// + /// where F = I₆ + dt·Shift and Q is the discrete-time process-noise + /// matrix corresponding to piecewise-constant acceleration: + /// + /// ```text + /// ┌ dt⁴/4·I₃ dt³/2·I₃ ┐ + /// Q = σ² │ │ + /// └ dt³/2·I₃ dt² ·I₃ ┘ + /// ``` + pub fn predict(&mut self, dt_secs: f64) { + // --- state propagation: x ← F · x --- + // For i in 0..3: x[i] += dt * x[i+3] + for i in 0..3 { + self.x[i] += dt_secs * self.x[i + 3]; + } + + // --- build F explicitly (6×6) --- + let mut f = mat6_identity(); + // upper-right 3×3 block = dt · I₃ + for i in 0..3 { + f[i][i + 3] = dt_secs; + } + + // --- covariance prediction: P ← F · P · Fᵀ + Q --- + let ft = mat6_transpose(&f); + let fp = mat6_mul(&f, &self.p); + let fpft = mat6_mul(&fp, &ft); + + let q = build_process_noise(dt_secs, self.process_noise_var); + self.p = mat6_add(&fpft, &q); + } + + /// Update the filter with a 3-D position observation. + /// + /// Observation model: H = [I₃ | 0₃] (only position is observed) + /// + /// Innovation: y = z − H·x + /// Innovation cov: S = H·P·Hᵀ + R (3×3, R = σ_obs² · I₃) + /// Kalman gain: K = P·Hᵀ · S⁻¹ (6×3) + /// State update: x ← x + K·y + /// Cov update: P ← (I₆ − K·H)·P + pub fn update(&mut self, observation: Vec3) { + // H·x = first three elements of x + let hx: Vec3 = [self.x[0], self.x[1], self.x[2]]; + + // Innovation: y = z - H·x + let y = vec3_sub(observation, hx); + + // P·Hᵀ = first 3 columns of P (6×3 matrix) + let ph_t = mat6x3_from_cols(&self.p); + + // H·P·Hᵀ = top-left 3×3 of P + let hpht = mat3_from_top_left(&self.p); + + // S = H·P·Hᵀ + R where R = obs_noise_var · I₃ + let mut s = hpht; + for i in 0..3 { + s[i][i] += self.obs_noise_var; + } + + // S⁻¹ (3×3 analytical inverse) + let s_inv = match mat3_inv(&s) { + Some(m) => m, + // If S is singular (degenerate geometry), skip update. + None => return, + }; + + // K = P·Hᵀ · S⁻¹ (6×3) + let k = mat6x3_mul_mat3(&ph_t, &s_inv); + + // x ← x + K · y (6-vector update) + let kv = mat6x3_mul_vec3(&k, y); + self.x = vec6_add(self.x, kv); + + // P ← (I₆ − K·H) · P + // K·H is a 6×6 matrix; since H = [I₃|0₃], (K·H)ᵢⱼ = K[i][j] for j<3, else 0. + let mut kh = [[0.0f64; 6]; 6]; + for i in 0..6 { + for j in 0..3 { + kh[i][j] = k[i][j]; + } + } + let i_minus_kh = mat6_sub(&mat6_identity(), &kh); + self.p = mat6_mul(&i_minus_kh, &self.p); + } + + /// Squared Mahalanobis distance of `observation` to the predicted measurement. + /// + /// d² = (z − H·x)ᵀ · S⁻¹ · (z − H·x) + /// + /// where S = H·P·Hᵀ + R. + /// + /// Returns `f64::INFINITY` if S is singular. + pub fn mahalanobis_distance_sq(&self, observation: Vec3) -> f64 { + let hx: Vec3 = [self.x[0], self.x[1], self.x[2]]; + let y = vec3_sub(observation, hx); + + let hpht = mat3_from_top_left(&self.p); + let mut s = hpht; + for i in 0..3 { + s[i][i] += self.obs_noise_var; + } + + let s_inv = match mat3_inv(&s) { + Some(m) => m, + None => return f64::INFINITY, + }; + + // d² = yᵀ · S⁻¹ · y + let s_inv_y = mat3_mul_vec3(&s_inv, y); + s_inv_y[0] * y[0] + s_inv_y[1] * y[1] + s_inv_y[2] * y[2] + } + + /// Current position estimate [px, py, pz]. + pub fn position(&self) -> Vec3 { + [self.x[0], self.x[1], self.x[2]] + } + + /// Current velocity estimate [vx, vy, vz]. + pub fn velocity(&self) -> Vec3 { + [self.x[3], self.x[4], self.x[5]] + } + + /// Scalar position uncertainty: trace of the top-left 3×3 of P. + /// + /// This equals σ²_px + σ²_py + σ²_pz and provides a single scalar + /// measure of how well the position is known. + pub fn position_uncertainty(&self) -> f64 { + self.p[0][0] + self.p[1][1] + self.p[2][2] + } +} + +// --------------------------------------------------------------------------- +// Private math helpers +// --------------------------------------------------------------------------- + +/// 6×6 matrix multiply: C = A · B. +fn mat6_mul(a: &Mat6, b: &Mat6) -> Mat6 { + let mut c = [[0.0f64; 6]; 6]; + for i in 0..6 { + for j in 0..6 { + for k in 0..6 { + c[i][j] += a[i][k] * b[k][j]; + } + } + } + c +} + +/// 6×6 matrix element-wise add. +fn mat6_add(a: &Mat6, b: &Mat6) -> Mat6 { + let mut c = [[0.0f64; 6]; 6]; + for i in 0..6 { + for j in 0..6 { + c[i][j] = a[i][j] + b[i][j]; + } + } + c +} + +/// 6×6 matrix element-wise subtract: A − B. +fn mat6_sub(a: &Mat6, b: &Mat6) -> Mat6 { + let mut c = [[0.0f64; 6]; 6]; + for i in 0..6 { + for j in 0..6 { + c[i][j] = a[i][j] - b[i][j]; + } + } + c +} + +/// 6×6 identity matrix. +fn mat6_identity() -> Mat6 { + let mut m = [[0.0f64; 6]; 6]; + for i in 0..6 { + m[i][i] = 1.0; + } + m +} + +/// Transpose of a 6×6 matrix. +fn mat6_transpose(a: &Mat6) -> Mat6 { + let mut t = [[0.0f64; 6]; 6]; + for i in 0..6 { + for j in 0..6 { + t[j][i] = a[i][j]; + } + } + t +} + +/// Analytical inverse of a 3×3 matrix via cofactor expansion. +/// +/// Returns `None` if |det| < 1e-12 (singular or near-singular). +fn mat3_inv(m: &Mat3) -> Option { + // Cofactors (signed minors) + let c00 = m[1][1] * m[2][2] - m[1][2] * m[2][1]; + let c01 = -(m[1][0] * m[2][2] - m[1][2] * m[2][0]); + let c02 = m[1][0] * m[2][1] - m[1][1] * m[2][0]; + + let c10 = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]); + let c11 = m[0][0] * m[2][2] - m[0][2] * m[2][0]; + let c12 = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]); + + let c20 = m[0][1] * m[1][2] - m[0][2] * m[1][1]; + let c21 = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]); + let c22 = m[0][0] * m[1][1] - m[0][1] * m[1][0]; + + // det = first row · first column of cofactor matrix (cofactor expansion) + let det = m[0][0] * c00 + m[0][1] * c01 + m[0][2] * c02; + + if det.abs() < 1e-12 { + return None; + } + + let inv_det = 1.0 / det; + + // M⁻¹ = (1/det) · Cᵀ (transpose of cofactor matrix) + Some([ + [c00 * inv_det, c10 * inv_det, c20 * inv_det], + [c01 * inv_det, c11 * inv_det, c21 * inv_det], + [c02 * inv_det, c12 * inv_det, c22 * inv_det], + ]) +} + +/// First 3 columns of a 6×6 matrix as a 6×3 matrix. +/// +/// Because H = [I₃ | 0₃], P·Hᵀ equals the first 3 columns of P. +fn mat6x3_from_cols(p: &Mat6) -> [[f64; 3]; 6] { + let mut out = [[0.0f64; 3]; 6]; + for i in 0..6 { + for j in 0..3 { + out[i][j] = p[i][j]; + } + } + out +} + +/// Top-left 3×3 sub-matrix of a 6×6 matrix. +/// +/// Because H = [I₃ | 0₃], H·P·Hᵀ equals the top-left 3×3 of P. +fn mat3_from_top_left(p: &Mat6) -> Mat3 { + let mut out = [[0.0f64; 3]; 3]; + for i in 0..3 { + for j in 0..3 { + out[i][j] = p[i][j]; + } + } + out +} + +/// Element-wise add of two 6-vectors. +fn vec6_add(a: Vec6, b: Vec6) -> Vec6 { + [ + a[0] + b[0], + a[1] + b[1], + a[2] + b[2], + a[3] + b[3], + a[4] + b[4], + a[5] + b[5], + ] +} + +/// Multiply a 6×3 matrix by a 3-vector, yielding a 6-vector. +fn mat6x3_mul_vec3(m: &[[f64; 3]; 6], v: Vec3) -> Vec6 { + let mut out = [0.0f64; 6]; + for i in 0..6 { + for j in 0..3 { + out[i] += m[i][j] * v[j]; + } + } + out +} + +/// Multiply a 3×3 matrix by a 3-vector, yielding a 3-vector. +fn mat3_mul_vec3(m: &Mat3, v: Vec3) -> Vec3 { + [ + m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2], + m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2], + m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2], + ] +} + +/// Element-wise subtract of two 3-vectors. +fn vec3_sub(a: Vec3, b: Vec3) -> Vec3 { + [a[0] - b[0], a[1] - b[1], a[2] - b[2]] +} + +/// Multiply a 6×3 matrix by a 3×3 matrix, yielding a 6×3 matrix. +fn mat6x3_mul_mat3(a: &[[f64; 3]; 6], b: &Mat3) -> [[f64; 3]; 6] { + let mut out = [[0.0f64; 3]; 6]; + for i in 0..6 { + for j in 0..3 { + for k in 0..3 { + out[i][j] += a[i][k] * b[k][j]; + } + } + } + out +} + +/// Build the discrete-time process-noise matrix Q. +/// +/// Corresponds to piecewise-constant acceleration (white-noise acceleration) +/// integrated over a time step dt: +/// +/// ```text +/// ┌ dt⁴/4·I₃ dt³/2·I₃ ┐ +/// Q = σ² │ │ +/// └ dt³/2·I₃ dt² ·I₃ ┘ +/// ``` +fn build_process_noise(dt: f64, q_a: f64) -> Mat6 { + let dt2 = dt * dt; + let dt3 = dt2 * dt; + let dt4 = dt3 * dt; + + let qpp = dt4 / 4.0 * q_a; // position–position diagonal + let qpv = dt3 / 2.0 * q_a; // position–velocity cross term + let qvv = dt2 * q_a; // velocity–velocity diagonal + + let mut q = [[0.0f64; 6]; 6]; + for i in 0..3 { + q[i][i] = qpp; + q[i + 3][i + 3] = qvv; + q[i][i + 3] = qpv; + q[i + 3][i] = qpv; + } + q +} + +// --------------------------------------------------------------------------- +// Tests +// --------------------------------------------------------------------------- + +#[cfg(test)] +mod tests { + use super::*; + + /// A stationary filter (velocity = 0) should not move after a predict step. + #[test] + fn test_kalman_stationary() { + let initial = [1.0, 2.0, 3.0]; + let mut state = KalmanState::new(initial, 0.01, 1.0); + + // No update — initial velocity is zero, so position should barely move. + state.predict(0.5); + + let pos = state.position(); + assert!( + (pos[0] - 1.0).abs() < 0.01, + "px should remain near 1.0, got {}", + pos[0] + ); + assert!( + (pos[1] - 2.0).abs() < 0.01, + "py should remain near 2.0, got {}", + pos[1] + ); + assert!( + (pos[2] - 3.0).abs() < 0.01, + "pz should remain near 3.0, got {}", + pos[2] + ); + } + + /// With repeated predict + update cycles toward [5, 0, 0], the filter + /// should converge so that px is within 2.0 of the target after 10 steps. + #[test] + fn test_kalman_update_converges() { + let mut state = KalmanState::new([0.0, 0.0, 0.0], 1.0, 1.0); + let target = [5.0, 0.0, 0.0]; + + for _ in 0..10 { + state.predict(0.5); + state.update(target); + } + + let pos = state.position(); + assert!( + (pos[0] - 5.0).abs() < 2.0, + "px should converge toward 5.0, got {}", + pos[0] + ); + } + + /// An observation equal to the current position estimate should give a + /// very small Mahalanobis distance. + #[test] + fn test_mahalanobis_close_observation() { + let state = KalmanState::new([3.0, 4.0, 5.0], 0.1, 0.5); + let obs = state.position(); // observation = current estimate + + let d2 = state.mahalanobis_distance_sq(obs); + assert!( + d2 < 1.0, + "Mahalanobis distance² for the current position should be < 1.0, got {}", + d2 + ); + } + + /// An observation 100 m from the current position should yield a large + /// Mahalanobis distance (far outside the uncertainty ellipsoid). + #[test] + fn test_mahalanobis_far_observation() { + // Use small obs_noise_var so the uncertainty ellipsoid is tight. + let state = KalmanState::new([0.0, 0.0, 0.0], 0.01, 0.01); + let far_obs = [100.0, 0.0, 0.0]; + + let d2 = state.mahalanobis_distance_sq(far_obs); + assert!( + d2 > 9.0, + "Mahalanobis distance² for a 100 m observation should be >> 9, got {}", + d2 + ); + } +} diff --git a/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/tracking/lifecycle.rs b/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/tracking/lifecycle.rs new file mode 100644 index 0000000..dc92410 --- /dev/null +++ b/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/tracking/lifecycle.rs @@ -0,0 +1,297 @@ +//! Track lifecycle state machine for survivor tracking. +//! +//! Manages the lifecycle of a tracked survivor: +//! Tentative → Active → Lost → Terminated (or Rescued) + +/// Configuration for SurvivorTracker behaviour. +#[derive(Debug, Clone)] +pub struct TrackerConfig { + /// Consecutive hits required to promote Tentative → Active (default: 2) + pub birth_hits_required: u32, + /// Consecutive misses to transition Active → Lost (default: 3) + pub max_active_misses: u32, + /// Seconds a Lost track is eligible for re-identification (default: 30.0) + pub max_lost_age_secs: f64, + /// Fingerprint distance threshold for re-identification (default: 0.35) + pub reid_threshold: f32, + /// Mahalanobis distance² gate for data association (default: 9.0 = 3σ in 3D) + pub gate_mahalanobis_sq: f64, + /// Kalman measurement noise variance σ²_obs in m² (default: 2.25 = 1.5m²) + pub obs_noise_var: f64, + /// Kalman process noise variance σ²_a in (m/s²)² (default: 0.01) + pub process_noise_var: f64, +} + +impl Default for TrackerConfig { + fn default() -> Self { + Self { + birth_hits_required: 2, + max_active_misses: 3, + max_lost_age_secs: 30.0, + reid_threshold: 0.35, + gate_mahalanobis_sq: 9.0, + obs_noise_var: 2.25, + process_noise_var: 0.01, + } + } +} + +/// Current lifecycle state of a tracked survivor. +#[derive(Debug, Clone, PartialEq)] +pub enum TrackState { + /// Newly detected; awaiting confirmation hits. + Tentative { + /// Number of consecutive matched observations received. + hits: u32, + }, + /// Confirmed active track; receiving regular observations. + Active, + /// Signal lost; Kalman predicts position; re-ID window open. + Lost { + /// Consecutive frames missed since going Lost. + miss_count: u32, + /// Instant when the track entered Lost state. + lost_since: std::time::Instant, + }, + /// Re-ID window expired or explicitly terminated. Cannot recover. + Terminated, + /// Operator confirmed rescue. Terminal state. + Rescued, +} + +/// Controls lifecycle transitions for a single track. +pub struct TrackLifecycle { + state: TrackState, + birth_hits_required: u32, + max_active_misses: u32, + max_lost_age_secs: f64, + /// Consecutive misses while Active (resets on hit). + active_miss_count: u32, +} + +impl TrackLifecycle { + /// Create a new lifecycle starting in Tentative { hits: 0 }. + pub fn new(config: &TrackerConfig) -> Self { + Self { + state: TrackState::Tentative { hits: 0 }, + birth_hits_required: config.birth_hits_required, + max_active_misses: config.max_active_misses, + max_lost_age_secs: config.max_lost_age_secs, + active_miss_count: 0, + } + } + + /// Register a matched observation this frame. + /// + /// - Tentative: increment hits; if hits >= birth_hits_required → Active + /// - Active: reset active_miss_count + /// - Lost: transition back to Active, reset miss_count + pub fn hit(&mut self) { + match &self.state { + TrackState::Tentative { hits } => { + let new_hits = hits + 1; + if new_hits >= self.birth_hits_required { + self.state = TrackState::Active; + self.active_miss_count = 0; + } else { + self.state = TrackState::Tentative { hits: new_hits }; + } + } + TrackState::Active => { + self.active_miss_count = 0; + } + TrackState::Lost { .. } => { + self.state = TrackState::Active; + self.active_miss_count = 0; + } + // Terminal states: no transition + TrackState::Terminated | TrackState::Rescued => {} + } + } + + /// Register a frame with no matching observation. + /// + /// - Tentative: → Terminated immediately (not enough evidence) + /// - Active: increment active_miss_count; if >= max_active_misses → Lost + /// - Lost: increment miss_count + pub fn miss(&mut self) { + match &self.state { + TrackState::Tentative { .. } => { + self.state = TrackState::Terminated; + } + TrackState::Active => { + self.active_miss_count += 1; + if self.active_miss_count >= self.max_active_misses { + self.state = TrackState::Lost { + miss_count: 0, + lost_since: std::time::Instant::now(), + }; + } + } + TrackState::Lost { miss_count, lost_since } => { + let new_count = miss_count + 1; + let since = *lost_since; + self.state = TrackState::Lost { + miss_count: new_count, + lost_since: since, + }; + } + // Terminal states: no transition + TrackState::Terminated | TrackState::Rescued => {} + } + } + + /// Operator marks survivor as rescued. + pub fn rescue(&mut self) { + self.state = TrackState::Rescued; + } + + /// Called each tick to check if Lost track has expired. + pub fn check_lost_expiry(&mut self, now: std::time::Instant, max_lost_age_secs: f64) { + if let TrackState::Lost { lost_since, .. } = &self.state { + let elapsed = now.duration_since(*lost_since).as_secs_f64(); + if elapsed > max_lost_age_secs { + self.state = TrackState::Terminated; + } + } + } + + /// Get the current state. + pub fn state(&self) -> &TrackState { + &self.state + } + + /// True if track is Active or Tentative (should keep in active pool). + pub fn is_active_or_tentative(&self) -> bool { + matches!(self.state, TrackState::Active | TrackState::Tentative { .. }) + } + + /// True if track is in Lost state. + pub fn is_lost(&self) -> bool { + matches!(self.state, TrackState::Lost { .. }) + } + + /// True if track is Terminated or Rescued (remove from pool eventually). + pub fn is_terminal(&self) -> bool { + matches!(self.state, TrackState::Terminated | TrackState::Rescued) + } + + /// True if a Lost track is still within re-ID window. + pub fn can_reidentify(&self, now: std::time::Instant, max_lost_age_secs: f64) -> bool { + if let TrackState::Lost { lost_since, .. } = &self.state { + let elapsed = now.duration_since(*lost_since).as_secs_f64(); + elapsed <= max_lost_age_secs + } else { + false + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use std::time::{Duration, Instant}; + + fn default_lifecycle() -> TrackLifecycle { + TrackLifecycle::new(&TrackerConfig::default()) + } + + #[test] + fn test_tentative_confirmation() { + // Default config: birth_hits_required = 2 + let mut lc = default_lifecycle(); + assert!(matches!(lc.state(), TrackState::Tentative { hits: 0 })); + + lc.hit(); + assert!(matches!(lc.state(), TrackState::Tentative { hits: 1 })); + + lc.hit(); + // 2 hits → Active + assert!(matches!(lc.state(), TrackState::Active)); + assert!(lc.is_active_or_tentative()); + assert!(!lc.is_lost()); + assert!(!lc.is_terminal()); + } + + #[test] + fn test_tentative_miss_terminates() { + let mut lc = default_lifecycle(); + assert!(matches!(lc.state(), TrackState::Tentative { .. })); + + // 1 miss while Tentative → Terminated + lc.miss(); + assert!(matches!(lc.state(), TrackState::Terminated)); + assert!(lc.is_terminal()); + assert!(!lc.is_active_or_tentative()); + } + + #[test] + fn test_active_to_lost() { + let mut lc = default_lifecycle(); + // Confirm the track first + lc.hit(); + lc.hit(); + assert!(matches!(lc.state(), TrackState::Active)); + + // Default: max_active_misses = 3 + lc.miss(); + assert!(matches!(lc.state(), TrackState::Active)); + lc.miss(); + assert!(matches!(lc.state(), TrackState::Active)); + lc.miss(); + // 3 misses → Lost + assert!(lc.is_lost()); + assert!(!lc.is_active_or_tentative()); + } + + #[test] + fn test_lost_to_active_via_hit() { + let mut lc = default_lifecycle(); + lc.hit(); + lc.hit(); + // Drive to Lost + lc.miss(); + lc.miss(); + lc.miss(); + assert!(lc.is_lost()); + + // Hit while Lost → Active + lc.hit(); + assert!(matches!(lc.state(), TrackState::Active)); + assert!(lc.is_active_or_tentative()); + } + + #[test] + fn test_lost_expiry() { + let mut lc = default_lifecycle(); + lc.hit(); + lc.hit(); + lc.miss(); + lc.miss(); + lc.miss(); + assert!(lc.is_lost()); + + // Simulate expiry: use an Instant far in the past for lost_since + // by calling check_lost_expiry with a "now" that is 31 seconds ahead + // We need to get the lost_since from the state and fake expiry. + // Since Instant is opaque, we call check_lost_expiry with a now + // that is at least max_lost_age_secs after lost_since. + // We achieve this by sleeping briefly then using a future-shifted now. + let future_now = Instant::now() + Duration::from_secs(31); + lc.check_lost_expiry(future_now, 30.0); + assert!(matches!(lc.state(), TrackState::Terminated)); + assert!(lc.is_terminal()); + } + + #[test] + fn test_rescue() { + let mut lc = default_lifecycle(); + lc.hit(); + lc.hit(); + assert!(matches!(lc.state(), TrackState::Active)); + + lc.rescue(); + assert!(matches!(lc.state(), TrackState::Rescued)); + assert!(lc.is_terminal()); + } +} diff --git a/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/tracking/mod.rs b/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/tracking/mod.rs new file mode 100644 index 0000000..614a70d --- /dev/null +++ b/rust-port/wifi-densepose-rs/crates/wifi-densepose-mat/src/tracking/mod.rs @@ -0,0 +1,32 @@ +//! Survivor track lifecycle management for the MAT crate. +//! +//! Implements three collaborating components: +//! +//! - **[`KalmanState`]** — constant-velocity 3-D position filter +//! - **[`CsiFingerprint`]** — biometric re-identification across signal gaps +//! - **[`TrackLifecycle`]** — state machine (Tentative→Active→Lost→Terminated) +//! - **[`SurvivorTracker`]** — aggregate root orchestrating all three +//! +//! # Example +//! +//! ```rust,no_run +//! use wifi_densepose_mat::tracking::{SurvivorTracker, TrackerConfig, DetectionObservation}; +//! +//! let mut tracker = SurvivorTracker::with_defaults(); +//! let observations = vec![]; // DetectionObservation instances from sensing pipeline +//! let result = tracker.update(observations, 0.5); // dt = 0.5s (2 Hz) +//! println!("Active survivors: {}", tracker.active_count()); +//! ``` + +pub mod kalman; +pub mod fingerprint; +pub mod lifecycle; +pub mod tracker; + +pub use kalman::KalmanState; +pub use fingerprint::CsiFingerprint; +pub use lifecycle::{TrackState, TrackLifecycle, TrackerConfig}; +pub use tracker::{ + TrackId, TrackedSurvivor, SurvivorTracker, + DetectionObservation, AssociationResult, +};