Visualizar y depurar un EKF

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.

Una forma muy informativa de visualizar el efecto de las mediciones (para mí) es trazar el estado del robot (media, con elipse de covarianza) antes y después de cada medición. Luego, tome los componentes individuales de la medición (demora, rango para los marcadores AR) y aplíquelos por separado para tener una idea.

Para hacer esto:

$a,b$$\theta$$[0, 2\pi]$

El seguimiento de la covarianza de la hipótesis anterior, el estado medido y la hipótesis posterior suele ser suficiente para determinar si las ecuaciones de un EKF se aplican correctamente.

Buena suerte y no actualice su pregunta con demasiada frecuencia. En cambio, vuelve con nuevas preguntas.

Algo que a menudo se hace es trazar las variables de estado a lo largo del tiempo, así como sus intervalos de 3 sigmas. Los puntos donde este intervalo se reduce son las actualizaciones, donde posiblemente podría anotar la fuente de las mediciones involucradas.

Además de los errores en la implementación que deben verificarse (no solo las ecuaciones incorrectas, sino también las ecuaciones numéricamente inestables), el efecto de las actualizaciones solo se ve directamente afectado por la diferencia entre lo que se "espera" y "se mide" y sus respectivas incertidumbres. Por lo tanto, es posible que le interese encontrar una forma de visualizar este equilibrio en términos de la progresión temporal en la primera trama.