Fusion d'estimations de positions multiples

J'ai un système dans lequel j'ai deux sous-systèmes distincts pour estimer les positions des robots. Le premier sous-système est composé de 3 caméras qui sont utilisées pour détecter les marqueurs que le robot porte et qui produisent 3 estimations de la position et de l'orientation du robot. Le deuxième sous-système est un système qui est situé sur le robot et mesure la vitesse sur les deux points du robot. En intégrant numériquement ces deux, je peux obtenir une estimation de la position et de l'orientation du robot (car je suis en train de suivre deux points à la fois).

Le premier système est moins précis mais le deuxième système dérive. Le premier système donne une sortie environ une fois par seconde tandis que le second donne une sortie beaucoup plus fréquemment (100-200 fois par seconde).

Je suppose qu'il doit y avoir une meilleure approche que de simplement réinitialiser la position avec l'estimation du premier système (car elle n'est pas précise à 100%), mais d'utiliser également la position accumulée à partir du deuxième système de capteur et fusionner cela avec les nouvelles données du premier système. De plus, la question se pose de savoir comment fusionner 3 estimations du premier système? Il doit y avoir un meilleur moyen que la moyenne pure car il pourrait arriver que les deux estimations soient exactement les mêmes et que la troisième soit complètement différente (ce qui signifie qu'elle est probablement plus erronée)?

Avez-vous des algorithmes de fusion à recommander dans un tel système? Je connais le filtre de Kalman, mais j'ai du mal à trouver comment l'utiliser car les deux systèmes produisent des données à des fréquences différentes.

J'espère que la question est suffisamment claire, quelle est la meilleure approche pour fusionner les estimations en une estimation plus correcte et précise?

Merci

Ce que vous décrivez est essentiellement un cas d'école pour l'utilisation d'un filtre de Kalman. Vous avez d'abord besoin d'une étape de prédiction. Supposons que vous prédisiez la pose du robot$(x,y,\theta)$, compte tenu de l'estimation de la pose précédente et de vos mesures de vitesse à haute fréquence $(v,\omega)$, où $v$ est la vitesse linéaire et $\omega$ est la vitesse angulaire.

$P$ est la matrice de covariance 3x3 qui représente l'incertitude de la pose du robot. $Q$ est la covariance de vos entrées (c.-à-d. à quel point ces mesures de vitesse sont-elles bruyantes?) $F$ est le jacobien du modèle de mouvement par rapport à l'état et $G$ est le jacobien par rapport aux entrées, c'est-à-dire

Vous avez maintenant vos mises à jour de correction moins fréquentes, qui mesurent en fait l'état complet, ce qui est assez simple, c'est-à-dire

où $z_k$ est votre mesure (de la caméra) et $R$est la matrice de covariance associée à cette mesure (probablement une matrice diagonale). Cette mesure est comparée à la mesure prévue (qui dans votre cas n'est que la dernière estimation de pose). Dans ce cas simple, le gain de Kalman est la proportion de la covariance de pose actuelle par rapport à la somme de la covariance de pose et de la covariance de mesure.

Pour répondre à votre question sur les différents taux, vous pouvez simplement exécuter votre mise à jour de mouvement à plusieurs reprises jusqu'à ce que votre mise à jour de prédiction arrive. Par exemple, il peut arriver que la mise à jour du mouvement se produise 100 fois avant d'effectuer une correction.

Vous avez également demandé comment gérer trois caméras. Le moyen le plus simple est de simplement les traiter séquentiellement; il suffit d'appliquer trois corrections consécutives. Une autre façon consiste à les empiler et à effectuer une seule mise à jour. Vous devez ajuster l'étape de mise à jour de la correction pour le faire de cette façon.