Filtro de Kalman Estendido (Extended Kalman Filter, EKF)

Visão geral

O Filtro de Kalman Estendido (Extended Kalman Filter, EKF) é um algoritmo clássico de estimação recursiva de estado (recursive state estimation) em sistemas com dinâmica não linear (nonlinear dynamics) e/ou modelos de medição não lineares (nonlinear measurement models). Ele generaliza o filtro de Kalman (Kalman filter) (linear) ao linearizar (linearizing) as funções não lineares em torno da estimativa atual usando jacobianos (Jacobians) e, em seguida, aplicar atualizações Gaussianas no estilo do filtro de Kalman.

EKFs são especialmente comuns em robótica, onde movimento e sensoriamento são naturalmente não lineares:

  • Localização (rastreamento de pose a partir de odometria + GPS/visão/LiDAR)
  • Fusão de sensores (por exemplo, IMU + GNSS + encoders de roda)
  • EKF-SLAM (estimativa simultânea da pose do robô e de um mapa de marcos)

Na taxonomia de Filtragem Bayesiana, o EKF fica entre:

  • o filtro de Kalman (exato para modelos lineares–Gaussianos), e
  • métodos mais flexíveis, porém mais caros, como o Filtro de Partículas.

Modelo de espaço de estados não linear (configuração do problema)

O EKF assume um estado latente (latent state) (x_k) (por exemplo, pose e velocidade do robô) que evolui ao longo do tempo. No passo de tempo (k):

  • Dinâmica (modelo de movimento): [ x_k = f(x_{k-1}, u_k) + w_{k-1}, \quad w_{k-1} \sim \mathcal{N}(0, Q_{k-1}) ]

  • Medições (modelo do sensor): [ z_k = h(x_k) + v_k, \quad v_k \sim \mathcal{N}(0, R_k) ]

Onde:

  • (x_k \in \mathbb{R}^n): estado (por exemplo, ([x, y, \theta]))
  • (u_k): entrada de controle (por exemplo, velocidades comandadas)
  • (z_k): medição (por exemplo, posição do GPS ou azimute para um marco)
  • (f(\cdot)): função de transição não linear
  • (h(\cdot)): função de medição não linear
  • (Q): covariância do ruído de processo
  • (R): covariância do ruído de medição

O EKF mantém uma crença Gaussiana (Gaussian belief): [ p(x_k \mid z_{1:k}) \approx \mathcal{N}(\mu_k, P_k) ] com média (\mu_k) e covariância (P_k).

Ideia-chave: linearizar com jacobianos

Como (f) e (h) são não lineares, o posterior exato em geral não é Gaussiano. O EKF o torna tratável ao aproximar (f) e (h) localmente com uma expansão de Taylor de primeira ordem (first-order Taylor expansion) em torno da estimativa atual.

Linearização do modelo de movimento

Linearize (f(x, u)) em torno de (\mu_{k-1}):

[ f(x_{k-1}, u_k) \approx f(\mu_{k-1}, u_k) + F_k (x_{k-1} - \mu_{k-1}) ]

Onde o jacobiano do estado (state Jacobian) é: [ F_k = \left.\frac{\partial f}{\partial x}\right|{x=\mu{k-1}, u=u_k} ]

Se o ruído de processo entrar de forma não aditiva, você também pode precisar de: [ L_k = \left.\frac{\partial f}{\partial w}\right|{x=\mu{k-1}} ] e então usar (L_k Q L_k^\top) na predição de covariância. Em muitos modelos de robótica, o ruído é tratado como aditivo, então (L_k = I).

Linearização do modelo de medição

Linearize (h(x)) em torno da média predita (\mu_k^{-}):

[ h(x_k) \approx h(\mu_k^-) + H_k (x_k - \mu_k^-) ]

Onde: [ H_k = \left.\frac{\partial h}{\partial x}\right|_{x=\mu_k^-} ]

Isso transforma o problema local em uma atualização Gaussiana linear (linear Gaussian), viabilizando equações no estilo do filtro de Kalman.

Algoritmo EKF: equações de predição–atualização

O EKF prossegue em duas fases a cada passo de tempo: predição (atualização no tempo) e correção (atualização por medição).

1) Predição (atualização no tempo)

Dado ((\mu_{k-1}, P_{k-1})):

Média predita [ \mu_k^- = f(\mu_{k-1}, u_k) ]

