Radar math — from two echoes to a moving target

1) The coordinate frame (get this wrong and nothing else matters)

Two sensors sit on a fixed baseline of length \(b\) (10 cm on the live rig), facing forward. We put the origin midway between them:

• Left sensor at \(S_L = (-b/2,\ 0)\)
• Right sensor at \(S_R = (+b/2,\ 0)\)
• \(y\) points forward, away from the rig — always positive for a valid target
• \(x\) points right along the baseline — negative is left of centre

2) From echo time to range

An HC-SR04 emits a 40 kHz burst and reports the time until the echo returns. Sound travels out and back, so:

\[ r = \frac{t_\text{echo} \cdot v_\text{sound}}{2}, \qquad v_\text{sound} \approx 343\ \text{m/s at } 20^\circ\text{C} \]

The popular Arduino shortcut distance_cm = duration_us / 58 is this same formula: \(343\ \text{m/s} = 0.0343\ \text{cm/µs}\), halved for the round trip gives \(\approx 1/58.3\ \text{cm per µs}\).

3) Trilateration: two circles → one point

Each range constrains the target to a circle around its sensor:

\[ r_1^2 = \left(x + \tfrac{b}{2}\right)^2 + y^2 \qquad\text{(left sensor)} \]

\[ r_2^2 = \left(x - \tfrac{b}{2}\right)^2 + y^2 \qquad\text{(right sensor)} \]

Subtract the second from the first. The \(x^2\), \(y^2\), and \(b^2/4\) terms all cancel, leaving something beautifully linear:

\[ r_1^2 - r_2^2 = 2bx \quad\Longrightarrow\quad \boxed{\,x = \frac{r_1^2 - r_2^2}{2b}\,} \]

Then substitute \(x\) back into either circle to get the forward distance:

\[ \boxed{\,y = \sqrt{r_1^2 - \left(x + \tfrac{b}{2}\right)^2}\,} \]

Before trusting the result, check it's geometrically possible. The circles only intersect when:

• \(|r_1 - r_2| \le b\) — otherwise one circle is inside the other (the sensors are seeing different objects, the most common failure)
• the expression under the square root is \(\ge 0\) (same condition, caught numerically)

4) Polar components: range and bearing

The display wants polar coordinates, which is one line each:

\[ R = \sqrt{x^2 + y^2} \qquad \theta = \operatorname{atan2}(x,\ y) \]

Always use atan2, never atan(x/y) — atan2 handles \(y \to 0\) and gets the sign of the quadrant right without special cases.

5) The velocity vector: differentiate the track, component by component

With a position fix \((x_k, y_k)\) at time \(t_k\) and the previous fix \((x_{k-1}, y_{k-1})\) at \(t_{k-1}\), the velocity components are finite differences:

\[ v_x = \frac{x_k - x_{k-1}}{t_k - t_{k-1}} \qquad v_y = \frac{y_k - y_{k-1}}{t_k - t_{k-1}} \]

From the components you recover the scalar quantities:

\[ \text{speed} = \sqrt{v_x^2 + v_y^2} \qquad \text{heading} = \operatorname{atan2}(v_x,\ v_y) \]

Heading uses the same swapped-argument convention as bearing: 0° = moving straight away from the rig, ±180° = straight at it. A useful derived signal is the radial speed (closing rate) — the component of velocity along the line of sight:

\[ v_\text{radial} = \frac{x\,v_x + y\,v_y}{R} \qquad \begin{cases} v_\text{radial} < 0 & \text{approaching} \\ v_\text{radial} > 0 & \text{receding} \end{cases} \]

That's the dot product \(\vec{v} \cdot \hat{r}\) — project the velocity vector onto the unit vector pointing at the target. It's what a Doppler radar would measure directly; we get it for free from the track.

Differentiation amplifies noise — always smooth

This is the step where naive implementations fall apart. If each position fix carries ±1 cm of noise and fixes arrive 60 ms apart, raw differencing turns that into ±0.33 m/s of velocity noise — easily bigger than the signal. Two standard fixes:

• Difference over a longer window: compare against the fix from ~0.5 s ago instead of the immediately previous one. Noise stays constant while the denominator grows.
• Exponential moving average (EMA): blend each raw estimate into a running value: \(\hat{v} \leftarrow \alpha\, v_\text{raw} + (1-\alpha)\,\hat{v}\) with \(\alpha \approx 0.2\text{–}0.4\). One line of code, dramatic improvement. (A Kalman filter is the principled version of this — overkill at this baseline, but the natural next step.)

