Site ELEC344/ELEC381

Partie interactive du site pédagogique ELEC344/ELEC381 de Télécom ParisTech (occurrence 2010).

Catégories

Wahba pour Kalman

Lundi

Après les larges lectures des articles sur Kalman, MAV, IMU que Alexis nos a donné maintenant je peut dire que je  comprendre mieux les choses. A début des lectures j’ai eu des problèmes pour comprendre mais après un coup d’œil aux cours de physique, probabilités, et autres aspectes commet les angles d’Euler et les quaternions les choses ont commencé à être plus claires.

Mardi

Les neuf variables d’état du filtre Kalman  sont les trois vitesses de rotation angulaire mesures pour les gyroscopes plus les deux colonnes de la matrice de rotation de la base du mobile à la base de référence. Ces colonnes de matrice de rotation son trouvés à partir du algorithme FQA. FQA prend les valeurs mesures par l’accéléromètre et magnétomètre et donné la matrice rotation. Après lecture de Wahba, FQA, et le code de Fernando on a vu il y a encore le problème de singularité quand le tangage c’est près de quatre-vingt dix degrés.

Mercredi

En cherchant résoudre le problème de singularité j’ai vu que on peut trouver les colonnes de la matrice de rotation sans passer pour les quaternions. Celui-ci utilise plusieurs opérations comme sinus et cosinus qui sont très couteuses. Ainsi, j’ai fait une fonction qui donne les colonnes de la matrice en utilisant le minimum des opérations. Avec F-X on testé sur l’IMU et on a un problème sur le tangage, quand on fait la rotation l’axe n’est pas constante. Il reste de résoudre ce problème et celui de singularité, on a besoin que le problème de Wahba soit bien résolu pour tester le filtre Kalman sur l’IMU.

Matrix reloaded ...

Hier, fort de la lecture de différents articles et codes source, je me suis lancé dans le code du filtre de Kalman, sans pouvoir toutefois réellement le tester (mais il compile !). J’ai fait attention d’optimiser un peu les opérations sur les flottants même si on peut sûrement faire quelque chose de mieux ‘hardcore style’. J’espère aussi qu’une opération flottante avec 0 prend moins de temps qu’une autre !

Au total, une passe de mon filtre de Kalman prend environ 870 multiplciations, 950 additions et 9 divisions. Je n’ai pas encore relié ça rigoureusement aux temps d’execution qu’on a mesuré avec Etienne mais il semble me souvenir que ça devrait faire aux alentours de 5ms. Vu le temps de réaction et l’inertie de l’hélicoptère, je pense qu’on est bon de ce point de vue là.

L’objectif de la journée d’aujourd’hui était d’intégrer l’algo FQA (pour la résolution du problème de Wahba) et le filtre de Kalman pour pouvoir le tester. Seulement, pour faire ça, il fallait d’abord vérifier que l’algo FQA marchait bien, à l’aide du visualisateur 3D. Le protocole est simple: Le visualisateur récupère les valeurs du capteur sur la carte (envoyées par Bluetooth), execute l’algo en local  (pour eviter des problèmes supplémentaires …) et met à jour l’affichage.

Et là, ça a été trèèèèèèèèèèèèèèèès long (oui, une journée complète de 10h, je ne comprends toujours pas comment le temps a pu filer ainsi !) pour avoir le lien entre l’affichage et la matrice renvoyée par l’algo avec tous les problèmes de changements de base (carte->monde physique réél->monde openGL->représentation de la carte dans le monde OpenGL), de matrices transposées ou pas, bref !

Voyant que je pataugeais, Etienne D. m’a bien aidé pour essayer de voir les choses clairement et systématiquement et on est (visiblement !) arrivé à quelque chose de cohérent là-dessus.

Ce qui nous a permis de voir que l’algo FQA (ainsi qu’une version allégée que Miguel a codé en parallèle) ne marche pas très bien, vraisemblablement à cause d’un problème d’ordre de rotation. J’ai essayé aussi de reprendre l’algo TRIAD (qui n’utilise que des opérations vectorielles donc normalement exempt de ces problèmes d’angle) sans grand succès non plus…

