Suavizador de Kalman

Visão geral

Um suavizador de Kalman (Kalman smoother) é o contraparte “fora de linha (offline)” do Filtro de Kalman (Kalman Filter). Enquanto um Filtro de Kalman estima o estado atual usando medições até o instante atual, um suavizador estima estados em todos os passos de tempo usando tanto medições passadas quanto futuras. Isso torna a suavização (smoothing) especialmente valiosa quando você pode processar um conjunto de dados (dataset) depois que ele foi coletado — algo comum em registros (logs) de robótica, pipelines de mapeamento e back-ends de SLAM (SLAM back-ends).

Em um modelo linear gaussiano em espaço de estados (linear Gaussian state-space model), a suavização de Kalman calcula a distribuição a posteriori (posterior distribution):

[ p(x_t \mid y_{1:T}) ]

para todo tempo (t), onde (x_t) é o estado latente (latent state) e (y_{1:T}) são todas as medições do tempo 1 até (T). Em contraste, a filtragem (filtering) calcula:

[ p(x_t \mid y_{1:t}) ]

A suavização geralmente reduz ruído, resolve atraso (lag) e corrige erros anteriores ao “revisitar” estimativas de estado passadas quando novas evidências chegam.

Modelo linear gaussiano em espaço de estados (LGSSM)

Suavizadores de Kalman assumem o sistema dinâmico linear padrão:

Dinâmica (modelo de processo): [ x_t = F_t x_{t-1} + B_t u_t + w_t,\quad w_t \sim \mathcal{N}(0, Q_t) ]

Medições (modelo de observação): [ y_t = H_t x_t + v_t,\quad v_t \sim \mathcal{N}(0, R_t) ]

Onde:

• (x_t \in \mathbb{R}^n): estado oculto (por exemplo, posição/velocidade)
• (y_t \in \mathbb{R}^m): medição (por exemplo, GPS, observação de marco)
• (F_t): matriz de transição de estado
• (H_t): matriz de medição
• (Q_t): covariância do ruído de processo
• (R_t): covariância do ruído de medição
• (u_t): entrada de controle conhecida (opcional)

Como tudo é linear e gaussiano, as posteriores relevantes também são gaussianas. Portanto, um suavizador retorna, para cada (t):

• Média suavizada (\hat{x}{t\mid T} = \mathbb{E}[x_t \mid y{1:T}])
• Covariância suavizada (P_{t\mid T} = \mathrm{Cov}(x_t \mid y_{1:T}))

Alguns suavizadores também calculam covariâncias cruzadas (cross-covariances) (P_{t,t-1\mid T}), úteis para aprender parâmetros do sistema via maximização de expectativa (Expectation-Maximization, EM) ou para certos otimizadores de back-end (back-end optimizers).

Filtragem vs suavização (intuição e consequências)

Filtragem: causal, em tempo real

A filtragem é em linha (online) e causal (causal):

• Usa apenas (y_{1:t})
• Projetada para rastreamento (tracking) e controle com baixa latência
• Não consegue “corrigir o passado”

Isso é ideal para loops de controle robótico em tempo real.

Suavização: acausal, em lote ou com atraso

A suavização é fora de linha ou com atraso:

• Usa todas as medições (y_{1:T})
• Produz uma estimativa de trajetória globalmente consistente
• Pode reduzir retrospectivamente deriva (drift) e ruído de medição

Isso é ideal para:

• Pós-processamento de dados de sensores registrados
• Mapeamento e back-ends de SLAM
• Estimativa de trajetória de alta qualidade para avaliação e geração de dados de treinamento

Uma intuição concreta: se uma medição de GPS no tempo (t=100) indica que você provavelmente estava 5 metros à direita, a suavização pode ajustar não apenas (x_{100}), mas também estados anteriores (x_{90}, x_{80}, \dots) de forma estatisticamente consistente.

Formulações comuns de suavização de Kalman

A suavização de Kalman aparece em algumas variantes padrão, dependendo de que parte da trajetória você quer refinar.

Suavização de intervalo fixo (fixed-interval smoothing) (mais comum)

