Optical Flow on MCU: Lucas-Kanade for UAV Stabilization

Optical flow gives a UAV flight controller a way to estimate ego-motion and hover stability without GPS — critical for indoor drones, low-altitude obstacle avoidance, and GPS-denied navigation. Running it on an MCU instead of an SoC means working within tight memory and cycle budgets: no OpenCV, no floating-point pyramids for free, and often no hardware floating-point unit at all. The Lucas-Kanade (LK) method survives in this environment because it reduces to a small, well-conditioned linear solve per tracked point rather than a dense per-pixel search — but getting it to run at 100+ Hz on a Cortex-M requires understanding exactly where the cost lives and where accuracy can be traded away.

Why Optical Flow for UAV Stabilization

A downward-facing camera observes the ground plane. Between two frames captured Δt apart, the apparent pixel displacement (u, v) of ground features is proportional to the drone's lateral velocity divided by height above ground:

u ≈ (f/Z)·Vx·Δt      v ≈ (f/Z)·Vy·Δt

where f is focal length in pixels and Z is height (from a rangefinder or barometer). Integrating this flow gives a velocity estimate that feeds the same control loop slot a GPS velocity fix would occupy — but it updates at camera frame rate (60–200 Hz) with much lower latency, which is exactly what a stabilization inner loop needs.

The Lucas-Kanade Model

LK assumes brightness constancy: a pixel's intensity doesn't change as it moves, only its position does.

I(x, y, t) = I(x + dx, y + dy, t + dt)

A first-order Taylor expansion gives the optical flow constraint equation:

Ix·u + Iy·v + It = 0

This is one equation with two unknowns (u, v) — underdetermined for a single pixel (the classic "aperture problem"). LK's contribution is to assume flow is constant over a small window (typically 5×5 to 15×15 pixels) and solve the resulting overdetermined system by least squares:

[Ix₁ Iy₁]           [-It₁]
[Ix₂ Iy₂] [u]   =    [-It₂]
[ ...    ] [v]        [ ... ]
[Ixₙ Iyₙ]           [-Itₙ]

Written as A·d = b, the least-squares solution is d = (AᵀA)⁻¹·Aᵀb. Expanding AᵀA for an n-pixel window:

AᵀA = | ΣIx²   ΣIxIy |     G, the structure tensor
      | ΣIxIy  ΣIy²  |

Aᵀb = | -ΣIxIt |
      | -ΣIyIt |

Solving the 2×2 system in closed form (no matrix library needed):

det = ΣIx²·ΣIy² − (ΣIxIy)²
u = (ΣIy²·(−ΣIxIt) − ΣIxIy·(−ΣIyIt)) / det
v = (ΣIx²·(−ΣIyIt) − ΣIxIy·(−ΣIxIt)) / det

This is the entire per-point computation: accumulate five sums (Ix², Iy², IxIy, IxIt, IyIt) over the window, then a handful of multiplies and one divide. No iteration is required for small motion; for larger motion, 2–4 Newton-Raphson refinement iterations (recomputing It at the warped position each time) converge quickly.

Conditioning: Why the Corner Matters

The 2×2 matrix G is singular or near-singular on flat or edge-only regions — this is the aperture problem again. Its eigenvalues λ₁ ≥ λ₂ indicate texture quality:

This is exactly the Shi-Tomasi "good features to track" criterion: select points where min(λ₁, λ₂) exceeds a threshold. On an MCU, computing eigenvalues explicitly is avoidable — det(G) and trace(G) are cheap proxies (det large relative to trace² ⇒ well-conditioned corner), so a quality gate can be applied using only the sums already computed for the flow solve.

Handling Large Motion: Pyramidal LK

Basic LK linearizes around zero motion and only converges for displacements of a pixel or two per iteration. UAV rotation or a low frame rate can easily produce 10–20 px of motion. The standard fix is a Gaussian pyramid: compute flow at the coarsest (smallest) level first, then use that estimate to initialize tracking at the next finer level, refining down to full resolution.

• 3-level pyramid (e.g., 80×60, 40×30, 20×15 from a 160×120 QQVGA base) extends the effective trackable displacement from ~2 px to ~16 px at full resolution.
• Cost: each pyramid level adds a downsample pass (cheap, box filter or 2×2 average) and repeats the per-point LK solve — roughly 1.3–1.5× the single-level cost for 3 levels when only a sparse point set is tracked, since downsampling the full image dominates over the point-wise solves.

