Extended Kalman Filter (EKF)

Le filtre de Kalman étendu (EKF) est un algorithme récursif qui estime l’état d’un système dont le mouvement ou les mesures sont non linéaires, par exemple l’orientation d’un drone ou la position d’une cible observée par un radar qui indique la distance et l’azimut. Le filtre de Kalman (KF) standard est exact uniquement pour les modèles linéaires avec un bruit gaussien. L’EKF gère les modèles non linéaires en les linéarisant autour de l’estimation actuelle à chaque étape à l’aide de matrices jacobiennes de dérivées premières, puis en appliquant les équations familières de prédiction et de mise à jour. Gerald Smith, Stanley Schmidt et Leonard McGee, de la NASA Ames, l’ont développé au début des années 1960 pour la navigation embarquée d’un véhicule circumlunaire. Ces travaux ont contribué à l’ordinateur de navigation Apollo, et l’EKF reste la méthode de référence pour l’estimation d’état non linéaire dans les systèmes de navigation, les récepteurs GPS, les drones et les robots.

Fonctionnement de l’EKF#

Comme le filtre de Kalman, l’EKF conserve une estimation d’état x et une covariance P qui décrit son incertitude, mais le modèle de mouvement f et le modèle de mesure h peuvent être des fonctions différentiables quelconques. Chaque étape comporte deux phases :

1. Prédiction : l’estimation passe par le modèle de mouvement non linéaire, x = f(x). Le filtre évalue la jacobienne F = ∂f/∂x à l’estimation et augmente l’incertitude en fonction du bruit de processus Q : P = F P Fᵀ + Q.
2. Mise à jour : une mesure z est reçue. Le filtre évalue la jacobienne de mesure H = ∂h/∂x à l’état prédit, calcule le résidu y = z − h(x) et le gain K = P Hᵀ (H P Hᵀ + R)⁻¹, où R représente le bruit de mesure, puis corrige l’estimation à l’aide de x = x + K y et P = (I − K H) P.

L’état et la mesure prédite passent par les fonctions non linéaires réelles ; seules la covariance et le gain utilisent les jacobiennes linéarisées. Lorsque f et h sont linéaires, les jacobiennes sont égales aux matrices du modèle et l’EKF se réduit exactement au filtre de Kalman. L’approximation est bonne lorsque les modèles sont presque linéaires sur l’étendue de l’incertitude et se dégrade lorsqu’ils présentent de fortes courbures dans cet intervalle.

Un radar qui mesure la distance et l’azimut en est l’exemple classique : une cible se déplaçant en ligne droite selon x et y suit un mouvement linéaire dans ces coordonnées, mais la distance √(x² + y²) et l’azimut atan2(y, x) ne le sont pas. Cet EKF suit une telle cible et estime sa vitesse, que le radar ne mesure jamais :

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)")

La conversion de chaque mesure brute de distance et d’azimut en coordonnées x et y entraîne une erreur moyenne de position de 1,43 m sur 60 étapes, tandis que l’estimation de l’EKF présente une erreur de 0,57 m. Le filtre retrouve une vitesse de (1,01, −0,50) m/s.

Limites et variantes#

L’EKF n’est pas un estimateur optimal. Si l’estimation initiale est très éloignée de la réalité ou si les modèles sont fortement non linéaires, les erreurs de linéarisation peuvent rendre le filtre trop confiant, puis le faire diverger. Les ingénieurs augmentent donc souvent le bruit de processus par précaution. Les jacobiennes doivent être dérivées à la main ou par différentiation automatique, et une dérivée erronée est une cause fréquente de défaillances silencieuses. Les résidus des angles, comme l’azimut ou le cap, doivent aussi être ramenés à l’intervalle de −π à π. Plusieurs variantes répondent à ces problèmes : l’EKF itéré relinéarise le modèle de mesure pendant la mise à jour, et l’EKF à état d’erreur estime une petite correction autour d’un état nominal, une méthode courante pour suivre l’orientation 3D avec des quaternions en navigation inertielle.

L’EKF comparé aux autres filtres#

Le choix d’un filtre dépend de l’écart entre les modèles et la linéarité, ainsi que de la forme de l’incertitude :

• Filtre de Kalman : optimal et plus économique lorsque les deux modèles sont linéaires. Les trackers de boîtes englobantes comme ByteTrack mesurent directement les coordonnées des boîtes ; un filtre linéaire leur suffit donc.
• Filtre de Kalman sans parfum (UKF) : fait passer un petit ensemble déterministe de points sigma dans les fonctions non linéaires au lieu de les linéariser, ne nécessite aucune jacobienne et est généralement plus précis que l’EKF pour les modèles très courbés.
• Filtre particulaire : représente la distribution de probabilité par de nombreux échantillons aléatoires pondérés. Il peut ainsi suivre des distributions multimodales, comme le cas d’un robot qui ne sait pas dans lequel de deux couloirs il se trouve, mais son coût de calcul est bien plus élevé.
• Modèle de Markov caché (HMM) : applique la même récursion de prédiction et de mise à jour à un ensemble fini d’états discrets.

Applications concrètes#

• Drones et aéronefs : l’estimateur EKF2 du pilote automatique PX4 exécute un EKF à 24 états qui fusionne les données de l’IMU, du GPS, du magnétomètre, du baromètre et d’autres capteurs pour estimer l’orientation, la vitesse, la position, les biais des capteurs, le champ magnétique terrestre et le vent.
• Robotique : le paquet robot_localization de ROS fournit des nœuds EKF et UKF qui fusionnent l’odométrie des roues, les IMU et le GPS. L’EKF-SLAM est la méthode classique de localisation et cartographie simultanées. L’odométrie visuo-inertielle fondée sur l’EKF, comme le filtre de Kalman à contraintes multi-états (MSCKF), suit la pose de la caméra à partir des caractéristiques de l’image et des relevés de l’IMU.
• Fusion de capteurs automobile : les systèmes d’aide à la conduite et les systèmes de véhicules autonomes fusionnent la distance et l’azimut fournis par le radar avec les détections des caméras et du lidar pour suivre la position et la vitesse des voitures environnantes.
• Localisation par caméra : une seule caméra mesure uniquement la direction d’un objet. Les pipelines qui placent les objets détectés sur une carte utilisent les détections d’Ultralytics YOLO26 comme mesures EKF : le centre de chaque boîte est une position en pixels, projection non linéaire de la position 3D de l’objet selon le modèle de la caméra, que le filtre fusionne avec la distance du radar ou les données GPS et IMU de la plateforme porteuse de la caméra.