Filtro de Kalman Unscented (UKF)

Visão geral

O Filtro de Kalman Unscented (Unscented Kalman Filter, UKF) é um estimador bayesiano recursivo de estado (recursive Bayesian state estimator) para sistemas dinâmicos não lineares (nonlinear dynamical systems). Assim como o Filtro de Kalman, ele mantém uma crença gaussiana (Gaussian belief) sobre o estado do sistema — representada por um vetor de média e uma matriz de covariância — e atualiza essa crença à medida que novas medições chegam.

O que torna o UKF diferente do Filtro de Kalman Estendido (Extended Kalman Filter, EKF) é como ele lida com a não linearidade:

  • EKF: lineariza os modelos não lineares de dinâmica e medição usando jacobianos (Jacobians) (expansão de Taylor de primeira ordem).
  • UKF: evita a linearização explícita ao propagar um conjunto cuidadosamente escolhido de pontos sigma (sigma points) pelas funções não lineares reais e, em seguida, recalcular a média e a covariância a partir dos pontos transformados.

Essa abordagem se baseia na Transformada Unscented (Unscented Transform, UT), que frequentemente fornece uma propagação de média/covariância mais precisa do que a linearização de primeira ordem — especialmente quando as não linearidades são moderadas a fortes e a distribuição de estado está razoavelmente próxima de Gaussiana.

UKFs são amplamente usados em robótica para fusão de sensores (sensor fusion), rastreamento (tracking) e localização (localization), incluindo fusão IMU+GPS, localização por distância-azimute (range-bearing) e rastreamento de alvos com sensores não lineares.

Configuração do problema: modelo de espaço de estados não linear

O UKF trata sistemas comumente escritos como:

Modelo de processo (movimento) [ \mathbf{x}{k} = f(\mathbf{x}{k-1}, \mathbf{u}{k-1}) + \mathbf{w}{k-1}, ] Modelo de medição [ \mathbf{z}{k} = h(\mathbf{x}{k}) + \mathbf{v}_{k}, ]

onde:

  • (\mathbf{x}_k \in \mathbb{R}^n): estado no tempo (k) (por exemplo, posição, velocidade, orientação)
  • (\mathbf{u}_k): entrada de controle (por exemplo, comandos das rodas)
  • (\mathbf{z}_k \in \mathbb{R}^m): medição (por exemplo, GPS, características de lidar, marcos de câmera)
  • (f(\cdot)): dinâmica não linear
  • (h(\cdot)): função de medição não linear
  • (\mathbf{w}_{k-1} \sim \mathcal{N}(0, Q)): ruído de processo
  • (\mathbf{v}_{k} \sim \mathcal{N}(0, R)): ruído de medição

O UKF mantém:

  • média (\hat{\mathbf{x}}_k)
  • covariância (P_k)

Ideia-chave: a Transformada Unscented (UT)

Dada uma variável aleatória (\mathbf{x} \sim \mathcal{N}(\mu, \Sigma)) e uma função não linear (\mathbf{y} = g(\mathbf{x})), a UT aproxima a distribuição de (\mathbf{y}) por:

  1. Escolher um conjunto determinístico de pontos sigma ({\chi_i}) cuja média/covariância amostrais correspondam a ((\mu, \Sigma)).
  2. Propagar cada ponto pela função não linear: (\gamma_i = g(\chi_i)).
  3. Reconstruir (\mu_y) e (\Sigma_y) via médias ponderadas.

Intuição: em vez de aproximar (g) (como o EKF faz), aproximar a distribuição sob (g) amostrando pontos representativos que capturam a média e a covariância.

Pontos sigma e parâmetros do UKF (\(\alpha, \beta, \kappa\))

Para um estado de dimensão (n), o UKF “básico” usa (2n + 1) pontos sigma:

[ \chi_0 = \mu ] [ \chi_i = \mu + \left[\sqrt{(n+\lambda)\Sigma}\right]i,\quad i=1,\dots,n ] [ \chi{i+n} = \mu - \left[\sqrt{(n+\lambda)\Sigma}\right]_i,\quad i=1,\dots,n ]

Aqui (\left[\sqrt{(n+\lambda)\Sigma}\right]_i) denota a (i)-ésima coluna de uma raiz quadrada matricial (comumente via decomposição de Cholesky (Cholesky)).

Parâmetro de escala \(\lambda\)

[ \lambda = \alpha^2 (n+\kappa) - n ]

Pesos para média e covariância

Pesos da média: [ W_0^{(m)} = \frac{\lambda}{n+\lambda},\quad W_i^{(m)} = \frac{1}{2(n+\lambda)} ;; (i=1,\dots,2n) ]

