O que é o filtro de Kalman estendido (EKF)?
O filtro de Kalman estendido (EKF) é uma versão não-linear do filtro de Kalman clássico. Ele lineariza funções não-lineares usando derivadas parciais (matrizes Jacobianas). A cada passo, a transição e a observação são aproximadas por suas tangentes. Essa linearização ocorre em torno da estimativa atual do estado. O EKF é amplamente usado em robótica, navegação e sistemas de localização. Ele permite modelar rotações, ângulos e cinemática não-linear de veículos. Apesar de não ser ótimo, ele funciona bem para não-linearidades moderadas. Para fortes não-linearidades, o UKF ou filtros de partículas são preferíveis.
Características fundamentais do EKF
O EKF possui três características principais que o distinguem do Kalman linear. Primeiro, ele usa Jacobianas da função de transição (F) e observação (H). Segundo, a predição é feita aplicando a função não-linear diretamente ao estado. Terceiro, a covariância é propagada usando as Jacobianas linearizadas. O filtro é sensível a erros de linearização se a não-linearidade for forte. A escolha do ponto de linearização (a estimativa atual) é crucial.
Vantagens e aplicações típicas
A principal vantagem é estender o Kalman para problemas reais não-lineares. Ele é usado em SLAM (localização e mapeamento simultâneos) e navegação de drones. Também é aplicado em rastreamento de alvos com movimento curvilíneo. Contudo, ele pode divergir se a Jacobiana for mal calculada.
O EKF é o padrão industrial para muitos sistemas de navegação. No passo de predição, x̂ₖ⁻ = f(x̂ₖ₋₁, uₖ), onde f é a função não-linear. A matriz Jacobiana Fₖ = ∂f/∂x é calculada no ponto x̂ₖ₋₁. A covariância predita é Pₖ⁻ = Fₖ * Pₖ₋₁ * Fₖᵀ + Q. No passo de correção, a inovação é y = zₖ – h(x̂ₖ⁻). A Jacobiana Hₖ = ∂h/∂x é calculada no ponto x̂ₖ⁻. O ganho é K = Pₖ⁻ * Hₖᵀ * (Hₖ * Pₖ⁻ * Hₖᵀ + R)⁻¹. A atualização é x̂ₖ = x̂ₖ⁻ + K * y e Pₖ = (I – K*Hₖ) * Pₖ⁻. O EKF requer que as funções f e h sejam diferenciáveis. Ele é computacionalmente mais custoso que o linear devido às Jacobianas. O desempenho do EKF depende da qualidade da linearização local. Para melhorar, usa-se o EKF iterativo (IEKF) que repete a correção. Outra variação é o EKF com múltiplos modelos (MMEKF) para saltos. Assim, o EKF é uma ferramenta prática e consolidada em engenharia.
Um exemplo clássico é rastrear um veículo com movimento circular (ângulo e velocidade). A posição (x,y) depende do seno e cosseno do ângulo, funções não-lineares. A medição é a distância e o ângulo de um radar (coordenadas polares). O EKF lineariza essas relações para estimar a posição cartesiana.
Enunciado do exemplo clássico
Implemente o EKF para rastrear um objeto em 2D com movimento de velocidade constante e ângulo variável. Estado: [x, y, vx, vy] (posição e velocidade cartesianas). Transição não-linear: x += vx*dt, y += vy*dt (linear, mas incluímos ruído). Observação não-linear: distância r = sqrt(x²+y²) e ângulo θ = atan2(y,x) medidas com ruído. Gere 60 medições sintéticas de uma trajetória circular (raio 10, velocidade angular 0.1 rad/s). Plote a trajetória real, medições (r,θ convertidos para x,y) e estimativa EKF.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 |
import numpy as np import matplotlib.pyplot as plt # Parâmetros dt = 0.1 n = 60 tempos = np.arange(n) * dt # Estado real: movimento circular (x, y, vx, vy) x_real = np.zeros((n, 4)) x_real[0] = [10.0, 0.0, 0.0, 1.0] # posição (10,0), velocidade (0,1) omega = 0.1 # velocidade angular for k in range(1, n): # Aceleração centrípeta (circular) vx = -omega * x_real[k-1, 1] vy = omega * x_real[k-1, 0] x_real[k, 0] = x_real[k-1, 0] + vx * dt x_real[k, 1] = x_real[k-1, 1] + vy * dt x_real[k, 2] = vx x_real[k, 3] = vy # Gerar medições polares (r, θ) com ruído np.random.seed(8) medicoes = np.zeros((n, 2)) # [r, theta] r_real = np.sqrt(x_real[:,0]**2 + x_real[:,1]**2) theta_real = np.arctan2(x_real[:,1], x_real[:,0]) for k in range(n): r_med = r_real[k] + np.random.normal(0, 0.5) theta_med = theta_real[k] + np.random.normal(0, 0.05) medicoes[k] = [r_med, theta_med] # EKF # Estado: [x, y, vx, vy] x_hat = np.array([[8.0], [1.0], [0.0], [1.0]]) # inicial P = np.eye(4) * 5.0 # Matrizes de ruído Q = np.eye(4) * 0.01 R = np.diag([0.5**2, 0.05**2]) # variâncias do r e theta # Funções não-lineares def transicao(x, dt): # x = [x, y, vx, vy] x_new = x.copy() x_new[0] = x[0] + x[2] * dt x_new[1] = x[1] + x[3] * dt # velocidade constante (sem aceleração) return x_new def Jacobiana_transicao(x, dt): F = np.eye(4) F[0, 2] = dt F[1, 3] = dt return F def observacao(x): # retorna [r, theta] px, py = x[0], x[1] r = np.sqrt(px**2 + py**2) theta = np.arctan2(py, px) return np.array([r, theta]) def Jacobiana_observacao(x): px, py = x[0], x[1] r = np.sqrt(px**2 + py**2) if r < 1e-6: r = 1e-6 H = np.zeros((2, 4)) H[0, 0] = px / r H[0, 1] = py / r H[1, 0] = -py / (r**2) H[1, 1] = px / (r**2) return H estimativas = np.zeros((n, 4)) erros = np.zeros(n) for k in range(n): # Predição x_pred = transicao(x_hat.flatten(), dt).reshape(-1, 1) F = Jacobiana_transicao(x_hat.flatten(), dt) P_pred = F @ P @ F.T + Q # Correção z = medicoes[k] h = observacao(x_pred.flatten()) y = z - h # Normalizar ângulo y[1] = np.arctan2(np.sin(y[1]), np.cos(y[1])) H = Jacobiana_observacao(x_pred.flatten()) S = H @ P_pred @ H.T + R K = P_pred @ H.T @ np.linalg.inv(S) x_hat = x_pred + K @ y.reshape(-1, 1) P = (np.eye(4) - K @ H) @ P_pred estimativas[k] = x_hat.flatten() erros[k] = np.linalg.norm(x_real[k, :2] - x_hat.flatten()[:2]) print(f"Erro RMS da posição: {np.sqrt(np.mean(erros**2)):.3f} m") # Converter medições polares para cartesianas (para plotagem) med_cart_x = medicoes[:,0] * np.cos(medicoes[:,1]) med_cart_y = medicoes[:,0] * np.sin(medicoes[:,1]) # Gráficos plt.figure(figsize=(12, 5)) plt.subplot(1, 2, 1) plt.plot(x_real[:,0], x_real[:,1], 'g-', linewidth=2, label='Trajetória real') plt.scatter(med_cart_x, med_cart_y, color='red', s=15, alpha=0.5, label='Medições polares (convertidas)') plt.plot(estimativas[:,0], estimativas[:,1], 'b-', linewidth=2, label='Estimativa EKF') plt.scatter(x_real[0,0], x_real[0,1], color='black', s=80, marker='o', label='Início') plt.xlabel('x (m)') plt.ylabel('y (m)') plt.title('Trajetória 2D - EKF com Medições Polares') plt.axis('equal') plt.legend() plt.grid(True) plt.subplot(1, 2, 2) plt.plot(tempos, erros, 'r-', linewidth=1.5) plt.xlabel('Tempo (s)') plt.ylabel('Erro de posição (m)') plt.title('Erro da Estimativa ao Longo do Tempo') plt.grid(True) # Mostrar distribuição do erro plt.tight_layout() plt.show() |
Este código implementa o EKF com observação polar não-linear. A trajetória circular é bem reconstruída apesar do ruído nas medições. O erro de posição permanece pequeno, validando o filtro. A Jacobiana da observação é calculada analiticamente para precisão. Para iniciantes, este exemplo mostra como estender o Kalman a problemas reais. O filtro de Kalman estendido é, portanto, uma ferramenta indispensável em robótica.