paso de innovación localización ekf?

Digamos que tenemos un montón de observaciones del sensor y tenemos un mapa en el que podemos obtener las mediciones predichas para puntos de referencia. En la localización de EKF en el paso de corrección, ¿deberíamos comparar cada observación con toda la medición pronosticada ?, , ¿en este caso tenemos dos bucles? ¿O simplemente comparamos cada observación con cada medida pronosticada ?, por lo que en este caso tenemos un bucle. Supongo que el sensor puede dar todas las observaciones para todos los puntos de referencia en cada escaneo. La siguiente imagen muestra el escenario. Ahora, cada vez que ejecuto la localización EKF obtengo y tengo , entonces puedo conseguir$z^{i}$$\hat{z}^{i}$$z^{i}$$\hat{z}^{i}$$z^{i} = \{ z^{1}, z^{2}, z^{3}, z^{4}\}$$m$$\hat{z}^{i} = \{ \hat{z}^{1}, \hat{z}^{2}, \hat{z}^{3}, \hat{z}^{4}\}$ . Para obtener el paso de innovación, esto es lo que hice donde es la innovación. Para cada iteración obtengo cuatro innovaciones. ¿Es esto correcto? Estoy usando EKF-Localization en este libro Probabilistic Robotics página 204.

$$Z^{1} = z^{1} - \hat{z}^{1} \\ Z^{2} = z^{2} - \hat{z}^{2} \\ Z^{3} = z^{3} - \hat{z}^{3} \\ Z^{4} = z^{4} - \hat{z}^{4} \\ $$ $Z$

Sí, esto es correcto, dados dos supuestos:

1. Cada medida es independiente (es decir, la distribución (gaussiana) de observación no está correlacionada con ). Por lo general, esta es una suposición justa (por ejemplo, medir la posición de los puntos de referencia con un escáner láser).$z_i$$z_j$

2. La asociación de datos es conocida. En otras palabras, "acaba de saber" que su primera observación fue de hecho una observación del punto de referencia 1. Por lo tanto, puede calcular la innovación con la observación predicha generada por el punto de referencia 1. Sin saber a qué punto de referencia pertenece la observación es donde está el doble entra el bucle. En ese caso, debe comparar la observación con las observaciones predichas de todos los * otros puntos de referencia, y elegir el que sea más probable **, utilizando una métrica como la distancia de Mahalanobis.

* Probablemente pueda acelerar esto solo comparándolo con puntos de referencia que se estima que están en el campo de visión del sensor.

** Este es solo un método de asociación de datos. Existen otros (por ejemplo, compatibilidad conjunta).