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:

pip install pygame numpy

Ejecucion

python carKF.py

Se controla mediante las flechas del teclado.

Explicación del Código

Librerias

import pygame
import numpy as np

• pygame: Motor gráfico para dibujar la simulación.
• numpy: Usado para matrices y operaciones del filtro de Kalman.

Configuración del entorno

pygame.init()
WIDTH, HEIGHT = 900, 650
screen = pygame.display.set_mode((WIDTH, HEIGHT))
pygame.display.set_caption("Kalman Filter Car Simulation")

Se inicializa Pygame, se crea la ventana y se especifican dimensiones.

Colores y fuentes

El bloque define colores RGB y fuentes del panel informativo:

WHITE = (255, 255, 255)
BLACK = (10, 10, 20)
GREEN = (80, 255, 80)
...
font_small = pygame.font.SysFont("Consolas", 14)
font_large = pygame.font.SysFont("Consolas", 20, bold=True)

Parámetros del Filtro de Kalman

Tiempo de muestreo

dt = 0.1

Matriz de transición del estado (A)

A = np.array([[1, 0, dt, 0],
              [0, 1, 0, dt],
              [0, 0, 1, 0],
              [0, 0, 0, 1]])

Modelo con movimiento constante.

Matriz de observación (H)

H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0]])

El sensor solo mide x e y.

Ruido del proceso y del sensor

Q = np.eye(4) * 0.01
R = np.eye(2) * 25

Estado inicial

x_est = np.array([[WIDTH/2], [HEIGHT/2], [0], [0]])
x_true = np.copy(x_est)
P = np.eye(4) * 500

• Posición inicial en el centro
• Covarianza grande para representar incertidumbre

Funciones del filtro

Predicción

def predict(x, P):
    x_pred = A @ x
    P _pred = A @ P @ A.T + Q
    return x_pred, P_pred

Calcula la evolución del estado y la covarianza.

Actualización

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

• Calcula la ganancia de Kalman
• Ajusta la predicción usando la medición

Funciones gráficas

Texto en pantalla

def draw_text(text, pos, color=WHITE, font=font_small):
    surf = font.render(text, True, color)
    screen.blit(surf, pos)

Dibujar trayectorias

def draw_trail(trail, color):
    if len(trail) > 2:
        pygame.draw.lines(screen, color, False, trail, 2)

Bucle principal

Lectura de eventos

for event in pygame.event.get():
    if event.type == pygame.QUIT:
        running = False

Control del vehículo

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

Modifica la velocidad.

Actualización del movimiento real

x_true = A @ x_true

Medición con ruido

z = H @ x_true + np.random.multivariate_normal([0,0], R).reshape(2,1)

Filtro de Kalman

x_pred, P_pred = predict(x_est, P)
x_est, P = update(x_pred, P_pred, z)

Guardar trazo

trail_true.append(...)
trail_meas.append(...)
trail_est.append(...)

Dibujado en pantalla

Se limpia pantalla:

screen.fill(BLACK)

Líneas de trayectoria:

draw_trail(trail_true, green)
draw_trail(trail_meas, red)
draw_trail(trail_est, blue)

Círculo de incertidumbre:

P_pos = P[0:2,0:2]
uncertainty = sqrt(trace(P_pos)) * 0.2
pygame.draw.circle(screen, BLUE, pos, radius, 2)

Objetos:

• Carro real → verde
• Medición → rojo
• Estimación → azul