commit 9c5967435ef7f3b0919b29b72b4fc515282b19e2 Author: Israel Herrera Gonzalez Date: Fri Dec 5 12:09:15 2025 -0600 first commit diff --git a/README.md b/README.md new file mode 100644 index 0000000..3a43518 --- /dev/null +++ b/README.md @@ -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 + diff --git a/carKF.py b/carKF.py new file mode 100644 index 0000000..c4c964c --- /dev/null +++ b/carKF.py @@ -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() \ No newline at end of file