O que é o filtro de Kalman?
O filtro de Kalman é um algoritmo de estimação recursiva que combina previsões e medições ruidosas. Ele foi desenvolvido por Rudolf E. Kálmán na década de 1960 para sistemas lineares gaussianos. O filtro mantém uma estimativa do estado oculto e sua incerteza (covariância). A cada passo, ele faz uma previsão (modelo dinâmico) e uma correção (medição). A correção é ponderada pela confiança em cada fonte: o ganho de Kalman. Esse ganho minimiza o erro quadrático médio da estimativa. O filtro é ótimo para sistemas lineares com ruídos gaussianos brancos. Ele é amplamente usado em navegação, controle, visão computacional e finanças. Sua implementação é eficiente, exigindo apenas operações matriciais.
Características fundamentais
O filtro de Kalman possui três características principais que o definem. Primeiro, ele é recursivo: não precisa de todo o histórico, apenas do estado atual. Segundo, ele fornece uma estimativa de incerteza (matriz de covariância) junto com o estado. Terceiro, ele é ótimo no sentido de mínimos quadrados para sistemas lineares. Ele também pode lidar com medições de diferentes frequências ou com dados faltantes. A predição usa um modelo de transição de estado (matriz A) e controle (B). A correção usa um modelo de observação (matriz H) que relaciona estado e medição.
Vantagens e aplicações típicas
A principal vantagem é a capacidade de fundir dados de múltiplos sensores ruidosos. Ele é usado em GPS, rastreamento de alvos, robótica e economia. Também é aplicado em filtragem de sinais e previsão de séries temporais. Contudo, ele falha em sistemas não-lineares; para esses, usa-se o filtro estendido (EKF) ou unscented (UKF).
O filtro de Kalman é um caso especial de inferência bayesiana em redes gaussianas dinâmicas. A predição é o passo de “propagação” da crença usando o modelo dinâmico. A correção é o passo de “atualização” usando a verossimilhança da medição. O ganho de Kalman K = P_pred * H^T * (H * P_pred * H^T + R)^(-1). Ele equilibra a incerteza da previsão (P_pred) e o ruído da medição (R). Se o ruído de medição é alto, K é pequeno, confiando mais na previsão. Se a previsão é incerta, K é grande, confiando mais na medição. O filtro converge rapidamente e é estável para sistemas detectáveis. A matriz de covariância P evolui e diminui com o tempo, convergindo para um valor estacionário. O filtro também pode estimar parâmetros desconhecidos (filtro de Kalman estendido dual). Ele é a base de sistemas de navegação inercial (INS/GPS) em aviões e carros autônomos. No rastreamento de objetos, ele prevê a posição futura e corrige com detecções visuais. O filtro de Kalman é frequentemente o primeiro algoritmo ensinado em cursos de estimação. Assim, ele é uma ferramenta essencial para engenheiros e cientistas de dados.
Um exemplo clássico é rastrear a posição de um carro em movimento retilíneo uniforme. O estado é [posição, velocidade]. A medição é a posição com ruído. O modelo de transição é posição += velocidade*dt, velocidade constante. O filtro estima a posição suavizada e a velocidade mesmo com medições ruidosas.
Enunciado do exemplo clássico
Implemente o filtro de Kalman para rastrear um objeto em 1D com movimento uniforme. Estado: [posição (m), velocidade (m/s)]. dt = 0.1 s. Modelo dinâmico: posição = posição + velocidade*dt + ruído de processo (q=0.5). Medição: posição observada com ruído gaussiano (r=2.0). Gere 50 medições sintéticas a partir de uma trajetória real (posição inicial 0, vel=3 m/s). Execute o filtro e plote a posição real, as medições e a estimativa filtrada.
|
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 |
import numpy as np import matplotlib.pyplot as plt # Parâmetros do sistema dt = 0.1 q = 0.5 # variância do ruído de processo (aceleração aleatória) r = 2.0 # variância do ruído de medição # Matrizes do filtro de Kalman A = np.array([[1, dt], [0, 1]]) # transição de estado H = np.array([[1, 0]]) # observação (só posição) Q = np.array([[q * dt**3 / 3, q * dt**2 / 2], [q * dt**2 / 2, q * dt]]) # covariância do ruído de processo R = np.array([[r]]) # covariância do ruído de medição # Estado inicial real x0_real = 0.0 v_real = 3.0 n_passos = 50 tempos = np.arange(n_passos) * dt # Gerar trajetória real e medições ruidosas np.random.seed(42) estado_real = np.zeros((n_passos, 2)) estado_real[0] = [x0_real, v_real] for k in range(1, n_passos): # Processo com ruído (aceleração aleatória) ruido_processo = np.array([[0.5 * q * dt**2], [q * dt]]) * np.random.randn() estado_real[k] = A @ estado_real[k-1] + ruido_processo.ravel() # Medições (posição com ruído) medicoes = np.zeros(n_passos) for k in range(n_passos): medicoes[k] = H @ estado_real[k] + np.random.normal(0, np.sqrt(r)) # Inicialização do filtro x_hat = np.array([[0.0], [0.0]]) # estimativa inicial P = np.array([[10, 0], [0, 10]]) # incerteza inicial alta # Armazenar estimativas estimativas = np.zeros((n_passos, 2)) covariancias = np.zeros((n_passos, 2, 2)) for k in range(n_passos): # Passo de predição x_hat_pred = A @ x_hat P_pred = A @ P @ A.T + Q # Passo de correção (se houver medição) # Ganho de Kalman K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R) # Atualizar com a medição z = medicoes[k] y = z - H @ x_hat_pred # inovação x_hat = x_hat_pred + K @ y P = (np.eye(2) - K @ H) @ P_pred estimativas[k] = x_hat.ravel() covariancias[k] = P # Extrair posição e velocidade estimadas pos_est = estimativas[:, 0] vel_est = estimativas[:, 1] pos_real = estado_real[:, 0] vel_real = estado_real[:, 1] # Calcular erro RMS erro_pos = np.sqrt(np.mean((pos_est - pos_real)**2)) print(f"Erro RMS da posição: {erro_pos:.3f} m") # Gráficos plt.figure(figsize=(12, 5)) plt.subplot(1, 2, 1) plt.plot(tempos, pos_real, 'g-', linewidth=2, label='Posição real') plt.scatter(tempos, medicoes, color='red', s=20, alpha=0.6, label='Medições ruidosas') plt.plot(tempos, pos_est, 'b-', linewidth=2, label='Estimativa filtrada (posição)') # Incerteza (2 desvios) desvio = np.sqrt(covariancias[:, 0, 0]) plt.fill_between(tempos, pos_est - 2*desvio, pos_est + 2*desvio, color='blue', alpha=0.2, label='Intervalo 95%') plt.xlabel('Tempo (s)') plt.ylabel('Posição (m)') plt.title('Filtro de Kalman - Rastreamento de Posição') plt.legend() plt.grid(True) plt.subplot(1, 2, 2) plt.plot(tempos, vel_real, 'g--', linewidth=2, label='Velocidade real') plt.plot(tempos, vel_est, 'b-', linewidth=2, label='Velocidade estimada') desvio_vel = np.sqrt(covariancias[:, 1, 1]) plt.fill_between(tempos, vel_est - 2*desvio_vel, vel_est + 2*desvio_vel, color='blue', alpha=0.2, label='Intervalo 95%') plt.xlabel('Tempo (s)') plt.ylabel('Velocidade (m/s)') plt.title('Estimativa da Velocidade') plt.legend() plt.grid(True) plt.tight_layout() plt.show() |
Este código implementa o filtro de Kalman para rastreamento 1D. As medições ruidosas são suavizadas pela estimativa recursiva. O intervalo de confiança (2σ) mostra a incerteza da estimativa. A velocidade é estimada indiretamente, mesmo sem ser medida diretamente. Para iniciantes, este exemplo demonstra o poder da fusão de dados. O filtro de Kalman é, portanto, uma ferramenta fundamental em engenharia de controle.