ACC 3: fusione radar e camera con il filtro di Kalman
Fusione centralizzata e traccia a traccia, modello del moto relativo, guadagno di Kalman dimostrato, perché due sensori sono meglio di uno, gating, associazione, gestione delle tracce e un errore di taratura.
https://www.electronics-engineering.com/acc-fusione-radar-camera-filtro-di-kalman-associazione-tracce
Radar e camera mandano, ciascuno al proprio ritmo, liste di misure rumorose, a volte mancanti, a volte false. La fusione le trasforma in una lista di oggetti tracciati: per ognuno una stima di posizione e velocità con la sua incertezza, aggiornata ogni ciclo. Lo strumento è il filtro di Kalman, che abbiamo introdotto nella lezione sul filtro di Kalman e ritrovato tra gli osservatori discreti; qui lo applichiamo a più sensori, aggiungiamo l’associazione delle misure agli oggetti e la gestione delle tracce, e vediamo un errore di taratura che rende la fusione peggiore di un sensore da solo.
Due architetture

- centralizzata (A): la più precisa, perché il filtro usa ogni misura con la sua vera incertezza; richiede misure grezze o quasi, sincronizzate;
- traccia a traccia (B): ogni centralina sensore fa già il suo tracciamento (succede spesso con sensori di fornitori diversi) e la centralina ADAS fonde le tracce. Due tracce dello stesso oggetto hanno errori correlati (hanno lo stesso modello di moto), e trattarle come indipendenti renderebbe la stima troppo ottimista. L’intersezione delle covarianze è sicura anche senza conoscere la correlazione:
,
, con
scelto per minimizzare la traccia di
.
Nel codice usiamo la fusione centralizzata.
Il modello del moto relativo
Lo stato di ogni oggetto è relativo alla nostra auto:
. Longitudinalmente l’accelerazione relativa è
: quella del bersaglio
non la conosciamo e la trattiamo come rumore, la nostra
la conosciamo e la usiamo come ingresso. Integrando su un passo
con accelerazione costante (vedi la cinematica del moto rettilineo):
![]()
e lo stesso lateralmente senza ingresso. In forma matriciale
con
![Rendered by QuickLaTeX.com \[ 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. \]](https://www.electronics-engineering.com/wp-content/ql-cache/quicklatex.com-5c72a73869966fe43ef086161194e36b_l3.png)
e
sono le deviazioni standard delle accelerazioni che il modello non conosce: quanto può frenare o accelerare il bersaglio, quanto in fretta può spostarsi di lato. Il loro valore conta moltissimo, come vedremo.
Il filtro di Kalman

e il modello
(con l’ingresso
) e diventa la previsione
; la misura
meno la misura prevista
è l’innovazione
, che moltiplicata per il guadagno
corregge la previsione.A ogni misura il filtro fa due passi:
- previsione fino all’istante della misura:
,
; - aggiornamento: innovazione
, sua covarianza
, guadagno
, poi
,
.
Perché quel guadagno. Dopo la correzione l’errore è
, con
errore della previsione e
rumore di misura, indipendenti. La sua covarianza è
![]()
(è la forma di Joseph, che usiamo anche nel codice perché resta simmetrica e positiva con i numeri in virgola mobile). Cerchiamo il
che rende minima la somma delle varianze, la traccia di
. Sviluppando e derivando rispetto a
, con
per
simmetrica:
![]()
Perché due sensori sono meglio di uno
Il caso più semplice dice tutto. Due stime indipendenti della stessa grandezza,
con varianza
e
con
: la loro media pesata
ha varianza
, minima per
. Sostituendo:
![]()
L’informazione (l’inverso della varianza) si somma: la stima fusa è sempre migliore della migliore delle due. Ed è esattamente quello che fa il filtro di Kalman, componente per componente: per la distanza pesa soprattutto il radar, per la posizione laterale soprattutto la camera (le formule della gaussiana danno lo stesso risultato con il teorema di Bayes).
Le misure dei due sensori
La camera dà
con covarianza
ricavata nella lezione 2:
prende la prima e la terza componente dello stato. Il radar misura in coordinate polari
; per usare un filtro lineare si converte la misura in coordinate cartesiane,
,
, e si propaga la covarianza con la matrice jacobiana:
![]()
Per un bersaglio davanti (
) l’ellisse d’errore è stretta in
(
) e larga in
(
). La velocità radiale
è la terza misura. I sensori non sono sincronizzati (radar a 20 Hz, camera a 15 Hz): il filtro prevede ogni oggetto fino all’istante della misura e la usa subito, in ordine di tempo.
Associare le misure agli oggetti
Prima di aggiornare bisogna sapere quale misura appartiene a quale oggetto.

- finestra di validazione (gating): la distanza di Mahalanobis
di una misura corretta segue una distribuzione
con tanti gradi di libertà quante le componenti della misura. Accettiamo la misura se
, con
(2 gradi, camera) o 11.34 (3 gradi, radar) per il 99%. A differenza della distanza euclidea, la finestra si allarga dove la traccia è incerta; - associazione globale (Global Nearest Neighbour): tra tutte le assegnazioni possibili (ogni traccia al massimo una misura, ogni misura al massimo una traccia) si sceglie quella con più coppie e somma delle distanze minima. Con pochi oggetti si possono provare tutte; con molti l’algoritmo ungherese lo fa in
. Nel traffico denso o con molti falsi echi si usano metodi probabilistici (JPDA, MHT), visti nella lezione sul radar ad inseguimento.
Gestire le tracce
Una misura non associata fa nascere una traccia provvisoria; diventa confermata quando è stata associata in almeno 3 degli ultimi 5 cicli (logica M su N), e viene cancellata dopo 6 cicli senza misure. Solo le tracce confermate vanno alla selezione dell’obiettivo: un eco isolato (una riflessione, un insetto sul radome) non arriva mai al pianificatore.
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, SUn errore che rende la fusione peggiore di un sensore
Nella simulazione un’auto davanti, da 30 a 110 m, a un certo punto cambia corsia. Con la prima taratura,
m/s², il risultato era paradossale: errore laterale quadratico medio 0.32 m fondendo radar e camera, 0.11 m con la sola camera. La fusione era peggiore del sensore migliore.
La causa: con tante misure e un
piccolo il filtro diventava troppo sicuro,
cm. Durante il cambio di corsia (un’accelerazione laterale vera di circa 1 m/s²) le misure della camera, corrette, finivano fuori dalla finestra di validazione di un filtro così sicuro di sé e venivano scartate: 36 su 222. Restava solo il radar, impreciso di lato, e la traccia inseguiva la manovra in ritardo.

m/s²: sopra, errore sulla distanza (radar e fusione 0.03 m, camera 1.1 m); sotto, la posizione laterale stimata durante il cambio di corsia tra 6 e 10 s. La fusione (verde) segue la manovra meglio di entrambi i sensori: errore 0.08 m contro 0.19 m del radar e 0.11 m della camera.Con
m/s², un valore che copre una manovra reale, la fusione torna a fare il suo mestiere: 0.03 m in distanza (come il radar) e 0.08 m di lato (meglio della camera). La lezione generale: il rumore di processo non è un parametro di lisciatura, è un’affermazione su quanto si possono muovere gli oggetti; se è troppo piccolo il filtro scarta la realtà. Nei sistemi di serie si usano spesso più modelli insieme (IMM, Interacting Multiple Models): uno a velocità costante per i tratti tranquilli e uno di manovra, pesati in base a quale spiega meglio le misure.
Il tracciatore dà ora una lista di oggetti con stato e covarianza. Quale di questi seguire? È il problema della prossima lezione, ed è il punto in cui un ACC si guadagna la fiducia del guidatore o la perde.




