Visualisation et débogage d'un EKF

Je suis actuellement en train de déboguer et de régler un EKF (Extended Kalman Filter). La tâche est le suivi classique de la pose d'un robot mobile où les points de repère sont des marqueurs AR.

Parfois, je suis surpris de voir comment certaines mesures affectent l'estimation. Quand je regarde et calcule les nombres et les matrices impliqués, je peux comprendre comment l'étape de mise à jour a été exécutée, quoi et pourquoi s'est exactement produite, mais c'est très fastidieux.

Je me demande donc si quelqu'un utilise une technique, une astuce ou une visualisation intelligente pour avoir une meilleure idée de ce qui se passe dans l'étape de mise à jour EKF?

MISE À JOUR # 1 (sera plus spécifique et montrera la première approximation de ce que j'ai en tête)

Ce que je recherche, c'est un moyen de visualiser une étape de mise à jour d'une manière qui me donne une idée de la façon dont chaque composant de la mesure affecte chaque composant de l'état.

Ma toute première idée est de tracer la mesure et sa prédiction avec quelques vecteurs tirés de la matrice K. Les vecteurs de K représentent comment le vecteur d'innovation (mesure - prédiction de mesure, non tracé) affectera chaque composante de l'état.

Actuellement, je travaille avec un EKF où l'état est une pose 2D (x, y, angle) et les mesures sont également des poses 2D.

Dans l'image jointe (ouvrez-la dans une nouvelle page / un nouvel onglet pour voir en pleine résolution), le vecteur (mis à l'échelle) K (1,1: 2) (syntaxe MATLAB pour prendre une sous-matrice à partir d'une matrice 3x3) devrait donner une idée de la façon dont le premier la composante de l'état EKF changera avec le vecteur d'innovation actuel, K (2,1: 2) comment la deuxième composante d'EKF changera, etc. Dans cet exemple, le vecteur d'innovation a une composante x relativement grande et il est aligné avec vecteur K (2,1: 2) - la deuxième composante de l'état (coordonnée y) changera le plus.

Un problème dans ce graphique est qu'il ne donne pas une idée de la façon dont la troisième composante (angle) du vecteur d'innovation affecte l'état. Le premier composant de l'état augmente un peu, contrairement à ce que K (1: 1: 2) indique - le troisième composant de l'innovation provoque cela, mais actuellement je ne peux pas le visualiser.

La première amélioration consisterait à visualiser comment la troisième composante de l'innovation affecte l'État. Il serait alors intéressant d'ajouter des données de covariance pour avoir une idée de la façon dont la matrice K est créée.

MISE À JOUR # 2 Le tracé a maintenant des vecteurs dans l'espace d'état qui montrent comment chaque composant de mesure change la position. De ce graphique, je peux voir que la troisième composante de la mesure change le plus l'état.

Une manière très informative de visualiser l'effet des mesures (pour moi) consiste à tracer l'état du robot (moyenne, avec ellipse de covariance) avant et après chaque mesure. Ensuite, prenez les composants individuels de la mesure (roulement, plage pour les marqueurs AR) et appliquez-les séparément pour en avoir une idée.

Pour faire ça:

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

Le suivi de la covariance de l'hypothèse antérieure, de l'état mesuré et de l'hypothèse postérieure est généralement suffisant pour déterminer si les équations d'un EKF sont appliquées correctement.

Bonne chance et ne mettez pas à jour votre question trop souvent. Revenez plutôt avec de nouvelles questions.

Quelque chose qui est souvent fait est de tracer les variables d'état au fil du temps ainsi que leurs intervalles de 3 sigma. Les points où cet intervalle diminue sont les mises à jour, où vous pouvez éventuellement annoter la source des mesures impliquées.

Outre les erreurs de mise en œuvre qui doivent être vérifiées (non seulement des équations erronées, mais aussi des équations numériquement instables), l'effet des mises à jour n'est directement affecté que par la différence entre ce qui est "attendu" et "mesuré" et leurs incertitudes respectives. Donc, vous pourriez être intéressé à trouver un moyen de visualiser cet équilibre en termes de progression temporelle dans la première intrigue.