🔬 Show & Tell 25.1 — SensorFusion Architecture

[web3guru888]

🔬 SensorFusion Architecture — Multi-Modal Perception Pipeline

Overview

Phase 25.1 introduces SensorFusion, the foundational perception layer for embodied cognition. Inspired by biological multisensory integration (Stein & Meredith 1993), SensorFusion merges heterogeneous sensor streams into a unified world state using an Extended Kalman Filter (EKF) backbone with multi-rate temporal alignment and Mahalanobis gating.

Data Flow

┌──────────────┐  ┌──────────────┐  ┌──────────────┐
│  Propriocep-  │  │   Visual     │  │  Tactile     │
│  tion Stream  │  │   Stream     │  │  Stream       │
│  (100 Hz)     │  │   (30 Hz)    │  │  (1000 Hz)   │
└──────┬───────┘  └──────┬───────┘  └──────┬───────┘
       │                 │                 │
       ▼                 ▼                 ▼
┌─────────────────────────────────────────────────┐
│           Temporal Alignment Layer               │
│  ┌────────────┐ ┌──────────┐ ┌───────────────┐  │
│  │ Interpolate│ │ Predict  │ │ Buffer+Resample│  │
│  │ (low→high) │ │ (EKF)    │ │ (high→target)  │  │
│  └────────────┘ └──────────┘ └───────────────┘  │
└─────────────────────┬───────────────────────────┘
                      ▼
┌─────────────────────────────────────────────────┐
│              KalmanSensorFusion                  │
│                                                  │
│  ┌──────────────────────────────────────────┐   │
│  │ PREDICT: x̂⁻ = F·x̂ + B·u               │   │
│  │          P⁻ = F·P·Fᵀ + Q               │   │
│  └──────────────────┬───────────────────────┘   │
│                     ▼                            │
│  ┌──────────────────────────────────────────┐   │
│  │ GATE: d² = (z-H·x̂⁻)ᵀ·S⁻¹·(z-H·x̂⁻)  │   │
│  │       reject if d² > χ²(α, dim)         │   │
│  └──────────────────┬───────────────────────┘   │
│                     ▼                            │
│  ┌──────────────────────────────────────────┐   │
│  │ UPDATE: K = P⁻·Hᵀ·S⁻¹                  │   │
│  │         x̂ = x̂⁻ + K·(z - H·x̂⁻)         │   │
│  │         P = (I - K·H)·P⁻                │   │
│  └──────────────────────────────────────────┘   │
└─────────────────────┬───────────────────────────┘
                      ▼
              ┌───────────────┐
              │  FusedState    │
              │  .position     │
              │  .velocity     │
              │  .covariance   │
              │  .confidence   │
              │  .timestamp    │
              └───────┬───────┘
                      │
          ┌───────────┼──────────────┐
          ▼           ▼              ▼
  AffordanceDet  SpatialReasoner  WorldModel
     (25.2)         (25.4)        (13.1)

EKF Predict/Update Equations

Step	Equation	Description
State predict	x̂⁻ₖ = F·x̂ₖ₋₁ + B·uₖ	Motion model forward propagation
Covariance predict	P⁻ₖ = F·Pₖ₋₁·Fᵀ + Q	Uncertainty growth
Innovation	ỹₖ = zₖ − H·x̂⁻ₖ	Measurement residual
Innovation covariance	Sₖ = H·P⁻ₖ·Hᵀ + R	Combined uncertainty
Kalman gain	Kₖ = P⁻ₖ·Hᵀ·Sₖ⁻¹	Optimal weighting
State update	x̂ₖ = x̂⁻ₖ + Kₖ·ỹₖ	Fused estimate
Covariance update	Pₖ = (I − Kₖ·H)·P⁻ₖ	Updated uncertainty

Multi-Rate Alignment

Different sensors report at different frequencies. The temporal alignment layer handles this via:

1. High→target downsampling: Tactile (1000 Hz) → buffer → resample to fusion rate (100 Hz)
2. Low→target interpolation: Visual (30 Hz) → linear/cubic interpolation to fusion rate
3. EKF prediction fill: Between measurements, pure EKF predict steps maintain state estimate

Mahalanobis Gating

Outlier rejection uses the Mahalanobis distance:

d²(z, x̂) = (z - H·x̂)ᵀ · S⁻¹ · (z - H·x̂)

Threshold is set from the χ² distribution at the desired confidence level (e.g., α=0.01 with dim degrees of freedom). Readings exceeding the threshold are rejected as spurious.

Prometheus Metrics

Metric	Type	Description
asi_sensor_fusion_fused_states_total	Counter	Total FusedState outputs produced
asi_sensor_fusion_rejected_readings_total	Counter	Readings rejected by Mahalanobis gating (label: modality)
asi_sensor_fusion_update_latency_seconds	Histogram	Time per EKF update cycle
asi_sensor_fusion_active_sensors	Gauge	Number of currently active sensor streams
asi_sensor_fusion_covariance_trace	Gauge	Trace of state covariance matrix (overall uncertainty)

PromQL examples:

# Rejection rate per modality
rate(asi_sensor_fusion_rejected_readings_total[5m])

# Uncertainty trend
asi_sensor_fusion_covariance_trace

# Sensor health
asi_sensor_fusion_active_sensors < 2

Integration Map

SensorFusion 25.1 ──→ AffordanceDetector 25.2 (FusedState → affordance detection)
                 ──→ SpatialReasoner 25.4 (FusedState → spatial map update)
                 ──→ WorldModel 13.1 (FusedState → belief update)
                 ──→ EmbodiedOrchestrator 25.5 (pipeline entry point)
TemporalGraph 17.1 ──→ SensorFusion (temporal indexing of sensor events)

Open Questions

1. Should we support UKF/particle filter as alternative backends behind the same Protocol?
2. How should sensor registration interact with the LiveModuleOrchestrator (Phase 15)?
3. What is the optimal fusion rate when all sensors have different native rates?

📌 Spec: #562 | Q&A: See companion discussion