Context

Wally (wall-mounted system) tracks a guide rail using RPLidar C1 for positioning.

Problem: Low-cost lidar produces noisy distance/angle measurements—raw data unusable for precise control.

Goal: Extract stable guide rail position and angle from noisy lidar scans.

---

Core Problem

Single-stage filtering fails because noise has multiple characteristics:

1. Spatial outliers: Random spikes from reflections, dust, occlusion
2. Temporal jitter: Frame-to-frame angle/distance fluctuation
3. Motion sensitivity: Need fast response when moving, smooth output when stationary

Key insight

Different noise types require different filters—combine them in a pipeline where each stage handles one noise characteristic.

---

Approach

1) Edge detection + segmentation

Separate guide rail from wall by range discontinuities:

• Sort points by X coordinate (lateral position)
• Detect edges where range jumps > 0.12m
• Group continuous points into segments
• Select closest segment as guide rail (guide is always nearer than wall)

---

2) Trimmed PCA for robust line fitting

RANSAC is non-deterministic (random sampling). Trimmed PCA provides consistent results:

1. First PCA: Fit line to all segment points
2. Calculate residuals: Perpendicular distance to line
3. Trim outliers: Remove top 20% by residual
4. Final PCA: Fit line on remaining inliers

Anisotropy check: Reject if eigenvalue ratio > 0.15 (points don’t form a line)

---

3) EWMA on normal vector

Smooth the line orientation before angle calculation:

• Apply EWMA (α = 0.15) to 2D normal vector
• Sign continuity: Flip vector if dot product with previous is negative

Why normal vector, not angle? EWMA on angles has wraparound issues at ±180°.

---

4) 1-Euro filter for adaptive smoothing

Final stage: adaptive low-pass on angle and distance.

Formula: cutoff = min_cutoff + beta × |derivative|

State	Derivative	Cutoff	Behavior
Stationary	Low	0.3 Hz	Strong smoothing
Moving	High	Higher	Fast response

Parameters: min_cutoff = 0.3 Hz, beta = 0.001

---

5) Debounce for validity

Prevent premature valid detection during startup:

• Initial detection: Require 2× debounce count
• Subsequent: Require 1× debounce count

Debounce count derived from EWMA settling time: N = ceil(log(0.1) / log(1 - α))

---

Tradeoffs & Limitations

• Latency: Multi-stage filtering adds delay (EWMA settling + debounce)
• Fixed trim ratio: 20% may over/under-trim depending on actual outlier proportion
• Parameter tuning: Each stage has parameters that need tuning for different environments

---

Results

• Stable output: Consistent angle/distance from noisy raw scans
• Deterministic: Same input → same output (no random sampling)
• Adaptive: Smooth when stationary, responsive when moving
• Robust: Handles spatial outliers via Trimmed PCA, temporal jitter via 1-Euro

---

Key Takeaway

Multi-stage filtering handles multi-characteristic noise:

• Spatial outliers → Edge detection + Trimmed PCA (20% trim)
• Orientation jitter → EWMA on normal vector (α = 0.15)
• Motion adaptation → 1-Euro filter (adaptive cutoff)
• Startup stability → Debounce with EWMA-derived count