Práctica 3 — SLAM: RHINO arma el mapa de columnas

En la práctica 2 RHINO conocía dónde estaba cada columna. ¿Y si nadie le da el mapa? En esta práctica RHINO hace EKF-SLAM: estima su pose y la posición de las columnas y postes en el mismo filtro, y vemos qué pasa cuando cierra el lazo.

Montaje

Abra worlds/slam.wbt del simulador de RHINO.
Tenga a mano las diapositivas del bloque SLAM.
El programa está en controllers/slam/slam.py. Usa las mismas funciones predecir y detectar_marcas de la práctica 2.

Paso a Paso

Paso 1 — Un mapa que crece

Ejecute la simulación. El mapa del museo aparece en gris claro solo como referencia: RHINO no lo conoce. Las marcas que va encontrando aparecen en morado, cada una con su elipse de 3σ.

El estado del filtro es el de la diapositiva:

\mathbf{x} = (x,\ y,\ \theta,\ m_{1x},\ m_{1y},\ \dots,\ m_{Nx},\ m_{Ny})

Cada vez que RHINO ve un objeto redondo que no se parece a ninguna marca conocida ( $d^2 > 25$ con todas), lo anota como candidata. Si la ve 5 veces en el mismo sitio, la agrega al estado con la función agregar_marca. La consola avisa cada marca nueva y el tamaño del estado.

RHINO solo usa marcas a menos de 4 m (ALCANCE_MARCAS): así pasa buena parte de la vuelta sin ver las primeras marcas, y su incertidumbre crece hasta que vuelve a ellas.

Preguntas para orientarse:

Con 14 marcas, ¿de qué tamaño es el estado? ¿Y la matriz de covarianza? Compare con el ejemplo de 8 marcadores de la clase.
¿Por qué el programa espera 5 avistamientos antes de agregar una marca?

Paso 2 — Las marcas heredan la incertidumbre

Observe las elipses moradas durante la primera vuelta, y la elipse verde de RHINO.

Elipses grandes justo antes de cerrar el lazo

Preguntas para orientarse:

¿Las primeras marcas tienen elipses más pequeñas o más grandes que las últimas? ¿Por qué?
¿Qué pasa con la elipse de RHINO a lo largo de la vuelta? Lea $\sigma$ del robot en la pantalla.
¿Por qué la elipse de una marca nueva nunca es más pequeña que la del robot que la vio?

Paso 3 — Cerrar el lazo

A los 59 segundos, aproximadamente, RHINO vuelve a ver las primeras marcas. La consola muestra algo como:

¡Cierre del lazo! Marca 1 vista de nuevo después de 52 s: σ del robot 64.1 → 13.4 cm, σ media del mapa 33.5 → 26.2 cm

Elipses pequeñas justo después de cerrar el lazo

Compare esta imagen con la del paso 2: es medio segundo después. Con una sola observación de una marca conocida, se corrigió la pose de RHINO y la de todas las marcas. El error medio de las marcas pasó de unos 15 cm a unos 6 cm.

Preguntas para orientarse:

¿Por qué ver la marca 1 corrige la posición de marcas que RHINO no está viendo? (Pista: busque en slam.py qué partes de P cambia predecir.)
Es la pregunta 3 de ¿Qué nos llevamos hoy?: al volver al primer marcador, el programa corrige la posición de todos. ¿Es un error del programa?
Después del cierre, ¿las elipses del mapa siguen bajando en las vueltas siguientes?

Paso 4 — Experimentos

Los parámetros están al inicio de slam.py. Cambie uno a la vez y reinicie con Ctrl+Shift+T:

Parámetro	Valor inicial	Pruebe con
`ALCANCE_MARCAS`	4 m	10 m
`VISTAS_PARA_AGREGAR`	5	1
`NUEVA`	25	10
`K_ODOM`	`1e-3`	`1e-2`

Preguntas para orientarse:

Con alcance de 10 m, ¿sigue habiendo un cierre de lazo que se note? ¿Por qué?
Con VISTAS_PARA_AGREGAR = 1, ¿aparecen marcas que no son columnas ni postes? ¿Qué las produce?
Con NUEVA = 10, ¿aparecen marcas repetidas en el mismo lugar? ¿Qué es más grave para el mapa: una marca falsa o una repetida?
Con K_ODOM grande, ¿RHINO reconoce las primeras marcas al volver?

Paso 5 — SLAM hoy

Vuelva a la diapositiva SLAM hoy y relacione cada desafío con lo que vio:

Reconocer que volvió: aquí todas las columnas son iguales y solo se distinguen por su posición. ¿Qué pasaría en una sala con columnas en una cuadrícula perfecta?
Entornos dinámicos: agregue personas como en el paso 7 de la práctica 1. ¿El programa las confunde con postes?
Escala: cada observación actualiza una covarianza de $(3 + 2N)^2$ números. ¿Cuántos serían con las 2000 columnas de un centro comercial?