filtro_kalman/README.md

# 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.

## Explicación del Código

### Librerias

```python 
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

```python
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:
```python
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
```python
dt = 0.1
```
#### Matriz de transición del estado (A)

```python
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)

```python
H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0]])
```
El sensor solo mide x e y.

#### Ruido del proceso y del sensor

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

#### Estado inicial

```python
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
```python
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

```python
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

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

#### Dibujar trayectorias

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

### Bucle principal

#### Lectura de eventos

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

#### Control del vehículo

```python
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
```python
x_true = A @ x_true
```

#### Medición con ruido

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

#### Filtro de Kalman

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

#### Guardar trazo

```python
trail_true.append(...)
trail_meas.append(...)
trail_est.append(...)
```

### Dibujado en pantalla

Se limpia pantalla:

```python 
screen.fill(BLACK)
```
Líneas de trayectoria:

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

Círculo de incertidumbre:

```python
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