Jacobiano da dinâmica [ F_k = \left.\frac{\partial f}{\partial x}\right|{x=\mu{k-1}} ]

Covariância predita [ P_k^- = F_k P_{k-1} F_k^\top + Q_{k-1} ] (ou (+ L_k Q L_k^\top) se estiver usando um jacobiano do ruído (L_k)).

2) Atualização (correção por medição)

Calcule a inovação (innovation) (também conhecida como residual): [ y_k = z_k - h(\mu_k^-) ]

Calcule o jacobiano de medição (measurement Jacobian): [ H_k = \left.\frac{\partial h}{\partial x}\right|_{x=\mu_k^-} ]

Calcule a covariância da inovação: [ S_k = H_k P_k^- H_k^\top + R_k ]

Calcule o ganho de Kalman: [ K_k = P_k^- H_k^\top S_k^{-1} ]

Atualize a média: [ \mu_k = \mu_k^- + K_k y_k ]

Atualize a covariância (forma “simples” comum): [ P_k = (I - K_k H_k) P_k^- ]

Uma alternativa numericamente mais segura é a forma de Joseph (Joseph form): [ P_k = (I - K_k H_k)P_k^-(I - K_k H_k)^\top + K_k R_k K_k^\top ] que preserva melhor a simetria e a semidefinição positiva.

EKF em pseudocódigo

given μ, P
for each timestep k:
  # Predict
  μ_minus = f(μ, u_k)
  F = jacobian(f, x=μ, u=u_k)
  P_minus = F P F^T + Q

  # Update
  H = jacobian(h, x=μ_minus)
  y = z_k - h(μ_minus)
  S = H P_minus H^T + R
  K = P_minus H^T S^{-1}
  μ = μ_minus + K y
  P = (I - K H) P_minus   # or Joseph form

Exemplo prático: localização de robô 2D com medições de marcos

Considere um robô móvel com estado: [ x = [,x,\ y,\ \theta,]^\top ] e um marco conhecido na posição ((m_x, m_y)). O robô mede alcance (range) e azimute (bearing) até o marco.

Modelo de medição (alcance–azimute)

\[ h(x) = \begin{bmatrix} r \\ \phi \end{bmatrix}

\begin{bmatrix} \sqrt{(m_x-x)^2 + (m_y-y)^2} \ \mathrm{atan2}(m_y-y,\ m_x-x) - \theta \end{bmatrix} ]

Isso é não linear por causa da raiz quadrada e de (\mathrm{atan2}).

Defina:

  • (\Delta x = m_x - x)
  • (\Delta y = m_y - y)
  • (q = \Delta x^2 + \Delta y^2)
  • (r = \sqrt{q})

Então o jacobiano (H = \frac{\partial h}{\partial x}) é:

[ H = \begin{bmatrix} -\Delta x / r & -\Delta y / r & 0 \ \Delta y / q & -\Delta x / q & -1 \end{bmatrix} ]

Um passo mínimo de atualização do EKF (estilo Python)

import numpy as np

def wrap_angle(a):
    return (a + np.pi) % (2*np.pi) - np.pi

def ekf_update(mu, P, z, landmark, R):
    x, y, th = mu
    mx, my = landmark

    dx = mx - x
    dy = my - y
    q = dx*dx + dy*dy
    r = np.sqrt(q)

    # predicted measurement
    zhat = np.array([r, np.arctan2(dy, dx) - th])
    y_res = z - zhat
    y_res[1] = wrap_angle(y_res[1])

    # Jacobian H
    H = np.array([
        [-dx/r,    -dy/r,     0.0],
        [ dy/q,    -dx/q,    -1.0],
    ])

    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)

    mu_new = mu + K @ y_res
    mu_new[2] = wrap_angle(mu_new[2])

    I = np.eye(3)
    P_new = (I - K @ H) @ P  # consider Joseph form in real systems
    return mu_new, P_new

Este exemplo destaca detalhes práticos do EKF:

  • O ajuste (wrapping) de ângulo (angle wrapping) é essencial para resíduos de rumo/azimute.
  • O jacobiano é a “ponte” da geometria não linear para atualizações Gaussianas lineares.

Suposições e limitações

O EKF é poderoso, mas suas aproximações podem falhar de maneiras importantes.

1) Linearidade local (aproximação de primeira ordem)

