Posición múltiple estima fusión

Tengo un sistema en el que tengo dos subsistemas separados para estimar las posiciones del robot. El primer subsistema está compuesto por 3 cámaras que se utilizan para detectar marcadores que lleva el robot y que genera 3 estimaciones de la posición y orientación del robot. El segundo subsistema es un sistema que se encuentra en el robot y mide la velocidad en los dos puntos del robot. Al integrar numéricamente esos dos, puedo obtener una estimación de la posición y orientación del robot (porque estoy rastreando dos puntos a la vez).

El primer sistema es menos preciso pero el segundo sistema se desvía. El primer sistema da salida aproximadamente una vez por segundo, mientras que el segundo da salida con mucha más frecuencia (100-200 veces por segundo).

Supongo que debe haber un mejor enfoque que simplemente restablecer la posición con la estimación del primer sistema (ya que no es 100% precisa), sino también usar la posición acumulada del segundo sistema de sensores y fusionar eso con los nuevos datos del primer sistema Además, hay una pregunta sobre cómo fusionar 3 estimaciones del primer sistema. Debe haber una mejor manera que el promedio puro, ya que podría suceder que las dos estimaciones sean exactamente iguales y que la tercera sea completamente diferente (lo que significa que probablemente esté más equivocado).

¿Tiene algún algoritmo de fusión que recomiende usar en dicho sistema? Sé sobre el filtro de Kalman, pero tengo problemas para descubrir cómo usarlo, ya que los dos sistemas emiten datos a diferentes frecuencias.

Espero que la pregunta sea lo suficientemente clara, ¿cuál es el mejor enfoque para fusionar las estimaciones en una estimación más correcta y precisa?

Gracias

Lo que está describiendo es esencialmente un caso de libro de texto para usar un filtro de Kalman. Primero necesitas un paso de predicción. Supongamos que está prediciendo la pose del robot.$(x,y,\theta)$, dada la estimación de pose anterior y sus mediciones de velocidad de alta frecuencia $(v,\omega)$, dónde $v$ es la velocidad lineal y $\omega$ es la velocidad angular

$P$ es la matriz de covarianza 3x3 que representa la incertidumbre de la pose del robot. $Q$ es la covarianza de sus entradas (es decir, ¿qué tan ruidosas son esas mediciones de velocidad?) $F$ es el jacobiano del modelo de movimiento con respecto al estado y $G$ es el jacobiano con respecto a las entradas, es decir,

Ahora tiene sus actualizaciones de corrección menos frecuentes, que en realidad miden el estado completo, lo que lo hace bastante simple, es decir,

dónde $z_k$ es su medida (de la cámara) y $R$es la matriz de covarianza asociada con esa medida (probablemente una matriz diagonal). Esta medida se compara con la medida predicha (que en su caso es solo la última estimación de pose). En este caso simple, la ganancia de Kalman es la proporción de la covarianza de pose actual en comparación con la suma de la covarianza de pose y la covarianza de medición.

Para responder a su pregunta sobre las diferentes tasas, puede ejecutar su actualización de movimiento repetidamente hasta que llegue su actualización de predicción. Por ejemplo, puede ocurrir que la actualización de movimiento ocurra 100 veces antes de que realice una corrección.

También preguntaste sobre cómo manejar tres cámaras. La forma más fácil es simplemente procesarlos secuencialmente; solo aplica tres correcciones seguidas. Otra forma es apilarlos y realizar una única actualización. Debería ajustar el paso de actualización de corrección para hacerlo de esta manera.