Bref, c’est un peu une journée désespérante de ce côté là, heureusement qu’Etienne avait à côté des résultats positifs (et que Pulse aussi, derrière nous, on pouvait s’amuser à les pinger ou à les synflooder pour se détendre :P ).

J-23 La note du banc

Aujourd’hui j’ai compilé réussi à bien compiler mon code et à l’exécuter sur la carte de l’Heliokter en RAM et en Flash ( XIP ). J’ai pu débugger ma plateforme de bench ( quelques problèmes avec le timer m’ont ralenti, en effet il faut  déclencher un event quelconque pour que le Prescaler soit bien pris en compte ce qui m’avait échappé, et qui passe inaperçu avec de l’autoreload ).

Après quoi on a benchmarké rapidement avec FX quelques fonctions de math.h : float cos(float) versus double cos ( double ) , sqrt , addition , mulitplication de double. Ce qui lui permettra d’évaluer la vitesse de son filtre de Kalman et de décider entre float et double. Il a noté les résultats je lui laisse le soin de les donner, et on fera peut être un rapport plus poussé plus tard pour mémoire.

Pour l’exécution in place (XIP) j’ai pu faire des tests extensifs avec des traces issues des capteurs dans le simulateur ( 180 mesures environ ).  J’ai obtenu un temps d’exécution de 755 µs en moyenne environ. Ci-dessous un schéma résumant les résultats ( avec en axe des ordonnées les µs et en abscisse le numéro de la mesure ). Il y a quelques petites choses bizarres sur la fin, je pense que je dois changer ma méthode de mesure pour prendre en charge le changement de contexte avec une macro de FreeRTOS exprès , à vérfier, ce que je ne fais absolument pas pour l’instant et donc les mesures ne sont pas pures …

Nous avons aussi regardé avec FX quelques bugs qu’il y avait sur les changements de bases entre les données des capteurs, la sortie de wahba / Kalman et le monde réel.

Il faut toujours que je définisse des symboles dans mon code pour réussir à mettre en ram la partie du code à benchmarker et laisser en flash les jeux de test.

Heliokter: Wahba

Aujourd’hui:

Première version d’une bibliothèque de quaternions (samble marcher).

Première version du FQA (semble marcher).

Hier:

Lecture du protocole i2c (partie théorique et le datasheet).

Première version d’une bibliothèque i2c.

Les PCB de Pâques

Non, ils ne sont pas enc chocolat mais ils sont arrivés aujourd’hui (pour nous en tout cas et pour plusieurs autres groupes aussi je crois). Cette après-midi, nous avons d’ailleurs (entre autres) aidé Alexis à souder les composants

Voici une petite photo du joujou :


Ce soir, j’ai converti tout ce que j’avais fit sur l’IMU sur cette carte et ça c’est fait plutôt sans douleur puisqu’il suffisait de changer les affectations des pins (et encore, le Bluetooth etait branché au même endroit et il y avait juste une permutation entre les ports SPI pour l’accéléromètre et le magnétomètre). Modulo le magnéto-résistor vertical, non encore soudé sur la carte pour des raisons de fragilité,  on a donc  pour le moment les mêmes fonctionnalités que sur la carte IMU.

Pour ce qui est du Wahba, le résultat n’est toujours pas très probant mais Fernando m’a parlé d’un algorithme qui pourrait mieux nous aider que le TRIAD. Pourtant les valeurs de base semblent bonnes puisqu’avec le magnétometre seul, je détecte bien le lacet et avec l’accéléromètre seul je détecte bien le roulis/tangage. C’est la combinaison des deux (avec les noeuds au cerveau qui viennent avec, dès qu’on pense ordre des rotations dans l’espace) qui n’est pas au point.

Le fait de se voir tous les quatres après les vacances de Pâques a été aussi l’occasion de mettre tout le monde à jour, de fixer les tâches de chacun et de préparer la soutenance de demain.

IMU : promis, demain j’arrete …

Bon, j’ai encore travaillé sur la partie numérique de la carte IMU (à savoir accéléromètre et magnétomètre) ce soir pour essayer de gérer au mieux les valeurs reçues (premier filtrage, normalisation …) et de préparer le terrain pour les algorithmes de filtrage et de positionnement (TRIAD ou QUEST).