O EKF assume que (f) e (h) são bem aproximadas por uma função linear perto da média atual. Se:

  • o posterior é amplo (alta incerteza),
  • o sistema é altamente não linear,
  • ou a estimativa está longe do estado verdadeiro,

então o erro de linearização pode ser grande.

2) Crença Gaussiana aproximada

Mesmo que (w) e (v) sejam Gaussianos, transformações não lineares em geral produzem distribuições não Gaussianas. O EKF força a crença de volta para uma Gaussiana a cada passo ao casar média e covariância.

Isso pode ser enganoso para:

  • posteriores multimodais (por exemplo, associação de dados ambígua),
  • ruído com caudas pesadas (outliers),
  • modelos de medição descontínuos.

3) Divergência e inconsistência

Um EKF pode divergir (diverge) (a estimativa se afasta da realidade) quando:

  • jacobianos estão incorretos,
  • (Q) ou (R) estão severamente mal ajustados,
  • existem vieses não modelados (por exemplo, viés de IMU não incluído no estado),
  • o ponto de linearização é ruim.

Ele também pode ficar excessivamente confiante (overconfident) (covariância pequena demais), o que é perigoso em robótica porque faz o filtro subponderar novas evidências e “congelar” crenças erradas.

Mitigações comuns:

  • Aumentar (Q) (admitir mais incerteza de movimento) ou (R) (reduzir o peso das medições)
  • Usar gateamento (gating) (rejeitar medições outliers) via estatísticas da inovação
  • Usar EKF iterado (Iterated EKF) (relinearizar a atualização de medição em torno da média atualizada)
  • Trocar para filtros alternativos (por exemplo, Filtro de Partículas) quando os posteriores são não Gaussianos

4) Sensibilidade à inicialização

O desempenho do EKF depende fortemente da média/covariância iniciais. Uma inicialização ruim pode colocar o filtro em uma região onde a linearização é inválida.

5) Problemas de observabilidade

Alguns estados não podem ser inferidos a partir dos sensores e movimentos disponíveis (por exemplo, posição absoluta apenas com IMU). EKFs podem se comportar mal se o sistema for pouco observável, especialmente se o modelo assumir incorretamente observabilidade.

Notas práticas de implementação (o que importa em sistemas reais)

Cálculo de jacobianos

Você pode obter jacobianos por:

  • Derivação analítica (analytical derivation) (rápida, exata, mas propensa a erros na implementação)
  • Diferenciação automática (automatic differentiation) (comum em ferramentas modernas)
  • Diferenças finitas numéricas (numerical finite differences) (fácil, mas pode ser ruidoso/instável)

Em robótica de segurança crítica, é comum verificar jacobianos com checagens por diferenças finitas em testes mesmo usando expressões analíticas.

Ajuste de ruído (\(Q\) e \(R\))

  • (Q) reflete incerteza na dinâmica: derrapagem de rodas, aceleração não modelada, erros de atuadores.
  • (R) reflete ruído do sensor: precisão do GPS, ruído de alcance do LiDAR, ruído de reprojeção em visão.

O ajuste costuma ser iterativo:

  • Se as estimativas ficam atrasadas em relação às medições, (R) pode estar grande demais (ou (Q) pequeno demais).
  • Se as estimativas ficam ruidosas ou “tremidas”, (R) pode estar pequeno demais (ou (Q) grande demais).

Estabilidade numérica

Práticas comuns:

  • Preferir a atualização de covariância de Joseph em produção
  • Impor simetria: (P \leftarrow \tfrac{1}{2}(P + P^\top))
  • Considerar variantes de EKF de raiz quadrada (square-root EKF) para melhor condicionamento (propagar fatores de Cholesky em vez de (P))

Tratamento de ângulos e variedades

Estados robóticos frequentemente incluem ângulos ou rotações 3D (SO(2), SO(3)). Aplicar EKF ingenuamente em coordenadas Euclidianas pode falhar devido a wrap-around ou geometria não Euclidiana.

No mínimo:

  • Ajuste (wrapping) de ângulos em resíduos e no estado Para 3D, muitos sistemas usam EKFs de “estado de erro” (error-state EKF) (comum em navegação inercial), onde o filtro estima pequenos erros em um espaço tangente.

Aplicações comuns em robótica

Localização (localização com EKF)

