Práctica 2 — Filtro de Kalman: odometría y columnas

En la semana 13 el robot le creía por completo al marcador ArUco y la pose saltaba. En esta práctica RHINO mezcla lo que dicen sus encoders con lo que ve, usando el filtro de Kalman extendido (EKF) de la clase. Las marcas son las columnas y postes del museo, que el Lidar reconoce porque son redondos.

Ahora RHINO sí sabe dónde arranca, y conoce el mapa de las marcas: es un problema de seguimiento, no de localización global.

Montaje

Abra worlds/kalman.wbt del simulador de RHINO.
Tenga a mano las diapositivas del bloque Filtro de Kalman (cinco pasos, elipse en 2D, compuerta de validación).
El programa está en controllers/kalman/kalman.py.

Paso a Paso

Paso 1 — Las marcas

Ejecute la simulación y observe la pantalla:

EKF con marcas: líneas verdes a las columnas usadas

Color	Qué es
Azul	La pose y el camino reales
Gris	Solo odometría, como en la semana 12
Verde	La estimación del EKF y su elipse de 3σ
Líneas verdes	Marcas observadas y usadas para corregir
Líneas naranjas	Observaciones rechazadas

La función detectar_marcas (en python/rhino.py) corta la lectura del Lidar donde la distancia salta, ajusta un círculo a cada trozo y se queda con los que son redondos, sueltos y de menos de 45 cm de radio. Entrega la distancia $\rho$ y el ángulo $\alpha$ al centro de cada uno. En el mapa hay 17 marcas.

Preguntas para orientarse:

¿Cuántas marcas ve RHINO a la vez? ¿Hay lugares del recorrido donde no ve ninguna?
¿Qué objetos producen las líneas naranjas? ¿Por qué el filtro hace bien en rechazarlas?

Paso 2 — Predecir con los encoders

En cada paso, la función predecir mueve la pose con la odometría y hace crecer la covarianza con las fórmulas de la diapositiva En 2D: la elipse de incertidumbre:

Q = np.diag([K_ODOM * abs(dsr), K_ODOM * abs(dsl)])
P = Fp @ P @ Fp.T + Frl @ Q @ Frl.T

Fp transforma la incertidumbre que ya había y Frl hace entrar el error de cada rueda. K_ODOM es la constante $k$ de la diapositiva: la varianza que agrega cada metro de una rueda.

Preguntas para orientarse:

Siga el camino gris durante una vuelta. ¿Cuánto se equivoca la odometría? ¿El error es más de posición o de orientación?
Las ruedas del robot simulado tienen radios un poco distintos (como en la semana 12). ¿Hacia qué lado se desvía la odometría?

Paso 3 — Corregir con una columna

Para cada observación $z = (\rho, \alpha)$ , la función corregir hace los pasos 3 a 5 de la diapositiva Kalman en el robot: cinco pasos:

Predice la medida de cada marca del mapa desde la pose estimada: $\hat{z} = (\hat\rho, \hat\alpha)$ , y su Jacobiano $H$ .
Empareja con la marca de menor distancia de Mahalanobis, $d^2 = \nu^T S^{-1} \nu$ , donde $\nu = z - \hat z$ es la innovación y $S = H \Sigma^- H^T + R$ . Se acepta si $d^2 < 9$ .
Corrige: $K = \Sigma^- H^T S^{-1}$ , $\hat x = \hat x^- + K \nu$ , $\Sigma = (I - K H)\, \Sigma^-$ .

Son las mismas fórmulas de 1D de la clase: la división por $P^- + R$ se volvió la inversa de $S$ , y la medida ya no es la pose sino la distancia y el ángulo a la marca. Por eso hace falta $H$ : es el filtro de Kalman extendido.

Preguntas para orientarse:

Con marcas, ¿cuánto se equivoca el EKF? Compare con la odometría al final de una vuelta.
La tercera línea de la pantalla muestra $\sigma_x$ , $\sigma_y$ y $\sigma_\theta$ . ¿Por qué la elipse casi no se ve?
Con ArUco, emparejar era fácil porque cada marcador tiene un ID. Aquí todas las columnas son iguales. ¿Cómo sabe el filtro cuál está viendo?

Paso 4 — Apagar las marcas

Haga clic en la vista 3D y presione M (o use el botón Marcas sí / no de la ventana del robot): RHINO deja de usar las marcas y solo predice. Espere unos 10 segundos y presione M otra vez.

Elipse de incertidumbre grande sin marcas

Preguntas para orientarse:

¿Cómo crece la elipse? ¿Crece igual en todas las direcciones? ¿Hacia dónde se alarga respecto a la dirección de avance, y por qué?
¿La pose real (azul) queda dentro de la elipse?
Al encender las marcas, ¿cuántas lecturas necesita el filtro para volver a la pose real? ¿Qué le pasa a la elipse?

Paso 5 — Cuando Kalman se pierde

Repita el paso 4, pero ahora deje las marcas apagadas durante 40 segundos o más, y vuelva a encenderlas.

El EKF empareja con la columna equivocada

Con una elipse de varios metros, una columna equivocada también puede pasar la compuerta. Si el filtro la acepta, “corrige” hacia el lugar equivocado y desde ahí lo que ve puede seguir confirmándolo: RHINO queda perdido y no se recupera solo. En la imagen, el EKF (verde) cree estar a más de 3 m de la pose real (azul).

Preguntas para orientarse:

¿Por qué la compuerta de Mahalanobis no protegió al filtro esta vez?
Kalman guarda una sola campana de Gauss. ¿Qué haría la localización de Markov o el filtro de partículas de la práctica 1 en la misma situación?
RHINO usaba Markov en lugar de Kalman. Con lo que vio, ¿por qué cree que lo eligieron para un museo lleno de gente?

Paso 6 — Experimentos

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

Parámetro	Valor inicial	Pruebe con
`K_ODOM`	`1e-3`	`1e-4` y `1e-2`
`SIGMA_DIST` y `SIGMA_ANG`	5 cm y 2°	100 veces más pequeños y 100 veces más grandes
`COMPUERTA`	9	`1e9` (aceptar todo)

Preguntas para orientarse:

Con K_ODOM = 1e-4, apague las marcas 20 segundos. ¿La pose real queda dentro de la elipse? ¿Qué significa que el filtro sea “demasiado confiado”?
Con $R$ muy pequeño, ¿a qué programa de la semana 13 se parece? ¿Y con $R$ muy grande, a cuál de la semana 12? Es la pregunta 2 de ¿Qué nos llevamos hoy?
Sin compuerta, ¿qué pasa con las observaciones que antes eran naranjas?