Extended Kalman Filter (EKF)

O filtro de Kalman estendido (EKF) é um algoritmo recursivo que estima o estado de um sistema cujo movimento ou cujas medições são não lineares, como a orientação de um drone ou a posição de um alvo observado por um radar que informa distância e azimute. O filtro de Kalman (KF) padrão é exato apenas para modelos lineares com ruído gaussiano. O EKF lida com modelos não lineares linearizando-os em torno da estimativa atual a cada passo, usando matrizes jacobianas de derivadas de primeira ordem e, em seguida, aplicando as conhecidas equações de predição e atualização. Gerald Smith, Stanley Schmidt e Leonard McGee, da NASA Ames, desenvolveram-no no início da década de 1960 para a navegação a bordo de um veículo em órbita circunlunar, trabalho que contribuiu para o computador de navegação Apollo; ele continua sendo o padrão de fato para estimativa não linear de estado em sistemas de navegação, receptores GPS, drones e robôs.

Como funciona o EKF#

Assim como o filtro de Kalman, o EKF mantém uma estimativa de estado x e uma covariância P que descreve sua incerteza, mas o modelo de movimento f e o modelo de medição h podem ser quaisquer funções diferenciáveis. Cada passo tem duas fases:

1. Predição: A estimativa passa pelo modelo de movimento não linear, x = f(x). O filtro calcula a jacobiana F = ∂f/∂x na estimativa e aumenta a incerteza com o ruído do processo Q: P = F P Fᵀ + Q.
2. Atualização: Uma medição z é recebida. O filtro calcula a jacobiana da medição H = ∂h/∂x no estado previsto, calcula o resíduo y = z − h(x) e o ganho K = P Hᵀ (H P Hᵀ + R)⁻¹, em que R é o ruído de medição, e corrige a estimativa com x = x + K y e P = (I − K H) P.

O estado e a medição prevista passam pelas funções não lineares reais; apenas a covariância e o ganho usam as jacobianas linearizadas. Quando f e h são lineares, as jacobianas são iguais às matrizes do modelo e o EKF se reduz exatamente ao filtro de Kalman. A aproximação é boa quando os modelos são quase lineares em toda a faixa de incerteza e piora quando apresentam curvaturas acentuadas dentro dessa faixa.

Um radar que mede distância e azimute é o caso clássico: um alvo que se move em linha reta em x e y é linear nessas coordenadas, mas a distância √(x² + y²) e o azimute atan2(y, x) não são. Este EKF acompanha esse alvo e estima sua velocidade, que o radar nunca mede:

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

Converter cada medição bruta de distância e azimute para x e y resulta em um erro médio de posição de 1,43 m ao longo de 60 passos, enquanto a estimativa do EKF erra por 0,57 m, e o filtro recupera a velocidade como (1,01, −0,50) m/s.

Limitações e variantes#

O EKF não é um estimador ótimo. Se a estimativa inicial estiver muito distante ou os modelos forem fortemente não lineares, os erros de linearização podem deixar o filtro confiante demais e, em seguida, fazê-lo divergir; por isso, os engenheiros costumam aumentar o ruído do processo por segurança. As jacobianas precisam ser derivadas manualmente ou por diferenciação automática, e uma derivada incorreta é uma causa comum de falhas silenciosas. Os resíduos de ângulos como azimute ou orientação também precisam ser ajustados ao intervalo de −π a π. Várias variantes tratam desses problemas: o EKF iterado relinieriza o modelo de medição durante a atualização, e o EKF de estado de erro estima uma pequena correção em torno de um estado nominal, uma forma comum de acompanhar a orientação 3D com quaternions na navegação inercial.

EKF versus outros filtros#

A escolha entre filtros depende de quão distantes da linearidade estão os modelos e da forma da incerteza:

• Filtro de Kalman: é ideal e mais barato quando os dois modelos são lineares. Os rastreadores de caixas delimitadoras, como o ByteTrack, medem diretamente as coordenadas das caixas, por isso um filtro linear é suficiente.
• Filtro de Kalman não linear (UKF): Passa um pequeno conjunto determinístico de pontos sigma pelas funções não lineares, em vez de linearizá-las; não precisa de jacobianas e costuma ser mais preciso que o EKF para modelos muito curvos.
• Filtro de partículas: Representa a crença com muitas amostras aleatórias ponderadas, podendo assim acompanhar distribuições multimodais, como um robô que não sabe em qual de dois corredores está, a um custo computacional muito maior.
• Modelo oculto de Markov (HMM): Aplica a mesma recursão de predição e atualização a um conjunto finito de estados discretos.

Aplicações no mundo real#

• Drones e aeronaves: O estimador EKF2 do piloto automático PX4 executa um EKF de 24 estados que combina dados da IMU, do GPS, do magnetômetro, do barômetro e de outros sensores para estimar orientação, velocidade, posição, vieses dos sensores, campo magnético da Terra e vento.
• Robótica: O pacote robot_localization do ROS oferece nós EKF e UKF que combinam odometria das rodas, IMUs e GPS; o EKF-SLAM é a abordagem clássica para localização e mapeamento simultâneos. A odometria visual-inercial baseada em EKF, como o filtro de Kalman com restrição de múltiplos estados (MSCKF), acompanha a pose da câmera usando características da imagem e leituras da IMU.
• Fusão de sensores automotivos: Os sistemas de assistência ao motorista e de veículos autônomos combinam distância e azimute do radar com detecções de câmera e lidar para acompanhar a posição e a velocidade dos carros ao redor.
• Localização baseada em câmera: Uma única câmera mede apenas a direção até um objeto. Os pipelines que posicionam objetos detectados em um mapa usam detecções do Ultralytics YOLO26 como medições do EKF: o centro de cada caixa é uma posição em pixels, uma projeção não linear da posição 3D do objeto pelo modelo da câmera, que o filtro combina com a distância do radar ou com os dados de GPS e IMU da plataforma da câmera.