J’ai perdu un peu de temps suite à un problème de mercurial (j’avais oublié de commiter vendredi soir  :s) mais j’ai réussi à convertir et normaliser les valeurs reçues correctement,  sur 8 bits, tout en essayant de rester le plus précis possible : C’est à dire que pour chaque mesure vectorielle, je repère la position du bit le plus significatif  (le plus à gauche) de la plus grande des composantes (en valeur absolue) . Et je décale ensuite mes composantes de sorte que ce bit devienne le bit de poids fort des nouvelles valeurs. Ca permet de rester le plus précis possible et d’éviter les phénomènes de saturation qui peuvent arriver si on choisit une valeur de décalage arbitraire …

Ensuite, j’ai modifié le simulateur pour qu’il serve plutôt de visualiseur : Il récupère des informations de la carte IMU par Bluetooth puis oriente le modèle 3D en fonction.

J’ai essayé d’implémenter l’algorithme TRIAD (sur la carte et sur le PC), sans grand succès. Je crains que ce ne soit dû aux valeurs du magnétomètre, qui sont assez instables. Je me demande de plus si on va pouvoir l’utiliser dans notre copter, en plus au milieu de huit moteurs. Avec un magnet’ de frigo (qui ne sont pasdes aimants réputés pour leur puissance), on perturbe serieusement le comportement à partir de 10cm donc bon, prudence …

J’ai donc essayé de faire quelque chose avec seulement l’accéléromètre (c’est à dire en prenant l’autre vecteur de l’algo TRIAD fixe et arbitraire) et ça marche plutôt bien, voire bien tout court !

Heliokter: Wahba

- Lectures:

« A Simplified Quaternion-Based Algorithm for Orientation Estimation From Earth Gravity and Magnetic Field Measurements »

attitude determination (50%)

- Mise en oeuvre du algorithme TRIAD simple.

- Simulateur marche sur linux.

A Simplified Quaternion-Based Algorithm for Orientation Estimation From Earth Gravity and Magnetic Field Measurements

Articles

Voici la série d’articles, faisant suite aux exposés sur les mêmes sujets, écrits par les élèves d’ELEC344 :

Merci à tous !

TP : Le bout du tunnel

Ce matin, j’ai commencé à rédiger ma partie de l’article sur le problème de Wahba, principalement l’algorithme TRIAD  il a fallu se remettre un peu à LaTeX …

Cette après-midi, j’ai résolu les quelques problèmes qu’il y avait sur mon ZigBee (il en reste peut-être du genre non-déterministes) et j’ai fait marcher les interruptions avec les boutons : un bouton permet de modifier l’intensité d’éclairage du backlight et l’autre active/désactive le buzzer (très utile pour ne pas embêter toute la salle !).

D’avoir fait le ZigBee hier m’a permis de pouvoir aider un peu les autres qui pouvaient eux m’aider sur le traitement des interruptions.

En fin d’après-midi, j’ai refait la partie qui gérait l’affichage sur le LCD en quelque chose de plus simple et plus modulaire qui a pas mal aéré mon ‘main.c’.

Donc ben je crois que ça clot à peu près le TP d’initiation, c’est bien chouette :p

En ce qui concerne le projet, on a fait un point de situation avec l’équipe pour faire un petit diagramme en blocs de nos composants logiciels et se répartir l’étude de ces blocs. Je ferais surement un petit schéma sur le wiki.

Avancement TP

Hier, j’ai avancé le TP jusqu’à la partie interruption avec Kellya. Malgré les explications de Samuel il me manquait quelques détails d’ordre synthaxique principalement.

Aujourd’hui j’ai amélioré mon code sur le LCD, mis en place le buzzer qui fait des gammes, mis en place les interruptions sur les boutons (l’un allume et éteint progressivement le rétro-éclairage, l’autre lance une gamme).

Reste à faire :

- le Zigbee (a priori je serais un peu aider parce que que j’ai déjà fait à la robotique),

- patcher le gestionnaire de bouton pour qu’il ne réagisse pas aux faux contacts des boutons.

-commenter le code et bien vérifier que rien n’est redondant.

Sinon j’ai aussi écrit ma partie de l’article sur le problème de Wahba (je me suis un peu emporté j’ai fait 4,5  pages.)