Skip to content
ELECTRONICS ENGINEERINGelectronics-engineering.com
Automotive

ACC 3: radar and camera fusion with the Kalman filter

Centralised and track-to-track fusion, relative motion model, Kalman gain proved, why two sensors beat one, gating, association, track management and a tuning mistake.

https://www.electronics-engineering.com/en/acc-radar-camera-fusion-kalman-filter-association-tracks/

No ratings yet: be the first to rate this page.

Radar and camera send, each at its own rate, lists of measurements that are noisy, sometimes missing, sometimes false. Fusion turns them into a list of tracked objects: for each one an estimate of position and speed with its uncertainty, updated every cycle. The tool is the Kalman filter, which we introduced in the lesson on the Kalman filter and met again among the discrete observers; here we apply it to several sensors, add the association of measurements to objects and track management, and look at a tuning mistake that makes fusion worse than a single sensor.

Two architectures

Centralised fusion and track-to-track fusion
A: centralised fusion, every radar and camera measurement updates the same filter. B: each sensor has its own tracker and the tracks are fused afterwards (T2T), with covariance intersection (CI) because the errors of the two tracks are not independent.
  • centralised (A): the most precise, because the filter uses every measurement with its true uncertainty; it needs raw or nearly raw, time-stamped measurements;
  • track-to-track (B): each sensor ECU already does its own tracking (common with sensors from different suppliers) and the ADAS ECU fuses the tracks. Two tracks of the same object have correlated errors (they share the same motion model), and treating them as independent would make the estimate overconfident. Covariance intersection is safe even without knowing the correlation: P^{-1} = \omega P_1^{-1} + (1-\omega) P_2^{-1}, \hat x = P\,(\omega P_1^{-1}\hat x_1 + (1-\omega)P_2^{-1}\hat x_2), with \omega \in [0,1] chosen to minimise the trace of P.

In the code we use centralised fusion.

The model of relative motion

The state of each object is relative to our car: \mathbf{s} = [x,\ v_x,\ y,\ v_y]^T. Longitudinally the relative acceleration is \ddot x = a_t - a_{ego}: the target’s a_t is unknown and we treat it as noise, our own a_{ego} is known and we use it as an input. Integrating over a step T with constant acceleration (see the kinematics of linear motion):

    \[ x_{k+1} = x_k + v_{x,k} T - \tfrac12 a_{ego} T^2 + \tfrac12 w_x T^2, \qquad v_{x,k+1} = v_{x,k} - a_{ego} T + w_x T, \]

and the same laterally, without input. In matrix form \mathbf{s}_{k+1} = F\mathbf{s}_k + B u + G\mathbf{w} with

    \[ F = \begin{bmatrix}1&T&0&0\\0&1&0&0\\0&0&1&T\\0&0&0&1\end{bmatrix},\quad G = \begin{bmatrix}T^2/2&0\\T&0\\0&T^2/2\\0&T\end{bmatrix},\quad Q = G\,\mathrm{diag}(q_x^2, q_y^2)\,G^T. \]

q_x and q_y are the standard deviations of the accelerations the model does not know: how hard the target can brake or accelerate, how quickly it can move sideways. Their value matters a lot, as we will see.

The Kalman filter

Kalman filter as a block diagram
The filter as a block diagram: the previous estimate goes through the delay z^{-1} and the model F (with the input u = -a_{ego}) and becomes the prediction \hat x_{k|k-1}; the measurement z_k minus the predicted measurement H\hat x_{k|k-1} is the innovation \nu, which multiplied by the gain K_k corrects the prediction.

For every measurement the filter takes two steps:

  • prediction up to the time of the measurement: \hat{\mathbf{s}} \leftarrow F\hat{\mathbf{s}} + Bu, P \leftarrow F P F^T + Q;
  • update: innovation \nu = z - H\hat{\mathbf{s}}, its covariance S = H P H^T + R, gain K = P H^T S^{-1}, then \hat{\mathbf{s}} \leftarrow \hat{\mathbf{s}} + K\nu, P \leftarrow (I - KH)P(I-KH)^T + K R K^T.

Why that gain. After the correction the error is e^+ = (I - KH)e^- - K v, with e^- the prediction error and v the measurement noise, independent. Its covariance is

    \[ P^+ = (I-KH)P(I-KH)^T + K R K^T \]

(this is the Joseph form, which we also use in the code because it stays symmetric and positive with floating point numbers). We look for the K that minimises the sum of the variances, the trace of P^+. Expanding and differentiating with respect to K, with \partial\,\mathrm{tr}(KAK^T)/\partial K = 2KA for symmetric A:

    \[ \frac{\partial\,\mathrm{tr}P^+}{\partial K} = -2 P H^T + 2K (H P H^T + R) = 0 \quad\Rightarrow\quad K = P H^T S^{-1}. \]

Why two sensors are better than one

The simplest case says it all. Two independent estimates of the same quantity, x_1 with variance \sigma_1^2 and x_2 with \sigma_2^2: their weighted mean \hat x = w x_1 + (1-w) x_2 has variance w^2\sigma_1^2 + (1-w)^2\sigma_2^2, minimum for w = \sigma_2^2/(\sigma_1^2+\sigma_2^2). Substituting:

    \[ \frac{1}{\sigma^2} = \frac{1}{\sigma_1^2} + \frac{1}{\sigma_2^2}. \]