Pesos da covariância: [ W_0^{(c)} = \frac{\lambda}{n+\lambda} + (1-\alpha^2 + \beta),\quad W_i^{(c)} = \frac{1}{2(n+\lambda)} ;; (i=1,\dots,2n) ]

O que \(\alpha, \beta, \kappa\) fazem?

  • (\alpha): controla a dispersão dos pontos sigma em torno da média.
    • Escolhas comuns: (10^{-3}) a (10^{-1}) (dependente do problema).
    • Pequeno demais pode capturar mal a não linearidade; grande demais pode introduzir instabilidade ou superponderar regiões distantes.
  • (\beta): incorpora conhecimento prévio sobre a curtose da distribuição.
    • Para distribuições gaussianas, (\beta = 2) é uma recomendação padrão.
  • (\kappa): parâmetro secundário de escala (frequentemente 0, às vezes (3-n)).
    • Afeta (\lambda) e, portanto, a dispersão dos pontos sigma.

Na prática, um padrão comum é:

  • (\beta = 2)
  • (\kappa = 0)
  • ajustar (\alpha) (e as covariâncias de ruído (Q, R)) para estabilidade e precisão.

Algoritmo do UKF: predição e atualização

O UKF segue o mesmo laço de alto nível da família do filtro de Kalman: Predição → Atualização.

Assuma que temos (\hat{\mathbf{x}}{k-1}, P{k-1}).

1) Etapa de predição (atualização temporal)

(a) Gerar pontos sigma a partir do prior [ {\chi^{x}{i,k-1}}{i=0}^{2n} = \text{SigmaPoints}(\hat{\mathbf{x}}{k-1}, P{k-1}) ]

(b) Propagar pontos sigma pela dinâmica [ \chi^{x}{i,k|k-1} = f(\chi^{x}{i,k-1}, \mathbf{u}_{k-1}) ]

(c) Reconstruir a média prevista [ \hat{\mathbf{x}}{k|k-1} = \sum{i=0}^{2n} W_i^{(m)} \chi^{x}_{i,k|k-1} ]

(d) Reconstruir a covariância prevista [ P_{k|k-1} = \sum_{i=0}^{2n} W_i^{(c)}(\chi^{x}{i,k|k-1} - \hat{\mathbf{x}}{k|k-1})(\chi^{x}{i,k|k-1} - \hat{\mathbf{x}}{k|k-1})^\top + Q ]

Essa forma assume ruído de processo aditivo (additive process noise). Se o ruído entra de modo não aditivo (por exemplo, dentro de (f)), normalmente se usa um UKF de estado aumentado (augmented-state UKF) (coberto abaixo).

2) Etapa de atualização (correção por medição)

(a) Transformar pontos sigma previstos para o espaço de medição [ \chi^{z}{i,k} = h(\chi^{x}{i,k|k-1}) ]

(b) Prever a média da medição [ \hat{\mathbf{z}}{k} = \sum{i=0}^{2n} W_i^{(m)} \chi^{z}_{i,k} ]

(c) Covariância da medição [ S_k = \sum_{i=0}^{2n} W_i^{(c)}(\chi^{z}{i,k} - \hat{\mathbf{z}}{k})(\chi^{z}{i,k} - \hat{\mathbf{z}}{k})^\top + R ]

(d) Covariância cruzada entre estado e medição [ P_{xz} = \sum_{i=0}^{2n} W_i^{(c)}(\chi^{x}{i,k|k-1} - \hat{\mathbf{x}}{k|k-1})(\chi^{z}{i,k} - \hat{\mathbf{z}}{k})^\top ]

(e) Ganho de Kalman [ K_k = P_{xz} S_k^{-1} ]

(f) Atualizar média e covariância [ \hat{\mathbf{x}}{k} = \hat{\mathbf{x}}{k|k-1} + K_k(\mathbf{z}k - \hat{\mathbf{z}}{k}) ] [ P_k = P_{k|k-1} - K_k S_k K_k^\top ]

Pseudocódigo (roteiro de implementação)

Given x, P, Q, R, alpha, beta, kappa

# Precompute weights and lambda
lambda = alpha^2 (n + kappa) - n
Compute Wm[0..2n], Wc[0..2n]

