Filtro de Kalman

Visão geral

O filtro de Kalman (Kalman filter) é um algoritmo clássico para estimar o estado oculto de um sistema dinâmico a partir de medições de sensores ruidosas. Ele é mais bem entendido como um estimador bayesiano recursivo de estado (recursive Bayesian state estimator) para sistemas lineares com ruído gaussiano (Gaussian noise). Como ele mantém tanto uma estimativa quanto uma incerteza, é amplamente utilizado em robótica para:

• Fusão de sensores (sensor fusion) (combinando IMU, GPS, odometria de rodas, visão etc.)
• Rastreamento (tracking) (objetos, robôs, pessoas)
• Estimação de estado (state estimation) (posição, velocidade, atitude, vieses)

Seu apelo é que, sob suas suposições, ele é ótimo (erro quadrático médio mínimo (minimum mean-squared error)) e computacionalmente eficiente: cada passo de tempo exige apenas operações matriciais em um estado de tamanho fixo.

Em pipelines modernos de robótica, a filtragem de Kalman frequentemente fica entre percepção e controle, fornecendo uma estimativa de estado limpa para controladores a jusante (incluindo Controle Preditivo por Modelo (Model Predictive Control, MPC)).

Estimação de estado como um problema probabilístico

Robôs raramente observam as grandezas de que realmente precisam para controle — por exemplo, velocidade, orientação, vieses ou a pose completa — diretamente e sem ruído. Em vez disso, eles combinam:

• Um modelo de movimento (física/cinemática): como o estado evolui ao longo do tempo
• Um modelo de medição: como os sensores se relacionam com o estado
• Modelos de incerteza: ruído de processo e ruído do sensor

Isso é um caso especial de filtragem bayesiana recursiva (recursive Bayesian filtering), em que buscamos a distribuição a posteriori (posterior distribution):

[ p(x_k \mid z_{1:k}) ]

a distribuição do estado (x_k) no tempo (k), dadas todas as medições até o tempo (k), (z_{1:k}).

O filtro de Kalman é a solução em forma fechada quando o sistema é linear e o ruído é gaussiano.

Modelo linear-gaussiano em espaço de estados

Um sistema dinâmico linear (em tempo discreto) é tipicamente escrito como:

Dinâmica (modelo de processo)

[ x_k = F_k x_{k-1} + B_k u_k + w_k ]

• (x_k \in \mathbb{R}^n): estado oculto (por exemplo, posição e velocidade)
• (u_k): entrada de controle (opcional; por exemplo, aceleração comandada)
• (F_k): matriz de transição de estado (state transition matrix)
• (B_k): matriz de entrada de controle (control-input matrix)
• (w_k): ruído de processo (process noise), assumido gaussiano:

[ w_k \sim \mathcal{N}(0, Q_k) ]

Medições (modelo de observação)

[ z_k = H_k x_k + v_k ]

• (z_k \in \mathbb{R}^m): medição do sensor (por exemplo, posição do GPS)
• (H_k): matriz de medição (measurement matrix)
• (v_k): ruído de medição (measurement noise), assumido gaussiano:

[ v_k \sim \mathcal{N}(0, R_k) ]

Principais suposições

1. Linearidade: dinâmica e medições são lineares no estado.
2. Ruído gaussiano: (w_k) e (v_k) são gaussianos.
3. Propriedade de Markov (Markov property): (x_k) depende de (x_{k-1}) (e (u_k)), mas não de estados anteriores diretamente.
4. Modelos conhecidos: (F_k, B_k, H_k, Q_k, R_k) são fornecidos ou ajustados.

Sob essas suposições, a distribuição a posteriori (p(x_k \mid z_{1:k})) permanece gaussiana a cada passo, então ela é totalmente representada por:

• Média: (\hat{x}_{k|k})
• Covariância (covariance): (P_{k|k})

As equações do filtro de Kalman (predição e atualização)

Uma iteração do filtro de Kalman tem duas fases:

• Predizer (atualização no tempo): propagar a crença pela dinâmica
• Atualizar (atualização por medição): corrigir a crença usando uma nova medição

A seguir, (\hat{x}{k|k}) significa “estimativa de (x_k) dadas as medições até o tempo (k)”. De forma similar, (\hat{x}{k|k-1}) é a predição antes de ver (z_k).

1) Etapa de predição

Média predita [ \hat{x}{k|k-1} = F_k \hat{x}{k-1|k-1} + B_k u_k ]

Covariância predita [ P_{k|k-1} = F_k P_{k-1|k-1} F_k^\top + Q_k ]

Intuição: a incerteza cresce tanto por conta da incerteza que você já tinha quanto pelo ruído de processo adicional (Q_k).