Na localização com EKF, o mapa é conhecido (por exemplo, um conjunto de marcos ou um mapa prévio), e o EKF estima a pose do robô ao longo do tempo:

  • Predizer a pose usando odometria/IMU: (f(x,u))
  • Corrigir a pose usando observações de características conhecidas: (h(x))

Exemplos:

  • Odometria de rodas + detecções de AprilTag por câmera
  • Integração de IMU + atualizações de posição por GNSS
  • Correspondência de varredura (scan matching) do LiDAR fornecendo restrições de pose como “medições”

Fusão de sensores

EKFs são amplamente usados para fusão multissensor (multi-sensor fusion) porque:

  • sensores diferentes podem ser modelados como diferentes (h_i(x)) com diferentes (R_i)
  • atualizações podem ser aplicadas de forma assíncrona conforme as medições chegam

Pilhas de fusão comuns:

  • IMU + odometria de rodas + GNSS (robôs móveis, veículos)
  • IMU + visão (odometria visual-inercial (visual–inertial odometry); muitos sistemas usam estimadores no estilo EKF)
  • IMU + magnetômetro (estimação de rumo), embora magnetômetros possam ser propensos a outliers em ambientes internos

EKF-SLAM (Localização e Mapeamento Simultâneos)

EKF-SLAM expande o estado para incluir tanto a pose do robô quanto os marcos do mapa: [ x = [x_r,\ m_1,\ m_2,\ \dots,\ m_N] ]

À medida que o robô observa marcos, o EKF:

  • estima as posições dos marcos,
  • mantém correlações entre a pose do robô e os marcos,
  • usa essas correlações para reduzir incerteza quando marcos são reobservados (comportamento do tipo fechamento de loop (loop closure), embora EKF-SLAM não seja o mesmo que o SLAM por grafo (graph SLAM) moderno).

Propriedades-chave:

  • Funciona melhor quando os marcos são razoavelmente bem modelados e a associação de dados é confiável.
  • O custo computacional escala mal: a covariância é ((3+2N)\times(3+2N)) (para marcos 2D), então o EKF-SLAM ingênuo tipicamente é (O(N^2)) por atualização.
  • Em cenários de grande escala, costuma ser substituído por abordagens de otimização de grafo esparso (sparse graph optimization), mas EKF-SLAM continua sendo educacionalmente importante e útil em problemas pequenos.

Para SLAM não Gaussiano/multimodal (por exemplo, associação de dados incerta), abordagens mais próximas de um Filtro de Partículas (por exemplo, métodos Rao–Blackwellizados (Rao–Blackwellized)) frequentemente são mais robustas.

Relação com métodos de filtragem próximos

  • filtro de Kalman: o EKF se reduz ao filtro de Kalman padrão quando (f) e (h) são lineares.
  • Suavizador de Kalman: estende a filtragem para suavização (smoothing) (estimar estados passados dadas medições futuras) em configurações lineares-Gaussianas; suavizadores não lineares frequentemente se baseiam em linearizações do EKF.
  • Filtro de Partículas: substitui a suposição Gaussiana por um conjunto de amostras ponderadas; muitas vezes é melhor para posteriores fortemente não lineares e multimodais, mas geralmente é mais caro computacionalmente.

Quando usar EKF (regra prática)

EKF é uma boa opção quando:

  • as não linearidades são moderadas (a linearização local é razoável),
  • o posterior permanece aproximadamente unimodal,
  • você precisa de um estimador online rápido com baixo uso de memória,
  • você consegue modelar o ruído de forma razoável.

Considere alternativas quando:

  • o posterior é multimodal ou com caudas pesadas,
  • a associação de dados é altamente ambígua,
  • você precisa de melhor acurácia sob transformações não lineares (por exemplo, métodos de ponto sigma (sigma-point methods)),
  • você pode arcar com otimização em lote (batch optimization) em vez de filtragem pura.

Resumo

O Filtro de Kalman Estendido é uma ferramenta fundamental em robótica e IA para estimação bayesiana online de estado (online Bayesian state estimation) sob movimento e sensoriamento não lineares. Seu mecanismo central — linearizar com jacobianos para aplicar recursões Gaussianas de predição–atualização — o torna eficiente e amplamente aplicável, mas também sensível a erros de modelagem, inicialização e não linearidades. Entender o EKF é essencial para construir sistemas práticos em localização, fusão de sensores e EKF-SLAM, e ele fornece um degrau conceitual para estimadores mais avançados dentro de Filtragem Bayesiana.