6) Prediction: dead reckoning with a constant-velocity model

The hollow blue dot on the radar is the simplest possible motion model — assume the velocity vector stays constant and extrapolate:

\[ x_\text{pred} = x + v_x\,\Delta t \qquad y_\text{pred} = y + v_y\,\Delta t \qquad (\Delta t = 0.5\ \text{s on the live rig}) \]

Each component extrapolates independently — that's the payoff of working in Cartesian components rather than range/bearing. (Extrapolating \(R\) and \(\theta\) directly is wrong for any target not moving radially: a target crossing in front of you at constant velocity has wildly non-constant \(\dot\theta\).)

7) The error budget: why a 10 cm baseline gives coarse bearing

Propagate range noise through \(x = (r_1^2 - r_2^2)/2b\). For a target near centreline at range \(R\), a small error \(\delta r\) in one range produces:

\[ \delta x \approx \frac{r\,\delta r}{b} \approx \frac{R}{b}\,\delta r \]

The baseline is the lever arm, and it's working against you: at \(R = 2\) m with \(b = 0.1\) m, lateral error is 20× the range error. With ±3 mm of HC-SR04 jitter, that's ±6 cm of \(x\) wobble — about ±1.7° of bearing jitter at that range. Meanwhile \(y\) (forward range) stays nearly as accurate as the raw measurement.

• Bearing precision scales with \(b/R\) — double the baseline, halve the jitter. This is the same reason long-baseline interferometry uses continent-sized baselines.
• The forward cone is the sweet spot: off to the side, one sensor sees the target at a grazing angle or not at all, and the common-target assumption collapses.
• Both sensors must see the same object — the validity checks in §3 are the only defence against fusing two different echoes into one phantom fix.

8) The whole pipeline in code

Everything above, in the exact field names the live radar's bridge emits (radar.js consumes these verbatim):

// inputs: r1, r2 (metres), b = baseline (metres), dt since last fix (seconds)
function solveFix(r1, r2, b, prev, dt) {
  // --- validity: are these two echoes the same object? ---
  if (Math.abs(r1 - r2) > b) return null;        // circles don't intersect

  // --- trilateration (§3) ---
  const x = (r1 * r1 - r2 * r2) / (2 * b);
  const y2 = r1 * r1 - (x + b / 2) ** 2;
  if (y2 < 0) return null;                       // numerically impossible
  const y = Math.sqrt(y2);

  // --- polar components (§4) ---
  const range_m = Math.hypot(x, y);
  const bearing_deg = Math.atan2(x, y) * 180 / Math.PI;  // 0 deg = forward

  // --- velocity vector via finite differences + EMA smoothing (§5) ---
  let vx = 0, vy = 0;
  if (prev && dt > 1e-3) {
    const a = 0.3;                               // EMA factor
    vx = a * (x - prev.x) / dt + (1 - a) * prev.vx;
    vy = a * (y - prev.y) / dt + (1 - a) * prev.vy;
  }
  const speed_mps = Math.hypot(vx, vy);

  // --- constant-velocity prediction, 0.5 s ahead (§6) ---
  const pred_x_m = x + vx * 0.5;
  const pred_y_m = y + vy * 0.5;

  return { x_m: x, y_m: y, range_m, bearing_deg,
           vx_mps: vx, vy_mps: vy, speed_mps,
           pred_x_m, pred_y_m, valid: true };
}

Cross-check against the synthetic target in radar.js (demoFrame()): it runs this identical math in reverse — picks an \((x, y)\), then computes r1_cm = hypot(x + b/2, y) and r2_cm = hypot(x − b/2, y) so the demo's range arcs land exactly on the demo blip.

9) Where to go from here

• Kalman filter: replace the EMA with a constant-velocity Kalman filter — it weighs each measurement by its expected noise and gives you confidence estimates for free.
• Wider baseline: §7 says bearing jitter is \(\propto 1/b\). Even 30 cm transforms the fix quality.
• Three sensors: a third range over-determines the fix, letting you reject phantom fixes by residual instead of by triangle inequality alone.
• The phase relationships behind atan2 and rotating vectors are the same machinery as AC analysis — see the phasors chapter.

← back to the live radar · home