For pure hover-stabilization (small inter-frame motion at 100+ Hz), a single-level LK is often sufficient and pyramids can be skipped entirely — a key simplification for MCU budgets.

MCU Implementation Budget

Assumptions: Cortex-M7 @ 400 MHz, QQVGA (160×120) grayscale frames, 40 tracked points, single-level LK, 7×7 windows (49 px/point).

At 400 MHz that's ≈100 µs — comfortably inside a 10 ms (100 Hz) frame period, leaving >99% of the budget for image capture DMA, attitude filtering, and the rest of the flight control loop. The dominant cost is gradient computation over the search windows, not the linear algebra — so restricting gradients to a sparse point set (rather than computing a dense gradient image) is the single biggest MCU-specific optimization versus a desktop OpenCV implementation.

Fixed-point note: intensities and gradients fit in int16 (Sobel output ≤ ±1020 for 8-bit input); the sums ΣIx² etc. need int32 accumulators to avoid overflow over a 49-sample window (49 × 1020² ≈ 5×10⁷, well within int32 range). The final divide can use a single Newton-Raphson reciprocal iteration if the MCU lacks a hardware divider, avoiding a full floating-point unit dependency.

Worked Example: Single Point, 5×5 Window

Suppose a 5×5 window (n = 25 px) around a tracked corner yields these accumulated sums (arbitrary but self-consistent intensity-gradient units):

ΣIx²  = 8400      ΣIy²  = 7600
ΣIxIy = 1200       ΣIxIt = -3100    ΣIyIt = -2600

Compute:

det = 8400·7600 − 1200² = 63,840,000 − 1,440,000 = 62,400,000
u = (7600·3100 − 1200·2600) / 62,400,000
  = (23,560,000 − 3,120,000) / 62,400,000 = 20,440,000 / 62,400,000 ≈ 0.328 px
v = (8400·2600 − 1200·3100) / 62,400,000
  = (21,840,000 − 3,720,000) / 62,400,000 = 18,120,000 / 62,400,000 ≈ 0.290 px

Check: substitute back into the original constraint for a representative pixel; with Ix ≈ 40, Iy ≈ 38, It ≈ -30 (typical magnitudes for these sums over 25 px), Ix·u + Iy·v + It ≈ 40(0.328) + 38(0.290) − 30 ≈ 13.1 + 11.0 − 30 ≈ −5.9, i.e. a residual much smaller than It itself (~20% of |It|) — consistent with least-squares minimizing but not zeroing residual across a noisy window. The det value (6.24×10⁷) is large relative to trace² (16,000² = 2.56×10⁸, ratio ≈0.24), indicating an adequately conditioned corner, not a degenerate edge.

Design Implications

• Point selection dominates robustness. Running the Shi-Tomasi/Harris-style corner gate before LK avoids wasting cycles on untrackable flat regions and prevents unstable flow estimates from corrupting the velocity fusion filter.
• Outlier rejection is mandatory. Even well-conditioned points can mistrack due to specular reflection or moving shadows; a RANSAC-lite consensus (median flow, or discarding points >2σ from the median) before feeding a Kalman filter is standard practice.
• Height dependency. Since flow scales as 1/Z, a rangefinder or barometer-derived Z estimate is required to convert pixel flow into metric velocity — optical flow alone gives velocity/height, not velocity.
• Rolling shutter and motion blur on cheap MCU-attached camera modules distort the brightness-constancy assumption at high angular rates; global-shutter sensors are strongly preferred for this application.
• Exposure/gain control must be fast and stable — auto-exposure hunting changes global brightness between frames, violating brightness constancy and injecting bias into It.

Key Takeaways

• Lucas-Kanade reduces optical flow to a per-point 2×2 linear solve from five accumulated gradient sums — cheap enough for real-time MCU execution without a dedicated NPU.
• The structure tensor's conditioning (det vs. trace²) doubles as the corner-quality gate, reusing sums already computed for the flow itself.
• Pyramidal refinement extends trackable displacement range but can be skipped for high-frame-rate hover stabilization where inter-frame motion stays small.
• On a Cortex-M7, sparse LK over ~40 points at QQVGA resolution costs roughly 41k cycles/frame (~102 µs at 400 MHz) — dominated by gradient computation, not the linear algebra.
• Flow gives velocity/height, not velocity directly; fusing it with a height sensor and rejecting outlier points before Kalman fusion is essential for stable UAV control.