first commit
Israel Herrera Gonzalez
4 days ago
commit
9c5967435e
@ -0,0 +1,83 @@
|
||||
# Juego de utilizando filtro de kalman
|
||||
|
||||
Este proyecto implementa una simulación visual del Filtro de Kalman aplicado al seguimiento de un vehículo en 2D.
|
||||
El usuario puede controlar la “posición real” del carro con las flechas del teclado, mientras el sistema:
|
||||
|
||||
- Genera mediciones ruidosas (sensores).
|
||||
- Predice la siguiente posición usando el modelo dinámico.
|
||||
- Corrige la estimación aplicando el Filtro de Kalman.
|
||||
|
||||
En el juego podemos visualizar:
|
||||
|
||||
- Trayectoria real (verde)
|
||||
- Mediciones ruidosas (rojo)
|
||||
- Estimación de Kalman (azul)
|
||||
- Elipse de incertidumbre (radio proporcional a la covarianza)
|
||||
|
||||
## Objetivo del proyecto
|
||||
Este programa fue creado como demostración educativa para mostrar cómo el Filtro de Kalman:
|
||||
1. Fusiona información del modelo con mediciones ruidosas
|
||||
2. Reduce la incertidumbre del sistema
|
||||
3. Proporciona una estimación más precisa que los datos del sensor
|
||||
|
||||
## Requisitos:
|
||||
- Python 3.8+
|
||||
- Pygame
|
||||
- NumPy
|
||||
|
||||
Instalación rápida:
|
||||
|
||||
```python
|
||||
pip install pygame numpy
|
||||
```
|
||||
|
||||
## Ejecucion
|
||||
|
||||
```python
|
||||
python carKF.py
|
||||
```
|
||||
|
||||
Se controla mediante las flechas del teclado.
|
||||
|
||||
🧠 ¿Cómo funciona el filtro de Kalman en esta simulación?
|
||||
|
||||
El estado del carro se modela como:
|
||||
|
||||
x =
|
||||
\begin{bmatrix}
|
||||
x \\
|
||||
y \\
|
||||
v_x \\
|
||||
v_y
|
||||
\end{bmatrix}
|
||||
|
||||
El modelo dinámico (movimiento con velocidad constante):
|
||||
|
||||
x_{k+1} = A x_k + w_k
|
||||
|
||||
Donde:
|
||||
|
||||
A =
|
||||
\begin{bmatrix}
|
||||
1 & 0 & dt & 0 \\
|
||||
0 & 1 & 0 & dt \\
|
||||
0 & 0 & 1 & 0 \\
|
||||
0 & 0 & 0 & 1
|
||||
\end{bmatrix}
|
||||
|
||||
Las mediciones simuladas solo observan la posición:
|
||||
|
||||
z_k =
|
||||
\begin{bmatrix}
|
||||
x \\
|
||||
y
|
||||
\end{bmatrix}
|
||||
+
|
||||
v_k
|
||||
|
||||
Matrices de ruido:
|
||||
• Ruido del proceso:
|
||||
Q = 0.01 I
|
||||
• Ruido del sensor (muy ruidoso):
|
||||
R = 25 I
|
||||
|
||||
@ -0,0 +1,160 @@
|
||||
import pygame
|
||||
import numpy as np
|
||||
|
||||
# ==============================
|
||||
# SETUP
|
||||
# ==============================
|
||||
pygame.init()
|
||||
WIDTH, HEIGHT = 900, 650
|
||||
screen = pygame.display.set_mode((WIDTH, HEIGHT))
|
||||
pygame.display.set_caption("🚗 Kalman Filter Car Simulation")
|
||||
|
||||
font_small = pygame.font.SysFont("Consolas", 14)
|
||||
font_large = pygame.font.SysFont("Consolas", 20, bold=True)
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
# Colors
|
||||
WHITE = (255, 255, 255)
|
||||
BLACK = (10, 10, 20)
|
||||
GREEN = (80, 255, 80)
|
||||
BLUE = (80, 180, 255)
|
||||
RED = (255, 80, 80)
|
||||
YELLOW = (255, 255, 100)
|
||||
PANEL_BG = (30, 30, 50)
|
||||
GRAY = (150, 150, 180)
|
||||
|
||||
# ==============================
|
||||
# KALMAN FILTER SETUP
|
||||
# ==============================
|
||||
dt = 0.1
|
||||
|
||||
A = np.array([[1, 0, dt, 0],
|
||||
[0, 1, 0, dt],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1]])
|
||||
|
||||
H = np.array([[1, 0, 0, 0],
|
||||
[0, 1, 0, 0]])
|
||||
|
||||
Q = np.eye(4) * 0.01
|
||||
R = np.eye(2) * 25
|
||||
|
||||
x_est = np.array([[WIDTH/2], [HEIGHT/2], [0], [0]])
|
||||
x_true = np.copy(x_est)
|
||||
P = np.eye(4) * 500
|
||||
|
||||
trail_true, trail_meas, trail_est = [], [], []
|
||||
|
||||
# ==============================
|
||||
# FUNCTIONS
|
||||
# ==============================
|
||||
def predict(x, P):
|
||||
x_pred = A @ x
|
||||
P_pred = A @ P @ A.T + Q
|
||||
return x_pred, P_pred
|
||||
|
||||
def update(x_pred, P_pred, z):
|
||||
S = H @ P_pred @ H.T + R
|
||||
K = P_pred @ H.T @ np.linalg.inv(S)
|
||||
y = z - H @ x_pred
|
||||
x_upd = x_pred + K @ y
|
||||
P_upd = (np.eye(4) - K @ H) @ P_pred
|
||||
return x_upd, P_upd
|
||||
|
||||
def draw_text(text, pos, color=WHITE, font=font_small):
|
||||
surf = font.render(text, True, color)
|
||||
screen.blit(surf, pos)
|
||||
|
||||
def draw_trail(trail, color):
|
||||
if len(trail) > 2:
|
||||
pygame.draw.lines(screen, color, False, trail, 2)
|
||||
|
||||
# ==============================
|
||||
# MAIN LOOP
|
||||
# ==============================
|
||||
running = True
|
||||
while running:
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
running = False
|
||||
|
||||
keys = pygame.key.get_pressed()
|
||||
if keys[pygame.K_ESCAPE]:
|
||||
running = False
|
||||
|
||||
# Control car
|
||||
if keys[pygame.K_UP]:
|
||||
x_true[3] -= 2
|
||||
if keys[pygame.K_DOWN]:
|
||||
x_true[3] += 2
|
||||
if keys[pygame.K_LEFT]:
|
||||
x_true[2] -= 2
|
||||
if keys[pygame.K_RIGHT]:
|
||||
x_true[2] += 2
|
||||
|
||||
# True motion
|
||||
x_true = A @ x_true
|
||||
|
||||
# Noisy measurement
|
||||
z = H @ x_true + np.random.multivariate_normal([0, 0], R).reshape(2, 1)
|
||||
|
||||
# Prediction
|
||||
x_pred, P_pred = predict(x_est, P)
|
||||
|
||||
# Update
|
||||
x_est, P = update(x_pred, P_pred, z)
|
||||
|
||||
# Store trails
|
||||
trail_true.append((int(x_true[0, 0]), int(x_true[1, 0])))
|
||||
trail_meas.append((int(z[0, 0]), int(z[1, 0])))
|
||||
trail_est.append((int(x_est[0, 0]), int(x_est[1, 0])))
|
||||
|
||||
if len(trail_true) > 150:
|
||||
trail_true.pop(0)
|
||||
trail_meas.pop(0)
|
||||
trail_est.pop(0)
|
||||
|
||||
# ==============================
|
||||
# DRAW
|
||||
# ==============================
|
||||
screen.fill(BLACK)
|
||||
|
||||
# Draw trails
|
||||
draw_trail(trail_true, (0, 180, 0))
|
||||
draw_trail(trail_meas, (180, 0, 0))
|
||||
draw_trail(trail_est, (0, 100, 255))
|
||||
|
||||
# === UNCERTAINTY CIRCLE (based on covariance P) ===
|
||||
P_pos = P[0:2, 0:2]
|
||||
uncertainty = float(np.sqrt(np.trace(P_pos))) * 0.2 # smooth scaling
|
||||
uncertainty = np.clip(uncertainty, 5, 120)
|
||||
|
||||
# Draw uncertainty circle for current estimate
|
||||
pygame.draw.circle(screen, BLUE,
|
||||
(int(x_est[0, 0]), int(x_est[1, 0])),
|
||||
int(uncertainty), 2)
|
||||
|
||||
# Draw car positions
|
||||
pygame.draw.circle(screen, GREEN, (int(x_true[0, 0]), int(x_true[1, 0])), 7) # True
|
||||
pygame.draw.circle(screen, RED, (int(z[0, 0]), int(z[1, 0])), 5) # Measured
|
||||
pygame.draw.circle(screen, BLUE, (int(x_est[0, 0]), int(x_est[1, 0])), 7, 2) # Estimated
|
||||
|
||||
# Panel
|
||||
pygame.draw.rect(screen, PANEL_BG, (10, 10, 310, 260), border_radius=10)
|
||||
pygame.draw.rect(screen, GRAY, (10, 10, 310, 260), 2, border_radius=10)
|
||||
|
||||
draw_text("KALMAN FILTER STATUS", (30, 20), YELLOW, font_large)
|
||||
draw_text(f"True Pos: ({x_true[0,0]:7.2f}, {x_true[1,0]:7.2f})", (30, 60), GREEN)
|
||||
draw_text(f"Measured Pos: ({z[0,0]:7.2f}, {z[1,0]:7.2f})", (30, 80), RED)
|
||||
draw_text(f"Estimated Pos: ({x_est[0,0]:7.2f}, {x_est[1,0]:7.2f})", (30, 100), BLUE)
|
||||
|
||||
error = np.linalg.norm(x_true[:2] - x_est[:2])
|
||||
draw_text(f"Prediction Error: {error:7.2f} px", (30, 130), YELLOW)
|
||||
draw_text(f"Uncertainty: {uncertainty:7.2f} px", (30, 150), YELLOW)
|
||||
|
||||
draw_text("Controls: ⬆️ ⬇️ ⬅️ ➡️ | ESC to quit", (30, 200), (180, 180, 255))
|
||||
|
||||
pygame.display.flip()
|
||||
clock.tick(30)
|
||||
|
||||
pygame.quit()
|
||||
Loading…
Reference in New Issue