Extended Kalman Filter (EKF)

El filtro de Kalman extendido (EKF) es un algoritmo recursivo que estima el estado de un sistema cuyo movimiento o mediciones son no lineales, como la orientación de un dron o la posición de un objetivo detectado por un radar que informa del alcance y el rumbo. El filtro de Kalman (KF) estándar solo es exacto para modelos lineales con ruido gaussiano. El EKF gestiona modelos no lineales linealizándolos en torno a la estimación actual en cada paso mediante matrices jacobianas de derivadas de primer orden y aplicando después las conocidas ecuaciones de predicción y actualización. Gerald Smith, Stanley Schmidt y Leonard McGee, de NASA Ames, lo desarrollaron a principios de la década de 1960 para la navegación a bordo de un vehículo circumlunar, un trabajo que contribuyó al ordenador de navegación del programa Apollo. Sigue siendo el estándar de facto para la estimación de estados no lineales en sistemas de navegación, receptores GPS, drones y robots.

Cómo funciona el EKF#

Al igual que el filtro de Kalman, el EKF mantiene una estimación del estado x y una covarianza P que describe su incertidumbre, pero el modelo de movimiento f y el modelo de medición h pueden ser funciones diferenciables cualesquiera. Cada paso tiene dos fases:

1. Predicción: La estimación pasa por el modelo de movimiento no lineal, x = f(x). El filtro evalúa el jacobiano F = ∂f/∂x en la estimación y aumenta la incertidumbre con el ruido del proceso Q: P = F P Fᵀ + Q.
2. Actualización: Llega una medición z. El filtro evalúa el jacobiano de medición H = ∂h/∂x en el estado predicho, calcula el residuo y = z − h(x) y la ganancia K = P Hᵀ (H P Hᵀ + R)⁻¹, donde R es el ruido de medición, y corrige la estimación con x = x + K y y P = (I − K H) P.

El estado y la medición predicha pasan por las funciones no lineales reales; solo la covarianza y la ganancia utilizan los jacobianos linealizados. Cuando f y h son lineales, los jacobianos equivalen a las matrices del modelo y el EKF se reduce exactamente al filtro de Kalman. La aproximación es buena cuando los modelos son casi lineales en todo el intervalo de incertidumbre y empeora cuando presentan una curvatura pronunciada dentro de él.

Un radar que mide el alcance y el rumbo es el ejemplo clásico: un objetivo que se mueve en línea recta en x e y es lineal en esas coordenadas, pero el alcance √(x² + y²) y el rumbo atan2(y, x) no lo son. Este EKF sigue a un objetivo así y estima su velocidad, que el radar nunca mide:

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

Convertir cada medición de alcance y rumbo sin procesar a x e y produce un error medio de posición de 1,43 m en 60 pasos, mientras que la estimación del EKF se desvía 0,57 m; además, el filtro recupera la velocidad como (1,01, −0,50) m/s.

Limitaciones y variantes#

El EKF no es un estimador óptimo. Si la estimación inicial se desvía mucho o los modelos son muy no lineales, los errores de linealización pueden hacer que el filtro peque de exceso de confianza y acabe divergiendo, por lo que los ingenieros suelen aumentar el ruido del proceso como medida de seguridad. Los jacobianos deben derivarse a mano o mediante diferenciación automática, y una derivada incorrecta es una causa habitual de fallos silenciosos. También hay que limitar los residuos de ángulos, como el rumbo o la orientación, al intervalo de −π a π. Hay varias variantes que abordan estos problemas: el EKF iterado vuelve a linealizar el modelo de medición durante la actualización, y el EKF de estado de error estima una pequeña corrección en torno a un estado nominal; es una forma habitual de seguir la orientación 3D con cuaterniones en la navegación inercial.

EKF frente a otros filtros#

La elección del filtro depende de cuánto se alejen los modelos de la linealidad y de la forma de la incertidumbre:

• Filtro de Kalman: es óptimo y más barato cuando ambos modelos son lineales. Los rastreadores de cajas delimitadoras, como ByteTrack, miden directamente las coordenadas de las cajas, así que basta con un filtro lineal.
• Filtro de Kalman unscented (UKF): Hace pasar un pequeño conjunto determinista de puntos sigma por las funciones no lineales en lugar de linealizarlas, no necesita jacobianos y suele ser más preciso que el EKF con modelos muy curvos.
• Filtro de partículas: Representa la distribución de probabilidad mediante muchas muestras aleatorias ponderadas, por lo que puede seguir distribuciones multimodales —por ejemplo, si un robot no sabe en cuál de dos pasillos está—, aunque con un coste computacional mucho mayor.
• Modelo oculto de Markov (HMM): Aplica la misma recursión de predicción y actualización a un conjunto finito de estados discretos.

Aplicaciones en el mundo real#

• Drones y aeronaves: El estimador EKF2 del piloto automático PX4 ejecuta un EKF de 24 estados que fusiona datos de la IMU, el GPS, el magnetómetro, el barómetro y otros sensores para estimar la orientación, la velocidad, la posición, los sesgos de los sensores, el campo magnético terrestre y el viento.
• Robótica: El paquete robot_localization de ROS proporciona nodos EKF y UKF que fusionan la odometría de las ruedas, las IMU y el GPS; además, EKF-SLAM es el método clásico de localización y mapeo simultáneos. La odometría visual-inercial basada en EKF, como el filtro de Kalman de restricción multiestado (MSCKF), sigue la pose de la cámara a partir de características de la imagen y lecturas de la IMU.
• Fusión de sensores en automoción: Los sistemas de asistencia a la conducción y de vehículos autónomos fusionan el alcance y el rumbo del radar con las detecciones de la cámara y el lidar para seguir la posición y la velocidad de los coches cercanos.
• Localización basada en cámara: Una sola cámara solo mide la dirección hacia un objeto. Las canalizaciones que sitúan los objetos detectados en un mapa utilizan las detecciones de Ultralytics YOLO26 como mediciones del EKF: el centro de cada cuadro es una posición en píxeles, una proyección no lineal de la posición 3D del objeto mediante el modelo de cámara, que el filtro fusiona con el alcance del radar o con los datos del GPS y la IMU de la plataforma de la cámara.