Implémentation d'un filtre kalman sensible au débordement

Question

J'essaie de mettre en œuvre un filtre kalman pour obtenir l'orientation d'un objet, en utilisant un accéléromètre 3 axes et un gyroscope 3 axes comme capteurs.

Le choix du modèle dynamique pour la prédire phase de ce filtre est simple, il est:

new_angle = angle + angular_velocity * time 
new_angular_velocity = angular_velocity 

Mais je n'ai pas flottant appui ponctuel à portée de main, et je dois chaque bit de précision pour modéliser la angle. Par conséquent, mon plan était de représenter l'angle comme des données entières de 32 bits, représentant un tour complet de 2 pi en 2^32 petites étapes. En conséquence, le débordement d'entier gère l'enveloppe (2 pi étant la même orientation que 0) gratuitement. Mais cela pose également un problème au filtre: si l'angle estimé serait, disons 359°, et que ma mesure est 0°, alors le filtre suppose une énorme innovation, conduisant à des incertitudes et des valeurs impaires.

Y a-t-il un moyen de rendre le filtre conscient de ce phénomène d'enroulement? Donner une innovation de seulement 1° dans le cas ci-dessus? Pour contourner le problème, j'ai pensé à utiliser des différences d'angles au lieu des angles, mais je ne trouve pas de modèle approprié.

Answer 1

J'ai eu le même problème avec KF similaire aujourd'hui ... mais je n'étais pas aussi contraint que vous.

Voici ce que je faisais:

1. Juste après la valeur predicted_angle est calculée:

   // éviter de sauter entre 0 et 360
   si (measured_angle < pi/2 & & predicted_angle> 3 * pi/2) prdict_angle = prdict_angle - 2 * pi;
   if (angle_mesuré> 3 * pi/2 & & estimate_aAngle < pi/2) triangle_prédigé = angle_prédicat + 2 * pi;
   

2. Normaliser le nombre estimé après son calcul.

Si vous ne me dérange pas sacrifier un peu de précision que vous pourriez passer à des entiers signés [-2 * Pi, + 2 * Pi) gamme et faire la même chose.

P.S. Si le changement d'angle maximum par échantillon est petit, je pense que vous pourriez sacrifier juste une fraction de précision d'un bit en utilisant le décalage d'angle interne dans le KF. Le décalage doit être plus grand que cette valeur. Vous aurez une plage OFFSET 2^32 = 2 * Pi + 2 * au lieu de 2 * pi. Dans le KF, ajoutez ce DECALAGE aux variables d'angle locales et renvoyez la valeur estimée à la valeur estimée (offset_estimated_angle - OFFSET, 0-2 * pi).

HTH