Encadenamiento de filtros de Kalman

Mi equipo está construyendo un robot para navegar de forma autónoma en un entorno al aire libre. Recientemente recibimos un nuevo sensor IMU / GPS integrado que aparentemente hace un filtrado Kalman extendido en el chip. Da velocidades de cabeceo, balanceo y guiñada, norte, este y hacia abajo, y latitud y longitud.

Sin embargo, también tenemos algunos codificadores conectados a nuestras ruedas, que proporcionan velocidades lineales y angulares. Antes de obtener este nuevo sensor IMU / GPS, hicimos nuestro propio EKF para estimar nuestro estado utilizando los codificadores y algunos otros sensores de bajo costo. Queremos usar el filtro en chip de este nuevo sensor, pero también incorporar nuestros codificadores en la mezcla.

¿Hay algún problema con el encadenamiento de los filtros? Lo que quiero decir es que usaríamos la salida del EKF en el chip del sensor IMU / GPS como una actualización de nuestro propio EKF, así como usamos los datos leídos de los codificadores como una actualización de nuestro EKF. Me parece razonable, pero me preguntaba qué se supone que se debe hacer en este caso.

Puede usar el INS / GPS como actualizaciones de la salida de su primer EKF. De hecho, esto no está encadenando, sino simplemente condicionando la estimación basada en la información agregada del INS / GPS.

Supongamos que tenemos las siguientes funciones:

$x_{t+1|t}$ , = EKF_PREDICT ( , , ), para entradas como estado , covarianza y entradas de control (estimadas por odometría) .$P_{t+1|t}$$x_t$$P_t$$u_t$$x$$P$$u_t$

y

$x_{t+1|t+1}$ , = EKF_UPDATE ( , , ). $P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}$

Las estimaciones de los sensores son . Tenemos cosas como:$\hat{x}_{t+1}$

$\hat{x}^{gps}_{t+1} = f(GPS)$

$\hat{x}^{map}_{t+1} = f(map)$

$\hat{x}^{ins}_{t+1} = f(INS)$

etc. para todas las demás formas de estimar el estado del robot. Por lo tanto, ejecutar la función EKF_UPDATE para todos esos sensores es lo suficientemente bueno.

Tu ciclo será algo como esto:

para todos los tiempos$t$

• Sea la estimación actual de odometría / cinemática de pose, y sea ​​el ruido en esa estimación.$u_t$$R_u$

• $x_{t+1|t}$ , = EKF_PREDICT ( , , , )$P_{t+1|t}$$x_t$$P_t$$u_t$$R_u$

• para todos los sensores ,$S$

  • Deje que sea ​​la estimación de la pose de ese sensor, y sea ​​el ruido en esa estimación$\hat{x}^{S}_{t+1}$$R_{S}$

  • $x_{t+1|t+1}$ , = EKF_UPDATE ( , , ). $P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}, R_S$

  • fin para

• fin para

Algunas advertencias son:

• Como estamos utilizando el EKF, no hay garantía de que la estimación sea independiente del pedido de las actualizaciones. Es decir, si hace INS y luego GPS, la estimación resultante puede ser diferente de si actualiza con GPS y luego INS. Esto no suele ser un gran problema, pero el filtro requerirá una afinación significativamente mayor.

• Tenga en cuenta que su INS tiene un sesgo y una deriva, lo que podría afectar su confiabilidad a largo plazo. El GPS puede ayudarte mucho aquí. La mayoría de la literatura estima simultáneamente el sesgo y la deriva en el INS.