O que é o filtro de Kalman linear?
O filtro de Kalman linear é a versão original do algoritmo, projetada para sistemas lineares gaussianos. Ele assume que tanto o modelo dinâmico quanto o de observação são funções lineares das variáveis de estado. As equações de transição e medição são matrizes que multiplicam o estado atual. Os ruídos de processo e de medição são gaussianos com média zero e covariâncias conhecidas. Essas suposições garantem que a distribuição de probabilidade do estado permaneça gaussiana. Portanto, o filtro é completamente descrito pela média (estimativa) e covariância. Ele é a solução ótima para o problema de filtragem linear quadrática. Não há necessidade de linearizações ou aproximações adicionais. Isso o torna computacionalmente eficiente e matematicamente tratável.
Características fundamentais do filtro linear
O filtro linear possui três características principais que o distinguem. Primeiro, as matrizes A (transição), H (observação), Q e R são constantes ou conhecidas. Segundo, a predição e a correção são feitas por operações matriciais diretas. Terceiro, o ganho de Kalman é calculado analiticamente a cada passo. O filtro é estável se o sistema for detectável e a matriz A for estável. A covariância P converge para um valor estacionário independente das medições. Isso permite pré-calcular o ganho de Kalman para sistemas invariantes no tempo.
Vantagens e aplicações típicas
A principal vantagem é a exatidão e a velocidade para problemas lineares. Ele é usado em navegação inercial, rastreamento de satélites e economia. Também é aplicado em processamento de sinais e controle ótimo (LQG). Contudo, ele não lida com não-linearidades; para isso, usa-se o EKF ou UKF.
O filtro de Kalman linear é frequentemente chamado de “filtro de Kalman simples”. Ele é a base para a maioria das aplicações de fusão de sensores. A predição usa x̂ₖ₋₁ → x̂ₖ⁻ = A * x̂ₖ₋₁ + B * uₖ (controle opcional). A covariância predita é Pₖ⁻ = A * Pₖ₋₁ * Aᵀ + Q. A correção usa a medição zₖ: y = zₖ – H * x̂ₖ⁻ (inovação). O ganho é K = Pₖ⁻ * Hᵀ * (H * Pₖ⁻ * Hᵀ + R)⁻¹. A estimativa atualizada é x̂ₖ = x̂ₖ⁻ + K * y. A covariância atualizada é Pₖ = (I – K * H) * Pₖ⁻. O filtro é recursivo e não precisa armazenar todo o histórico. A matriz de covariância P representa a incerteza da estimativa. O traço de P é minimizado pelo ganho de Kalman em cada passo. O filtro também pode ser usado para suavização (estimativa offline). O suavizador de Kalman (RTS) processa os dados para frente e depois para trás. Assim, o filtro de Kalman linear é uma joia da engenharia de controle.
Um exemplo clássico é estimar a posição e a velocidade de um veículo com aceleração desconhecida. O estado é [posição, velocidade] e a medição é a posição do GPS. O modelo linear assume aceleração como ruído branco (processo). O filtro linear produz estimativas ótimas mesmo com GPS ruidoso.
Enunciado do exemplo clássico
Implemente o filtro de Kalman linear para um sistema de segunda ordem (posição e velocidade). Modelo: posição(k+1) = posição(k) + velocidade(k)*dt + 0.5*a*dt², com aceleração a como ruído Q. Velocidade(k+1) = velocidade(k) + a*dt. Medimos apenas a posição com ruído R. Use dt=0.05s, Q=0.1 (aceleração aleatória), R=0.5. Gere 100 passos com aceleração real variável (senoidal). Plote a posição real, medições e estimativa, além do erro de estimativa.
|
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 |
import numpy as np import matplotlib.pyplot as plt # Parâmetros dt = 0.05 n = 100 tempos = np.arange(n) * dt # Matrizes do filtro linear A = np.array([[1, dt], [0, 1]]) H = np.array([[1, 0]]) Q = np.array([[0.1 * dt**3 / 3, 0.1 * dt**2 / 2], [0.1 * dt**2 / 2, 0.1 * dt]]) # aceleração aleatória R = np.array([[0.5]]) # ruído de medição # Estado real com aceleração variável (senoidal) x_real = np.zeros((n, 2)) x_real[0] = [0.0, 1.0] # posição inicial 0, velocidade 1 m/s for k in range(1, n): a = 0.5 * np.sin(0.2 * tempos[k]) # aceleração variável # Atualização exata (sem ruído de processo) x_real[k, 0] = x_real[k-1, 0] + x_real[k-1, 1] * dt + 0.5 * a * dt**2 x_real[k, 1] = x_real[k-1, 1] + a * dt # Gerar medições (posição com ruído) np.random.seed(7) medicoes = np.zeros(n) for k in range(n): medicoes[k] = H @ x_real[k] + np.random.normal(0, np.sqrt(R[0,0])) # Filtro de Kalman linear x_hat = np.array([[0.0], [0.0]]) # estimativa inicial P = np.eye(2) * 5.0 # incerteza inicial estimativas = np.zeros((n, 2)) covariancias = np.zeros((n, 2, 2)) erros = np.zeros(n) for k in range(n): # Predição x_pred = A @ x_hat P_pred = A @ P @ A.T + Q # Inovação z = medicoes[k] y = z - H @ x_pred # Ganho de Kalman S = H @ P_pred @ H.T + R K = P_pred @ H.T @ np.linalg.inv(S) # Correção x_hat = x_pred + K @ y P = (np.eye(2) - K @ H) @ P_pred estimativas[k] = x_hat.ravel() covariancias[k] = P erros[k] = x_real[k, 0] - x_hat[0, 0] print(f"Erro RMS da posição: {np.sqrt(np.mean(erros**2)):.4f} m") # Gráficos plt.figure(figsize=(12, 6)) plt.subplot(2, 2, 1) plt.plot(tempos, x_real[:, 0], 'g-', linewidth=2, label='Posição real') plt.scatter(tempos, medicoes, color='red', s=15, alpha=0.5, label='Medições') plt.plot(tempos, estimativas[:, 0], 'b-', linewidth=2, label='Estimativa filtrada') desvio = np.sqrt(covariancias[:, 0, 0]) plt.fill_between(tempos, estimativas[:, 0] - 2*desvio, estimativas[:, 0] + 2*desvio, color='blue', alpha=0.2, label='Intervalo 95%') plt.xlabel('Tempo (s)') plt.ylabel('Posição (m)') plt.title('Filtro Linear - Posição') plt.legend() plt.grid(True) plt.subplot(2, 2, 2) plt.plot(tempos, x_real[:, 1], 'g--', linewidth=2, label='Velocidade real') plt.plot(tempos, estimativas[:, 1], 'b-', linewidth=2, label='Velocidade estimada') desvio_v = np.sqrt(covariancias[:, 1, 1]) plt.fill_between(tempos, estimativas[:, 1] - 2*desvio_v, estimativas[:, 1] + 2*desvio_v, color='blue', alpha=0.2) plt.xlabel('Tempo (s)') plt.ylabel('Velocidade (m/s)') plt.title('Velocidade Estimada') plt.legend() plt.grid(True) plt.subplot(2, 2, 3) plt.plot(tempos, erros, 'r-', linewidth=1.5) plt.axhline(0, color='black', linestyle='--') plt.fill_between(tempos, -2*desvio, 2*desvio, color='red', alpha=0.1) plt.xlabel('Tempo (s)') plt.ylabel('Erro de posição (m)') plt.title('Erro da Estimativa') plt.grid(True) plt.subplot(2, 2, 4) # Mostrar ganho de Kalman (simula um passo) # Para fins didáticos, mostramos a evolução da covariância P[0,0] plt.plot(tempos, covariancias[:, 0, 0], 'purple', linewidth=2) plt.xlabel('Tempo (s)') plt.ylabel('Variância da posição (m²)') plt.title('Incerteza da Estimativa') plt.grid(True) plt.tight_layout() plt.show() |
Este código implementa o filtro de Kalman linear com aceleração variável. A posição real é suavizada, e a velocidade é estimada indiretamente. O intervalo de confiança reflete a incerteza propagada pelo filtro. O erro da posição permanece dentro do envelope de 2σ na maioria do tempo. Para iniciantes, este exemplo mostra a eficácia do filtro linear. O filtro de Kalman linear é, portanto, uma ferramenta precisa e confiável.