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) The Kalman filter, gently

The EMA in §5 smooths with a fixed trust knob \(\alpha\) and quietly ignores that we already have a motion model (§6). A Kalman filter fixes both: it makes the blend adaptive instead of guessed, and it folds the constant-velocity model straight into the smoothing. The name sounds intimidating, but the whole thing is a two-step loop you repeat once per frame.

Predict, then correct

• Predict. Take your last best estimate and coast it forward with the constant-velocity model: "if it was there moving like that, it should be here now." At the same time, grow an uncertainty — the model isn't perfect, so the longer you coast on it without a fresh look, the less sure you are.
• Correct. A new measurement arrives. Blend it with the prediction — but not 50/50. Weight each side by how much you trust it. The blended estimate lands between the two, and its uncertainty shrinks, because you just learned something.

In one dimension the correction step is a single line that should look familiar:

\[ \hat{x} = \hat{x}^- + K\,(\,z - \hat{x}^-\,), \qquad K = \frac{P^-}{P^- + R} \]

\(\hat{x}^-\) is the prediction, \(z\) is the new measurement, \(P^-\) is how unsure the prediction is, and \(R\) is how noisy the sensor is. That weight \(K\) — the Kalman gain — is just \(\alpha\) from the EMA, except instead of guessing it once you compute it every frame from the two uncertainties:

• Noisy sensor (\(R\) large) \(\Rightarrow\) \(K\) small \(\Rightarrow\) lean on the prediction, smooth hard.
• Sharp, confident sensor (\(R\) small) \(\Rightarrow\) \(K\) near \(1\) \(\Rightarrow\) snap to the measurement.

Two things fall out for free. First, because the predict step anticipates motion, a Kalman filter follows a moving target with far less lag than an EMA at the same smoothness. Second, the running uncertainty \(P\) is the confidence estimate — an honest error bar on every fix (the §7 error budget, computed live). You can draw it as a halo around the blip, or use it to reject phantom fixes: a measurement that lands many \(\sqrt{P}\) away from the prediction is almost certainly the two sensors locking onto different objects, and you can drop it before it ever reaches the screen.

The honest caveat: at a 10 cm baseline the bearing is already noise-limited (§7), so a full Kalman filter is more machinery than this rig strictly needs — but it's exactly the right tool the moment you widen the baseline or add a third sensor (both below).

10) Where to go from here

• Kalman filter: the principled, self-tuning upgrade to the EMA — walked through in §9 above.
• 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