loop over time k:
  # Predict
  X = sigma_points(x, P, lambda)            # shape: (2n+1, n)
  X_pred[i] = f(X[i], u[k-1])               # propagate each sigma point
  x_pred = sum_i Wm[i] * X_pred[i]
  P_pred = sum_i Wc[i] * (X_pred[i]-x_pred)(...)^T + Q

  # Update
  Z_pred[i] = h(X_pred[i])
  z_pred = sum_i Wm[i] * Z_pred[i]
  S = sum_i Wc[i] * (Z_pred[i]-z_pred)(...)^T + R
  Pxz = sum_i Wc[i] * (X_pred[i]-x_pred)(Z_pred[i]-z_pred)^T
  K = Pxz * inv(S)

  x = x_pred + K * (z[k] - z_pred)
  P = P_pred - K * S * K^T

UKF com ruído aditivo vs UKF com ruído aumentado

As equações acima adicionam (Q) e (R) após recomputar as covariâncias, o que é válido quando o ruído é aditivo.

Se o sistema é: [ \mathbf{x}k = f(\mathbf{x}{k-1}, \mathbf{u}{k-1}, \mathbf{w}{k-1}), \quad \mathbf{z}_k = h(\mathbf{x}_k, \mathbf{v}_k) ] então uma abordagem comum é aumentar o estado com variáveis de ruído e gerar pontos sigma no espaço aumentado, de modo que o ruído seja propagado diretamente por (f) e (h).

Isso é mais geral, mas aumenta a dimensionalidade e o custo.

Quando o UKF é preferível ao EKF

UKF e EKF são ambos aproximações da filtragem bayesiana (Bayesian filtering) não linear sob suposições gaussianas (Gaussian assumptions). O UKF costuma ser preferível quando:

  • Jacobianos são difíceis de derivar ou propensos a erro (modelos de sensores complexos, matemática de quatérnios, modelos aprendidos).
  • As não linearidades são fortes o suficiente para que a linearização de primeira ordem cause viés ou inconsistência.
  • Você quer um método de substituição direta (drop-in) para modelos não lineares sem derivadas simbólicas.

Regra prática:

  • Se você consegue derivar jacobianos facilmente e o sistema é levemente não linear, o EKF pode ser eficiente e robusto.
  • Se os jacobianos são complicados, o modelo é mais não linear, ou o EKF é instável/inconsistente, o UKF é uma alternativa forte.

Dito isso, o UKF não é “sempre melhor”:

  • Ele pode ser mais caro computacionalmente do que o EKF (propagando (2n+1) pontos).
  • Ele ainda assume que a crença pode ser resumida adequadamente por média + covariância; para posteriores multimodais (multimodal), um filtro de partículas (particle filter) pode ser mais apropriado.

Usos comuns em robótica

1) Fusão de sensores (IMU + GPS / odometria de rodas)

Robôs frequentemente combinam:

  • dados inerciais de alta taxa (IMU: velocidade angular, aceleração)
  • medições absolutas mais lentas (posição GPS, rumo de magnetômetro)
  • odometria de rodas (estimativas de velocidade)

O modelo de processo pode ser não linear (integração de orientação, compensação de gravidade), e as medições podem ser não lineares (por exemplo, GPS em lat/long convertido para um referencial local). Um UKF pode fundir esses fluxos para produzir estimativas suaves de:

  • posição e velocidade
  • orientação (roll/pitch/yaw ou quatérnio (quaternion))
  • vieses da IMU (estados de viés do giroscópio/acelerômetro)

Nota prática: ao usar quatérnios, você deve renormalizar o quatérnio após as atualizações e ter cuidado ao representar o erro de orientação (muitas implementações usam uma formulação de estado de erro (error-state formulation)).

2) Rastreamento com sensores não lineares (radar, somente azimute, distância-azimute)

Um exemplo clássico de rastreamento:

  • Estado: ([x, y, v_x, v_y])
  • Sensor fornece distância e azimute: [ r = \sqrt{x^2+y^2}, \quad \theta = \text{atan2}(y,x) ] Essa função de medição é não linear e pode causar problemas de linearização no EKF, especialmente perto da origem ou com grande incerteza. O UKF frequentemente apresenta melhor comportamento porque transforma pontos sigma pela verdadeira (h(\cdot)) não linear.

Dica prática: trate o embrulhamento de ângulo (angle wrapping) (por exemplo, (\theta \in [-\pi,\pi))) ao computar a inovação (innovation) ((z - \hat{z})) e ao fazer a média de ângulos dos pontos sigma.

3) Localização (movimento não linear + medições baseadas em mapa)

Na localização de robôs móveis, modelos de movimento (tração diferencial, direção Ackermann) são não lineares, por exemplo: [ x_{k} = x_{k-1} + v \Delta t \cos \psi,\quad y_{k} = y_{k-1} + v \Delta t \sin \psi,\quad \psi_{k} = \psi_{k-1} + \omega \Delta t ] As medições podem ser distâncias/azimutes até marcos, ou observações de pose relativa. O UKF é comumente usado para:

  • estimação local de estado para pilhas de navegação
  • fusão de odometria de rodas + IMU + observações de marcos