Estime todos os estados em uma janela completa (t=1\dots T): (p(x_t\mid y_{1:T})).
O suavizador Rauch–Tung–Striebel (RTS) é o algoritmo clássico aqui.

Suavização de atraso fixo (fixed-lag smoothing)

Estime (x_{t-L}) usando medições até o tempo (t): (p(x_{t-L}\mid y_{1:t})).
Isso troca um pequeno atraso (lag (L)) por melhor acurácia, ainda operando de forma quase em linha.

Suavização de ponto fixo (fixed-point smoothing)

Estime um estado específico (x_\tau) (um “ponto”) usando todas as medições (y_{1:T}), frequentemente usado em diagnósticos ou quando apenas um instante particular importa.

Suavizador Rauch–Tung–Striebel (RTS)

O suavizador RTS é amplamente usado porque é simples, eficiente e construído diretamente sobre o Filtro de Kalman.

Ideia-chave: filtro para frente + passagem para trás

1. Passagem para frente (forward pass): execute um Filtro de Kalman padrão em (t=1\dots T), produzindo:

   • Média/covariância filtradas: (\hat{x}{t\mid t}, P{t\mid t})
   • Média/covariância previstas: (\hat{x}{t+1\mid t}, P{t+1\mid t})

2. Passagem para trás (backward pass): refine cada estado usando informação do futuro.

Equações do RTS

Defina o ganho do suavizador (smoother gain): [ G_t = P_{t\mid t} F_{t+1}^\top (P_{t+1\mid t})^{-1} ]

Então a média e a covariância suavizadas são calculadas de trás para frente:

[ \hat{x}{t\mid T} = \hat{x}{t\mid t} + G_t\left(\hat{x}{t+1\mid T} - \hat{x}{t+1\mid t}\right) ]

[ P_{t\mid T} = P_{t\mid t} + G_t\left(P_{t+1\mid T} - P_{t+1\mid t}\right)G_t^\top ]

Inicialização no tempo final: [ \hat{x}{T\mid T} = \hat{x}{T\mid T}, \quad P_{T\mid T} = P_{T\mid T} ]

Por que funciona (visão de alto nível)

A filtragem fornece a melhor estimativa usando dados passados. A passagem para trás adiciona uma correção proporcional ao quanto o estado “suavizado pelo futuro” (x_{t+1}) discorda da previsão de um passo a partir do tempo (t). O ganho do suavizador (G_t) determina quanta informação deve fluir para trás através da dinâmica.

Complexidade

Para dimensão de estado (n) e horizonte (T), o RTS é tipicamente:

• Tempo: (O(T \cdot n^3)) devido a inversões/resoluções de matrizes
• Memória: (O(T \cdot n^2)) para armazenar covariâncias filtradas e previstas

Na prática, você evita inversões explícitas de matrizes resolvendo sistemas lineares (por exemplo, resoluções baseadas em Cholesky) para estabilidade numérica.

Exemplo prático: suavizando uma trajetória 1D com velocidade constante

Imagine rastrear um robô em uma linha com estado (x_t = [p_t, v_t]^\top) (posição e velocidade). Você observa medições ruidosas de posição (y_t \approx p_t).

Dinâmica com passo de tempo (\Delta t): [ F = \begin{bmatrix} 1 & \Delta t\ 0 & 1 \end{bmatrix},\quad H = \begin{bmatrix} 1 & 0 \end{bmatrix} ]

A filtragem estimará (p_t, v_t) em linha, mas, se as medições forem ruidosas, a trajetória filtrada pode tremer. Um suavizador RTS vai “tirar a média” dessa tremulação usando medições futuras para inferir melhor velocidades e posições passadas.

Esboço mínimo em estilo Python (RTS sobre Filtro de Kalman)

import numpy as np