Information (the inverse of the variance) adds up: the fused estimate is always better than the better of the two. And this is exactly what the Kalman filter does, component by component: for the distance it weights mostly the radar, for the lateral position mostly the camera (the formulas of the Gaussian give the same result with Bayes’ theorem).

The measurements of the two sensors

The camera gives z_c = [x,\ y] with covariance R_c = \mathrm{diag}(\sigma_x^2, \sigma_y^2) derived in lesson 2: H_c takes the first and third components of the state. The radar measures in polar coordinates (r, \theta, \dot r); to use a linear filter the measurement is converted to Cartesian coordinates, x = r\cos\theta, y = r\sin\theta, and the covariance is propagated with the Jacobian matrix:

    \[ J = \begin{bmatrix}\cos\theta & -r\sin\theta\\ \sin\theta & r\cos\theta\end{bmatrix},\qquad R_{xy} = J\,\mathrm{diag}(\sigma_r^2, \sigma_\theta^2)\,J^T. \]

For a target ahead (\theta \approx 0) the error ellipse is narrow in x (\sigma_r) and wide in y (r\sigma_\theta). The radial speed \dot r \approx v_x is the third measurement. The sensors are not synchronised (radar at 20 Hz, camera at 15 Hz): the filter predicts each object up to the time of the measurement and uses it right away, in time order.

Associating measurements to objects

Before updating we have to know which measurement belongs to which object.

Validation gates and association
Each track (square) has a validation gate, the ellipse where its next measurement falls with 99% probability. Measurements (crosses) inside a gate are candidates for that track; the one outside all gates (red) starts a new track.
  • validation gate (gating): the Mahalanobis distance d^2 = \nu^T S^{-1}\nu of a correct measurement follows a \chi^2 distribution with as many degrees of freedom as the components of the measurement. We accept the measurement if d^2 \mathchar"313C  \gamma, with \gamma = 9.21 (2 degrees, camera) or 11.34 (3 degrees, radar) for 99%. Unlike the Euclidean distance, the gate widens where the track is uncertain;
  • global association (Global Nearest Neighbour): among all possible assignments (each track at most one measurement, each measurement at most one track) we pick the one with the most pairs and the smallest sum of distances. With few objects all can be tried; with many the Hungarian algorithm does it in O(n^3). In dense traffic or with many false echoes probabilistic methods are used (JPDA, MHT), seen in the lesson on radar tracking.

Managing the tracks

A measurement that is not associated starts a tentative track; it becomes confirmed when it has been associated in at least 3 of the last 5 cycles (M-of-N logic), and it is deleted after 6 cycles without measurements. Only confirmed tracks go to target selection: an isolated echo (a reflection, an insect on the radome) never reaches the planner.

def update(self, z, R, H):
    y = z - H @ self.s                   # innovation
    S = H @ self.P @ H.T + R             # innovation covariance
    K = self.P @ H.T @ np.linalg.inv(S)  # Kalman gain
    self.s = self.s + K @ y
    I_KH = np.eye(4) - K @ H
    self.P = I_KH @ self.P @ I_KH.T + K @ R @ K.T   # Joseph form
    return y, S

A mistake that makes fusion worse than one sensor

In the simulation a car ahead, from 30 to 110 m, changes lane at some point. With the first tuning, q_y = 0.5 m/s², the result was paradoxical: root mean square lateral error 0.32 m fusing radar and camera, 0.11 m with the camera alone. Fusion was worse than the better sensor.

The cause: with many measurements and a small q_y the filter became overconfident, \sigma_y \approx 2 cm. During the lane change (a true lateral acceleration of about 1 m/s²) the camera measurements, which were correct, fell outside the validation gate of such a self-assured filter and were discarded: 36 out of 222. Only the radar was left, imprecise sideways, and the track followed the manoeuvre late.

Tracking with radar, camera and fusion
With q_y = 1.5 m/s²: top, distance error (radar and fusion 0.03 m, camera 1.1 m); bottom, the estimated lateral position during the lane change between 6 and 10 s. Fusion (green) follows the manoeuvre better than either sensor: error 0.08 m against 0.19 m for the radar and 0.11 m for the camera.

With q_y = 1.5 m/s², a value that covers a real manoeuvre, fusion does its job again: 0.03 m in distance (like the radar) and 0.08 m sideways (better than the camera). The general lesson: process noise is not a smoothing parameter, it is a statement about how much objects can move; if it is too small the filter throws reality away. Series systems often use several models together (IMM, Interacting Multiple Models): a constant velocity one for quiet stretches and a manoeuvre one, weighted according to which explains the measurements better.

The tracker now gives a list of objects with state and covariance. Which one should we follow? That is the problem of the next lesson, and it is where an ACC earns the driver’s trust or loses it.

Subscribe
Notify of
guest
0 Comments
Oldest
Newest Most Voted
0
Would love your thoughts, please comment.x
()
x