2) Etapa de atualização

Primeiro, calcule a inovação (innovation) (resíduo de medição (measurement residual)) e sua covariância:

Inovação [ y_k = z_k - H_k \hat{x}_{k|k-1} ]

Covariância da inovação [ S_k = H_k P_{k|k-1} H_k^\top + R_k ]

Então, calcule o ganho de Kalman (Kalman gain):

[ K_k = P_{k|k-1} H_k^\top S_k^{-1} ]

Atualize a estimativa de estado e a covariância:

Média atualizada [ \hat{x}{k|k} = \hat{x}{k|k-1} + K_k y_k ]

Covariância atualizada (forma comum) [ P_{k|k} = (I - K_k H_k), P_{k|k-1} ]

Uma forma numericamente mais segura, frequentemente usada na prática, é a forma de Joseph (Joseph form):

[ P_{k|k} = (I - K_k H_k)P_{k|k-1}(I - K_k H_k)^\top + K_k R_k K_k^\top ]

Intuição: “média ponderada” com pesos sensíveis à incerteza

O filtro de Kalman é frequentemente descrito como um compromisso ótimo entre:

• Predição a partir do modelo de movimento (o que você espera)
• Correção a partir das medições (o que você observa)

O ponto-chave é que a ponderação não é arbitrária; ela depende da incerteza.

Ganha de Kalman como ponderação pela incerteza

• Se sua predição é muito incerta (grande (P_{k|k-1})), o ganho (K_k) aumenta e você confia mais na medição.
• Se a medição é ruidosa (grande (R_k)), o ganho diminui e você confia mais no modelo.

Isso generaliza a ideia 1D de uma média ponderada para o caso multivariado, com estrutura completa de covariância (correlações entre variáveis de estado).

Inovação como “nova informação”

A inovação (y_k) mede surpresa: quão distante a medição está do que você previu. Inovações grandes podem indicar:

• Uma mudança real que o modelo não capturou
• Uma falha temporária do sensor
• (Q_k) ou (R_k) mal ajustados
• Um erro de modelagem (por exemplo, (H_k) incorreto)

Sistemas robóticos frequentemente monitoram estatísticas normalizadas da inovação (normalized innovation statistics) para detecção de falhas (fault detection).

Exemplo prático: rastreamento 1D de velocidade constante

Suponha que você queira rastrear um robô se movendo ao longo de uma linha. Você mede posição com ruído (por exemplo, uma câmera ou um sensor tipo GPS), mas você quer tanto posição quanto velocidade.

Seja o estado:

[ x_k = \begin{bmatrix} p_k \ v_k \end{bmatrix} ]

Assuma velocidade constante em um pequeno passo de tempo (\Delta t):

[ F = \begin{bmatrix} 1 & \Delta t \ 0 & 1 \end{bmatrix} ,\quad H = \begin{bmatrix} 1 & 0 \end{bmatrix} ]

Sem entrada de controle por simplicidade ((u_k = 0)).

Uma implementação simples em estilo Python:

import numpy as np

dt = 0.1
F = np.array([[1.0, dt],
              [0.0, 1.0]])
H = np.array([[1.0, 0.0]])

# Noise covariances (tune these)
Q = np.array([[1e-4, 0.0],
              [0.0, 1e-3]])   # process noise
R = np.array([[1e-2]])        # measurement noise (position sensor variance)

x = np.array([[0.0],          # initial position
              [1.0]])         # initial velocity guess
P = np.eye(2) * 1.0           # initial uncertainty

def step(z, x, P):
    # Predict
    x_pred = F @ x
    P_pred = F @ P @ F.T + Q

    # Update
    y = z - (H @ x_pred)                       # innovation
    S = H @ P_pred @ H.T + R                   # innovation covariance
    K = P_pred @ H.T @ np.linalg.inv(S)        # Kalman gain

    x_new = x_pred + K @ y

    I = np.eye(P.shape[0])
    P_new = (I - K @ H) @ P_pred               # or Joseph form for stability
    return x_new, P_new

# Example measurement (noisy position)
z = np.array([[0.12]])
x, P = step(z, x, P)
print("position, velocity:", x.ravel())

O que você obtém:

• Uma estimativa de posição suavizada (menos ruidosa do que medições brutas)
• Uma estimativa de velocidade inferida a partir de posições sucessivas (mesmo que a velocidade nunca seja medida diretamente)
• Estimativas de incerteza que diminuem quando as medições são informativas e aumentam quando você fica sem medições

Filtragem de Kalman para fusão de sensores em robótica

Robôs frequentemente têm sensores com forças complementares:

• IMU: alta taxa, bom no curto prazo, deriva ao longo do tempo
• GPS: baixa taxa, referência global, ruidoso e pode falhar/interromper
• Odometria de rodas: boa em terreno plano, patina em terreno solto
• Visão: informação rica, pode falhar com pouca textura/iluminação

Um filtro de Kalman (ou suas variantes não lineares) fornece um arcabouço fundamentado para fundir esses fluxos.

Exemplo: IMU + GPS para navegação

Uma configuração comum:

• O estado inclui posição, velocidade e, às vezes, vieses de acelerômetro/giroscópio.
• A IMU é usada na etapa de predição em alta taxa (integrando aceleração/rotação).
• As correções de posição do GPS chegam em baixa taxa e impulsionam a etapa de atualização.

Mesmo que o GPS caia, o filtro continua predizendo (a incerteza cresce) e “volta” quando o GPS retorna.

Na prática, como a dinâmica da IMU é não linear (rotações), os sistemas frequentemente usam o Filtro de Kalman Estendido (Extended Kalman Filter, EKF) ou o Filtro de Kalman Unscented (Unscented Kalman Filter, UKF).

Exemplo: odometria de rodas + visão

Para um robô terrestre:

• Predizer com odometria de rodas (navegação estimada (dead reckoning))
• Atualizar com medições visuais (por exemplo, marcos, restrições de odometria visual)

Isso melhora a robustez: quando as rodas patinam, a visão pode corrigir; quando a visão falha por pouco tempo, as rodas mantêm você em movimento.

Dados assíncronos e ausentes

Sistemas reais frequentemente têm:

• Taxas diferentes de sensores
• Medições atrasadas
• Carimbos de tempo fora de ordem

Estratégias comuns incluem:

• Rodar a predição na taxa mais alta, aplicando atualizações quando as medições chegam
• Armazenar em buffer e fazer suavização de defasagem fixa (fixed-lag smoothing) (relacionada à suavização de Kalman (Kalman smoothing))
• Usar múltiplos modelos de medição (H_k), (R_k) dependendo do tipo de sensor

Considerações práticas de implementação

Inicialização importa

Você deve escolher:

• (\hat{x}_{0|0}): chute inicial do estado
• (P_{0|0}): incerteza inicial

Se você estiver inseguro, inicialize (P_{0|0}) grande para que o filtro esteja disposto a se adaptar rapidamente às medições. Mas torná-lo grande demais pode causar problemas numéricos ou overshoot transitório em alguns sistemas.

Ajuste de \(Q\) e \(R\) (a “parte difícil” mais comum)

• (R): covariância do ruído de medição
  Estime a partir de especificações do sensor ou da variância empírica quando o estado verdadeiro é aproximadamente constante.

• (Q): covariância do ruído de processo
  Codifica dinâmicas não modeladas (por exemplo, mudanças de aceleração em um modelo de velocidade constante, derrapagem de rodas, vento). Se (Q) for pequeno demais, o filtro fica confiante demais e pode divergir quando o modelo está errado.

Regra prática em robótica: se o rastreamento fica atrasado em relação ao movimento real, aumente (Q); se as estimativas ficam ruidosas demais e passam a “perseguir” o ruído das medições, aumente (R).

Dicas de estabilidade numérica

• Prefira resolver sistemas lineares em vez de inversão explícita de matriz para (S_k^{-1}) (por exemplo, fatoração de Cholesky (Cholesky factorization)).
• Garanta simetria: (P = (P + P^\top)/2) ocasionalmente para contrapor deriva de ponto flutuante.
• Use a forma de Joseph na atualização da covariância quando a estabilidade for crítica.
• Considere filtros de raiz quadrada (square-root filters) para aplicações de alta precisão.

Discretizando modelos de tempo contínuo

Muitos modelos físicos são de tempo contínuo (continuous-time):

[ \dot{x}(t) = A x(t) + G w(t) ]

Para usar um filtro de Kalman discreto padrão, você precisa de (F) e (Q) para o passo de tempo (\Delta t). Para (\Delta t) pequeno, aproximações simples podem funcionar, mas implementações robustas usam métodos de exponencial de matriz (matrix exponential) (e uma derivação cuidadosa de (Q) discreto).

Relação com controle (e MPC)

Estimação de estado e controle são fortemente acoplados:

• Controladores precisam do estado (por exemplo, velocidade), que frequentemente não é medido diretamente.
• Estimadores precisam de modelos que frequentemente vêm de dinâmicas orientadas a controle.

Um resultado-chave é o princípio da separação (separation principle): para sistemas lineares com ruído gaussiano, você pode projetar:

• um estimador de estado (filtro de Kalman) e
• um controlador com realimentação de estado (state-feedback controller) (por exemplo, Regulador Linear-Quadrático (Linear Quadratic Regulator, LQR))