Em mapeamento em maior escala, ideias do UKF também aparecem em variantes de SLAM, embora EKF-SLAM e métodos de grafo de fatores sejam mais comuns em sistemas modernos.

Exemplo trabalhado (conceitual): rastreamento distância-azimute

Suponha que você rastreie um alvo móvel em 2D:

Estado [ \mathbf{x} = [x, y, v, \psi]^\top ] onde (v) é a velocidade e (\psi) o rumo.

Modelo de processo (velocidade constante, rumo constante ao longo de (\Delta t)) [ x' = x + v\Delta t \cos\psi,\quad y' = y + v\Delta t \sin\psi,\quad v' = v,\quad \psi' = \psi ] com ruído de processo permitindo mudanças lentas em (v) e (\psi).

Modelo de medição (sensor na origem) [ r = \sqrt{x^2 + y^2},\quad \theta = \text{atan2}(y, x) ]

No UKF:

  • Pontos sigma gerados em torno de ((\hat{x}, P)) representam estados plausíveis do alvo.
  • Cada ponto sigma é levado pelo modelo de movimento para obter pontos sigma previstos.
  • Esses pontos previstos são levados pelo modelo de medição distância-azimute.
  • A média/covariância previstas da medição são computadas a partir dos pontos transformados, o que captura naturalmente a não linearidade em (\sqrt{\cdot}) e (\text{atan2}).

Isso frequentemente produz estatísticas previstas de medição mais precisas do que a linearização do EKF, especialmente com grande incerteza ou não linearidades acentuadas.

Detalhes práticos de implementação e armadilhas

Estabilidade numérica e definitude positiva

Matrizes de covariância devem permanecer simétricas semidefinidas positivas, mas erros numéricos podem quebrar isso.

Técnicas comuns:

  • Simetrizar: (P \leftarrow (P + P^\top)/2)
  • Adicionar perturbação (jitter (jitter)): (P \leftarrow P + \epsilon I)
  • Usar o UKF de Raiz Quadrada (Square-Root UKF, SR-UKF), que propaga diretamente um fator de Cholesky e costuma ser mais estável.

Escolher \(Q\) e \(R\) importa tanto quanto UKF vs EKF

Muitos “problemas de UKF” na verdade são problemas de modelagem de ruído (noise modeling):

  • (Q) subestimado → filtro confiante demais, pode divergir quando a dinâmica não corresponde.
  • (R) subestimado → filtro confia demais nas medições, fica ruidoso ou inconsistente. Uma abordagem prática é começar com especificações razoáveis dos sensores e depois ajustar com base em estatísticas de resíduos (residuals)/inovação.

Estados angulares: embrulhamento e média

Se o estado ou a medição inclui ângulos (yaw, azimute):

  • Normalize ângulos residuais para ([-\pi,\pi))
  • Ao computar um ângulo médio a partir de pontos sigma, use estatística circular: [ \bar{\theta} = \text{atan2}\left(\sum w_i\sin\theta_i,; \sum w_i\cos\theta_i\right) ]

Estados de alta dimensão podem ser caros

O custo do UKF escala com (2n+1) avaliações de função por passo. Para (n) muito grande, você pode preferir:

  • EKF (se os jacobianos forem administráveis)
  • suavização por grafo de fatores (factor-graph smoothing) (em lote ou incremental)
  • filtros de ordem reduzida ou desacoplados

Relação com controle e outros estimadores

Resumo

O Filtro de Kalman Unscented (UKF) é uma extensão não linear poderosa do filtro de Kalman que:

  • representa incerteza com média e covariância,
  • usa pontos sigma e a Transformada Unscented para propagar incerteza por modelos não lineares,
  • evita jacobianos e frequentemente supera o EKF quando as não linearidades são significativas.

Principais conclusões para a prática:

  • Entenda e ajuste (Q) e (R) com cuidado.
  • Use padrões sensatos para (\alpha, \beta, \kappa) (frequentemente (\beta=2, \kappa=0), ajuste (\alpha)).
  • Trate ângulos e quatérnios corretamente.
  • Considere o SR-UKF para estabilidade numérica em pipelines de robótica exigentes.

Em robótica, o UKF é uma escolha comum para fusão de sensores, rastreamento e localização, especialmente quando os modelos de medição são não lineares e a robustez importa mais do que computação mínima.