def rts_smoother(x_filt, P_filt, x_pred, P_pred, F_list):
    """
    RTS fixed-interval smoother.

    Inputs (lists length T):
      x_filt[t] = x_{t|t} (n,)
      P_filt[t] = P_{t|t} (n,n)
      x_pred[t] = x_{t|t-1} (n,)  (with x_pred[0] unused or prior)
      P_pred[t] = P_{t|t-1} (n,n)
      F_list[t] = F_t mapping x_{t-1} -> x_t  (so use F_list[t] for step t)

    Returns:
      x_smooth[t] = x_{t|T}
      P_smooth[t] = P_{t|T}
    """
    T = len(x_filt)
    n = x_filt[0].shape[0]

    x_smooth = [None] * T
    P_smooth = [None] * T

    x_smooth[-1] = x_filt[-1]
    P_smooth[-1] = P_filt[-1]

    for t in range(T - 2, -1, -1):
        F_next = F_list[t + 1]
        # G_t = P_{t|t} F_{t+1}^T (P_{t+1|t})^{-1}
        # Prefer solve over explicit inverse:
        G = P_filt[t] @ F_next.T @ np.linalg.inv(P_pred[t + 1])

        x_smooth[t] = x_filt[t] + G @ (x_smooth[t + 1] - x_pred[t + 1])
        P_smooth[t] = P_filt[t] + G @ (P_smooth[t + 1] - P_pred[t + 1]) @ G.T

    return x_smooth, P_smooth

Em código de produção, substitua inv por uma resolução linear (por exemplo, scipy.linalg.cho_factor/cho_solve) para melhorar a estabilidade.

Suavizadores de dois filtros e outras variantes

Embora o RTS seja o padrão em muitas aplicações, ele não é o único suavizador.

Suavizador de dois filtros (e.g., Bryson–Frazier)

Essa abordagem calcula:

• Um filtro para frente (forward filter) de (1 \to T)
• Um filtro de informação (information filter) para trás de (T \to 1)

Então, combina ambos para obter a posterior suavizada. Métodos de dois filtros podem ser atraentes em certos cenários numéricos ou quando você já trabalha na forma de informação (information form), mas o RTS é mais comumente ensinado e implementado.

Suavizadores de raiz quadrada (square-root smoothers)

Implementações de raiz quadrada (fator de Cholesky) propagam fatores de matriz em vez de covariâncias diretamente, melhorando a estabilidade numérica — importante em problemas de alta dimensionalidade ou mal condicionados.

Suavizadores não lineares

Se a dinâmica ou as medições forem não lineares, você tipicamente usa extensões:

• Suavizadores baseados em EKF (RTS aplicado a linearizações), relacionados ao Filtro de Kalman Estendido (Extended Kalman Filter, EKF)
• Suavizadores unscented (UKS)
• Suavizadores baseados em otimização (linearização iterada), comuns em back-ends de SLAM

Para problemas fortemente não gaussianos, considere suavizadores por partículas relacionados ao Filtro de Partículas (Particle Filter).

Suavização de Kalman em rastreamento e fusão de sensores

Rastreamento (refinamento fora de linha)

Em rastreamento por radar/visão, a suavização é usada para gerar trajetórias de alta qualidade a partir de sequências gravadas:

• Remover tremulação de detecções ruidosas
• Recuperar velocidade/aceleração com mais precisão
• Melhorar detecção de eventos (por exemplo, detectar paradas/curvas)

Um fluxo de trabalho comum:

1. Execute um filtro em linha para necessidades em tempo real.
2. Armazene os resultados do filtro.
3. Execute um suavizador fora de linha para gerar trilhas “finais” para relatórios ou para treinar modelos a jusante.

Fusão de sensores: IMU + GPS (conceitual)

Considere um veículo com:

• IMU de alta taxa (boas restrições de movimento no curto prazo, com deriva)
• GPS de baixa taxa (posição absoluta, ruidoso)

A filtragem combina isso em linha. A suavização melhora a trajetória completa ao permitir que correções de GPS posteriores corrijam a deriva anterior de modo estatisticamente consistente. Isso é particularmente eficaz quando o GPS é intermitente: futuras reaquisições podem “puxar” toda a trajetória de volta ao alinhamento.

Como a suavização difere da filtragem em back-ends de SLAM

Em SLAM, você frequentemente ouve “SLAM baseado em filtro” vs “SLAM baseado em suavização”.

SLAM baseado em filtro (estado em linha)

