Extended Kalman Filter (EKF)

Il filtro di Kalman esteso (EKF) è un algoritmo ricorsivo che stima lo stato di un sistema il cui moto o le cui misurazioni sono non lineari, come l'orientamento di un drone o la posizione di un bersaglio rilevata da un radar che misura distanza e angolo. Il filtro di Kalman (KF) standard è esatto solo per modelli lineari con rumore gaussiano. L'EKF gestisce i modelli non lineari linearizzandoli intorno alla stima corrente a ogni passo, usando matrici jacobiane delle derivate prime, quindi applica le note equazioni di predizione e aggiornamento. Gerald Smith, Stanley Schmidt e Leonard McGee della NASA Ames lo svilupparono all'inizio degli anni Sessanta per la navigazione a bordo di un veicolo circumlunare; quel lavoro contribuì al computer di navigazione Apollo. L'EKF rimane lo standard di fatto per la stima non lineare dello stato nei sistemi di navigazione, nei ricevitori GPS, nei droni e nei robot.

Come funziona l'EKF#

Come il filtro di Kalman, l'EKF mantiene una stima dello stato x e una covarianza P che ne descrive l'incertezza, ma il modello di moto f e il modello di misurazione h possono essere funzioni differenziabili qualsiasi. Ogni passo prevede due fasi:

1. Predizione: la stima viene propagata attraverso il modello di moto non lineare, x = f(x). Il filtro valuta la matrice jacobiana F = ∂f/∂x in corrispondenza della stima e aumenta l'incertezza in base al rumore di processo Q: P = F P Fᵀ + Q.
2. Aggiornamento: arriva una misurazione z. Il filtro valuta la matrice jacobiana della misurazione H = ∂h/∂x nello stato previsto, calcola il residuo y = z − h(x) e il guadagno K = P Hᵀ (H P Hᵀ + R)⁻¹, dove R è il rumore di misurazione, quindi corregge la stima con x = x + K y e P = (I − K H) P.

Lo stato e la misurazione prevista attraversano le funzioni non lineari effettive; solo la covarianza e il guadagno usano le matrici jacobiane linearizzate. Quando f e h sono lineari, le matrici jacobiane coincidono con le matrici del modello e l'EKF si riduce esattamente al filtro di Kalman. L'approssimazione è valida quando i modelli sono quasi lineari nell'intervallo dell'incertezza e peggiora quando la curvatura all'interno di tale intervallo è pronunciata.

Il caso classico è un radar che misura distanza e angolo: un bersaglio che si muove in linea retta lungo x e y è lineare in quelle coordinate, mentre distanza √(x² + y²) e angolo atan2(y, x) non lo sono. Questo EKF traccia un bersaglio di questo tipo e ne stima la velocità, che il radar non misura mai:

import numpy as np

rng = np.random.default_rng(0)
F = np.eye(4) + np.eye(4, k=2)  # constant velocity, state = [x, y, vx, vy], 1 s per step
Q, R = 1e-4 * np.eye(4), np.diag([1.0**2, 0.02**2])  # process noise; radar noise of 1 m and 0.02 rad
s = np.array([20.0, 50.0, 1.0, -0.5])  # true state
x, P = np.array([20.0, 50.0, 0.0, 0.0]), np.diag([1.0, 1.0, 4.0, 4.0])  # estimate starts with unknown velocity

raw_err, ekf_err = [], []
for _ in range(60):
    s = F @ s
    z = np.array([np.hypot(s[0], s[1]), np.arctan2(s[1], s[0])]) + rng.normal(0, [1.0, 0.02])  # range, bearing
    x, P = F @ x, F @ P @ F.T + Q  # predict
    r = np.hypot(x[0], x[1])
    H = np.array([[x[0] / r, x[1] / r, 0, 0], [-x[1] / r**2, x[0] / r**2, 0, 0]])  # Jacobian of h(x)
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
    x = x + K @ (z - np.array([r, np.arctan2(x[1], x[0])]))  # update with the nonlinear residual
    P = (np.eye(4) - K @ H) @ P
    raw_err.append(np.hypot(z[0] * np.cos(z[1]) - s[0], z[0] * np.sin(z[1]) - s[1]))
    ekf_err.append(np.hypot(x[0] - s[0], x[1] - s[1]))

