Ultima parte del README

README.md

@@ -39,45 +39,194 @@ python carKF.py
 
 Se controla mediante las flechas del teclado.
 
-🧠 ¿Cómo funciona el filtro de Kalman en esta simulación?
+## Explicación del Código
-
+
-El estado del carro se modela como:
+### Librerias
-
+
-x =
+```python 
-\begin{bmatrix}
+import pygame
-x \\
+import numpy as np
-y \\
+```
-v_x \\
+- pygame: Motor gráfico para dibujar la simulación.
-v_y
+- numpy: Usado para matrices y operaciones del filtro de Kalman.
-\end{bmatrix}
+
-
+### Configuración del entorno
-El modelo dinámico (movimiento con velocidad constante):
+
-
+```python
-x_{k+1} = A x_k + w_k
+pygame.init()
-
+WIDTH, HEIGHT = 900, 650
-Donde:
+screen = pygame.display.set_mode((WIDTH, HEIGHT))
-
+pygame.display.set_caption("Kalman Filter Car Simulation")
-A =
+```
-\begin{bmatrix}
+Se inicializa Pygame, se crea la ventana y se especifican dimensiones.
-1 & 0 & dt & 0 \\
+
-0 & 1 & 0 & dt \\
+### Colores y fuentes
-0 & 0 & 1 & 0  \\
+
-0 & 0 & 0 & 1
+El bloque define colores RGB y fuentes del panel informativo:
-\end{bmatrix}
+```python
-
+WHITE = (255, 255, 255)
-Las mediciones simuladas solo observan la posición:
+BLACK = (10, 10, 20)
-
+GREEN = (80, 255, 80)
-z_k =
+...
-\begin{bmatrix}
+font_small = pygame.font.SysFont("Consolas", 14)
-x \\
+font_large = pygame.font.SysFont("Consolas", 20, bold=True)
-y
+```
-\end{bmatrix}
+
-+
+### Parámetros del Filtro de Kalman
-v_k
+
-
+#### Tiempo de muestreo
-Matrices de ruido:
+```python
-	•	Ruido del proceso:
+dt = 0.1
-Q = 0.01 I
+```
-	•	Ruido del sensor (muy ruidoso):
+#### Matriz de transición del estado (A)
-R = 25 I
+
+```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
+