Um filtro mantém uma crença sobre o estado atual (e às vezes um mapa) e atualiza conforme novas medições chegam. Isso está conceitualmente alinhado com (p(x_t \mid y_{1:t})).

SLAM baseado em suavização (otimização de trajetória + mapa)

Back-ends modernos de SLAM frequentemente resolvem um problema de estimação em lote (batch) ou em janela deslizante (sliding-window) sobre muitos estados de uma vez — conceitualmente alinhado com a suavização (p(x_{1:T} \mid y_{1:T})) ou com a estimação MAP (MAP estimation) de toda a trajetória.

Isso está intimamente conectado a:

• Formulações por grafo de fatores (factor graph)
• Solucionadores de mínimos quadrados esparsos (sparse least-squares solvers)
• Otimização no estilo “ajuste de feixe (bundle adjustment)” em SLAM visual

Se você quiser um enquadramento probabilístico: a suavização de Kalman é a solução gaussiana em forma fechada para um grafo de fatores com estrutura em cadeia. Back-ends de SLAM generalizam essa ideia para modelos não lineares e estruturas de grafo mais complexas. Veja Localização e Mapeamento Simultâneos (SLAM) e Grafos de Fatores.

Janela deslizante = suavização de atraso fixo na prática

Muitos sistemas de SLAM em tempo real não suavizam todo o histórico (caro demais). Em vez disso, eles usam um suavizador de atraso fixo:

• Mantêm uma janela das (N) poses mais recentes
• Reotimizam essas poses conforme novas restrições entram
• Marginalizam estados mais antigos para manter o custo computacional limitado

Isso é essencialmente o análogo não linear, baseado em grafos, da suavização de Kalman com atraso fixo.

Quando (e quando não) usar um suavizador de Kalman

Boa adequação

Use suavização de Kalman quando:

• O modelo é linear (ou bem aproximado como linear)
• O ruído é aproximadamente gaussiano
• Você quer trajetórias de melhor estimativa (pós-processamento, mapeamento, analytics)
• A latência é aceitável (fora de linha ou com atraso)

Limitações e modos de falha

• Não linearidade: o RTS assume dinâmica/medições lineares. Suavizadores baseados em linearização podem ter dificuldades com não linearidade forte ou más suposições iniciais.
• Ruído não gaussiano / outliers: algumas medições ruins podem distorcer toda a trajetória suavizada. Alternativas robustas incluem modelos de medição com caudas pesadas ou rejeição de outliers.
• Incompatibilidade de modelo: (Q) e (R) incorretos podem levar a super-suavização (confiança excessiva) ou sub-suavização.

Dicas práticas para implementação

• Evite inversões explícitas: compute (G_t) via resolução de sistemas lineares para estabilidade numérica.
• Armazene o que você precisa: o RTS requer covariâncias filtradas e previstas (e tipicamente médias) em cada passo de tempo.
• Ajuste (Q) e (R): a qualidade da suavização depende fortemente de modelos de ruído corretos. Se (Q) for pequeno demais, a trajetória fica irrealisticamente “rígida”.
• Considere formas de raiz quadrada em alta dimensionalidade ou quando as covariâncias se tornam mal condicionadas.
• Use suavização de atraso fixo para sistemas em tempo real que precisam de acurácia melhor do que a de um filtro sem o custo total de um lote.

Resumo

Métodos de suavização de Kalman estendem a filtragem de Kalman ao estimar a trajetória inteira do estado usando todas as medições, produzindo (p(x_t\mid y_{1:T})) em vez de (p(x_t\mid y_{1:t})). O suavizador RTS é o suavizador de intervalo fixo mais comum: execute um Filtro de Kalman padrão para frente e, em seguida, faça uma recursão para trás que propaga informação de medições futuras de volta no tempo.

Em robótica, a suavização é central para estimativa de trajetória de alta qualidade e sustenta a intuição por trás de back-ends de SLAM: combinar muitas restrições ao longo do tempo para produzir uma estimativa globalmente consistente. Para sistemas não lineares, a mesma ideia de suavização aparece via suavizadores baseados em EKF/UKF ou por otimização não linear em grafos de fatores.

---