Actualmente estoy depurando y ajustando un EKF (filtro extendido de Kalman). La tarea es el rastreo clásico de pose de robot móvil donde los puntos de referencia son marcadores AR.
A veces me sorprende cómo algunas mediciones afectan la estimación. Cuando miro y calculo los números y las matrices involucradas, puedo averiguar cómo se ejecutó el paso de actualización, qué y por qué sucedió exactamente, pero esto es muy tedioso.
Entonces, me pregunto si alguien está usando alguna técnica, truco o visualización inteligente para tener una mejor idea de lo que está sucediendo en el paso de actualización de EKF.
ACTUALIZACIÓN # 1 (será más específico y mostrará la primera aproximación de lo que tengo en mente)
Lo que estoy buscando es una forma de visualizar un paso de actualización de una manera que me dé una idea de cómo cada componente de la medición afecta a cada componente del estado.
Mi primera idea es trazar la medición y su predicción junto con algunos vectores tomados de la matriz K. Los vectores de K representan cómo el vector de innovación (medición - predicción de medición, no trazada) afectará a cada componente del estado.
Actualmente estoy trabajando con un EKF donde el estado es pose 2D (x, y, ángulo) y las medidas también son poses 2D.
En la imagen adjunta (ábrala en una página / pestaña nueva para verla en resolución completa), el vector K (a escala) (1,1: 2) (sintaxis MATLAB para tomar una submatriz de matriz 3x3) debería dar una idea de cómo el componente del estado EKF cambiará con el vector de innovación actual, K (2,1: 2) cómo cambiará el segundo componente de EKF, etc. En este ejemplo, el vector de innovación tiene un componente x relativamente grande y está alineado con vector K (2,1: 2): el segundo componente del estado (coordenada y) cambiará más.
Un problema en este gráfico es que no da una idea de cómo el tercer componente (ángulo) del vector de innovación afecta el estado. El primer componente del estado aumenta un poco, al contrario de lo que indica K (1: 1: 2): el tercer componente de la innovación causa esto, pero actualmente no puedo visualizarlo.
La primera mejora sería visualizar cómo el tercer componente de la innovación afecta al estado. Entonces sería bueno agregar datos de covarianza para tener una idea de cómo se crea la matriz K.
ACTUALIZACIÓN # 2 Ahora la gráfica tiene vectores en el espacio de estados que muestran cómo cada componente de medición cambia la posición. A partir de este gráfico, puedo ver que el tercer componente de la medición cambia más el estado.
