Arquivo de Python - Página 6 de 31

Filtro de Kalman Estendido – EKF

16/07/202618/06/2026 Por antonino

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.

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()

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.

Filtro de Kalman Linear

16/07/202618/06/2026 Por antonino

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.

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()

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.