de forma independente, e combiná-los produz o controlador ótimo Gaussiano Linear-Quadrático (Linear Quadratic Gaussian, LQG).

Em robótica, o Controle Preditivo por Modelo (Model Predictive Control, MPC) frequentemente consome a estimativa de estado do filtro de Kalman como sua condição inicial a cada ciclo.

Extensões e algoritmos relacionados

O filtro de Kalman padrão se aplica a sistemas linear-gaussianos. Muitos problemas de robótica são não lineares, motivando extensões:

• Filtro de Kalman Estendido (EKF): lineariza dinâmicas/medições não lineares via jacobianos (Jacobians); muito comum em robótica.
• Filtro de Kalman Unscented (UKF): usa pontos sigma (sigma points); frequentemente melhor que o EKF quando a linearização é ruim.
• Suavizador de Kalman (Kalman smoother) (por exemplo, Rauch–Tung–Striebel): usa dados futuros para refinar estados passados (offline ou com defasagem fixa).
• Filtro de informação (Information filter): trabalha com a covariância inversa; útil em alguns contextos de fusão distribuída (distributed fusion).
• Filtro de Partículas (Particle Filter): lida com distribuições a posteriori não gaussianas e multimodais (multimodal), a um custo computacional maior.

Para mapeamento e localização, filtros se conectam a Localização e Mapeamento Simultâneos (Simultaneous Localization and Mapping, SLAM), em que o estado pode incluir tanto a pose do robô quanto características do mapa.

Modos comuns de falha e diagnóstico

Mesmo com as equações corretas, o uso no mundo real pode falhar. Problemas típicos incluem:

Divergência do filtro

A estimativa se afasta e a incerteza pode se tornar irrealisticamente pequena. Causas:

• (Q) pequeno demais (modelo confiante demais)
• Ruído do sensor (R) incorreto
• (F) ou (H) incorretos (incompatibilidade de modelo)
• Vieses não modelados (especialmente em IMUs)

Incerteza inconsistente

Se seu filtro reporta covariância pequena, mas os erros são grandes, ele é inconsistente. Isso é perigoso em robótica de segurança crítica porque pode levar a decisões de planejamento/controle excessivamente confiantes.

Problemas de observabilidade

Se os sensores não fornecem informação suficiente para inferir certos componentes do estado, esses componentes são não observáveis (unobservable) e não serão estimados de forma confiável (a covariância deve refletir isso). Por exemplo, estimar posição absoluta apenas a partir de dados de acelerômetro é fundamentalmente mal condicionado ao longo do tempo sem referências externas.

Ruído não gaussiano e valores atípicos

Sensores reais frequentemente têm valores atípicos (outliers) (por exemplo, multipercurso de GPS, incompatibilidades de visão). Atualizações padrão do filtro de Kalman podem ser sensíveis demais. Mitigações incluem:

• Gating da inovação (innovation gating) (rejeitar atualizações quando (|y_k|) é grande demais em relação a (S_k))
• Modelos robustos de ruído (robust noise models) (aproximar aumentando (R) de forma adaptativa)
• Trocar para filtros robustos ou métodos de partículas quando necessário

Quando usar um filtro de Kalman

Use um filtro de Kalman quando:

• Seu sistema é bem aproximado por dinâmicas lineares (ou pode ser linearizado localmente)
• O ruído é aproximadamente gaussiano (ou pelo menos unimodal e não muito de caudas pesadas (heavy-tailed))
• Você precisa de estimativas em tempo real, com baixa latência, e com incerteza

Se a distribuição a posteriori provavelmente for multimodal (por exemplo, associação de dados (data association) ambígua em localização), considere um Filtro de Partículas ou um método de SLAM projetado para esse regime.

Resumo

O filtro de Kalman é uma ferramenta fundamental em robótica e sistemas de IA que interagem com o mundo físico. Ele fornece:

• Um arcabouço bayesiano fundamentado para estimação recursiva de estado
• Atualizações eficientes em forma fechada para modelos linear-gaussianos
• Uma forma natural de realizar fusão de sensores com ponderação sensível à incerteza

Apesar de sua idade, o filtro de Kalman continua central porque muitos sistemas robóticos ainda dependem de estimação de estado em tempo real — e porque suas extensões (EKF/UKF/suavizadores) cobrem uma grande parte da robótica não linear prática.

Se você quiser, posso adicionar um exemplo resolvido com foco em robótica (por exemplo, pose 2D com velocidade, ou fusão IMU+GPS), incluindo definições explícitas de matrizes para (F, H, Q, R) e diretrizes de ajuste.

---