print(f"Mean position error: raw radar {np.mean(raw_err):.2f} m, EKF {np.mean(ekf_err):.2f} m")
print(f"Estimated velocity: ({x[2]:.2f}, {x[3]:.2f}) m/s, true (1.00, -0.50)")

Convertire ogni misurazione grezza di distanza e angolo in x e y produce un errore medio sulla posizione di 1.43 m in 60 passi, mentre la stima dell'EKF ha un errore di 0.57 m e il filtro recupera la velocità come (1.01, −0.50) m/s.

Limiti e varianti#

L'EKF non è uno stimatore ottimale. Se la stima iniziale è molto imprecisa o i modelli sono fortemente non lineari, gli errori di linearizzazione possono rendere il filtro troppo sicuro e poi farlo divergere; per questo gli ingegneri spesso aumentano il rumore di processo per sicurezza. Le matrici jacobiane devono essere derivate a mano o tramite differenziazione automatica, e una derivata errata è una causa frequente di errori silenziosi. Anche i residui degli angoli, come l'angolo di rilevamento o l'orientamento, devono essere ricondotti all'intervallo da −π a π. Diverse varianti affrontano questi problemi: l'EKF iterato linearizza nuovamente il modello di misurazione durante l'aggiornamento, mentre l'EKF a stato d'errore stima una piccola correzione intorno a uno stato nominale, un approccio comune per tracciare l'orientamento 3D con quaternioni nella navigazione inerziale.

EKF a confronto con altri filtri#

La scelta tra i filtri dipende da quanto i modelli si discostano dalla linearità e dalla forma dell'incertezza:

• Filtro di Kalman: ottimale e meno costoso quando entrambi i modelli sono lineari. I tracker basati su riquadri, come ByteTrack, misurano direttamente le coordinate dei riquadri, quindi per loro è sufficiente un filtro lineare.
• Filtro di Kalman unscented (UKF): propaga un piccolo insieme deterministico di punti sigma attraverso le funzioni non lineari invece di linearizzarle; non richiede matrici jacobiane ed è in genere più accurato dell'EKF con modelli fortemente curvi.
• Filtro particellare: rappresenta la distribuzione di probabilità con molti campioni casuali ponderati, per cui può tracciare distribuzioni multimodali, come nel caso di un robot che non sa in quale dei due corridoi si trovi, ma con un costo di calcolo molto maggiore.
• Modello di Markov nascosto (HMM): applica la stessa ricorsione di predizione e aggiornamento a un insieme finito di stati discreti.

Applicazioni nel mondo reale#

• Droni e aeromobili: lo stimatore EKF2 del pilota automatico PX4 esegue un EKF a 24 stati che fonde i dati di IMU, GPS, magnetometro, barometro e altri sensori per stimare orientamento, velocità, posizione, polarizzazioni dei sensori, campo magnetico terrestre e vento.
• Robotica: il pacchetto ROS robot_localization fornisce nodi EKF e UKF che fondono l'odometria delle ruote, le IMU e il GPS; EKF-SLAM è l'approccio classico per la localizzazione e mappatura simultanee. L'odometria visivo-inerziale basata su EKF, come il filtro di Kalman con vincoli a stati multipli (MSCKF), traccia la posa della videocamera usando le caratteristiche dell'immagine e le letture delle IMU.
• Fusione dei sensori nel settore automobilistico: i sistemi di assistenza alla guida e i sistemi per veicoli autonomi fondono distanza e angolo rilevati dal radar con i rilevamenti di videocamere e lidar per tracciare la posizione e la velocità delle auto circostanti.
• Localizzazione basata su videocamera: una singola videocamera misura solo la direzione verso un oggetto. Le pipeline che collocano gli oggetti rilevati su una mappa usano i rilevamenti di Ultralytics YOLO26 come misurazioni per l'EKF: il centro di ogni riquadro è una posizione in pixel, cioè una proiezione non lineare della posizione 3D dell'oggetto attraverso il modello della videocamera, che il filtro fonde con la distanza del radar o con i dati GPS e IMU della piattaforma della videocamera.