Site ELEC344/ELEC381

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

Catégories

Améliorations pour le code de Wheely

Cette nuit, Alexis nous a communiqué le fruit de ses recherches sur le filtrage. Sa conclusion : Le filtre complémentaire que nous utilisions est très bon une fois qu’on lui soustrait le drift, mesuré à l’aide d’un filtre de Kalman tournant en parallèle.

Avec l’aide du code fourni par Alexis, j’ai intégré le Kalman et la soustraction du drift. Le code d’Alexis m’a par ailleurs inspiré sur pas mal de points, en terme de propreté, donc j’en ai profité pour faire une bonne petite refonte du code de Wheely, avec des variables plus claires, et une API pratique et compréhensible.

Cet après midi nous avons testé le code. Après que nous ayons corrigé quelques coquilles, nous obtenons sans l’asservissement sur X des résultats plutôt satisfaisant. Le robot tient assez bien sur place, à condition que le léger offset sur l’angle soit bien réglé à la main. Mais il est assez stable, autour de 0.6°, désormais codé en dur.

Difficile pour l’instant de bien contrôler les effets d’un asservissement sur X lorsque nous l’introduisons.

J-22

Alors comme ça fait plusieurs jours que je n’ai pas mis à jour voici ce que j’ai fait durant les derniers jours :

Jeudi 15

J’ai amélioré mon code de benchmark, voici des résultats plus extensifs sur les opérations :

.

.

Math timings double Timing ( µs ) Cycles

.

offset add mul div cos sin abs sqrt abs

.

offset 14492 14.492 1043.424

.

add 28827 28.827 2075.544

.

mul 28823 28.823 2075.256

.

div 28826 28.826 2075.472

.

cos 14464 14.464 1041.408

.

sin 14487 14.487 1043.064

.

offset abs 14463 14.463 1041.336

.

sqrt(abs()) 41676 41.676 3000.672

.

Math timings floats

.

offset add mul div cos sin abs sqrt abs

.

offset 14501 14.501 1044.072

.

add 28826 28.826 2075.472

.

mul 28855 28.855 2077.56

.

div 28855 28.855 2077.56

.

cos 14473 14.473 1042.056

.

sin 14492 14.492 1043.424

.

offset abs 14501 14.501 1044.072

.

sqrt(abs()) 42032 42.032 3026.304

Je ne suis pas vraiment satisfait car les résultats ne me semblent pas vraiment cohérents, mais peut être est-ce à cause de l’influence de divers paramètres pas controlés : cache, optimisation, autre.

Explication de la méthode : le temps à gauche représente le temps d’exécution pour 1000 opérations en µs.
offset : temps d’exécution d’une fonction pour avoir une nombre alétatoire double ou float( mul, add, modulo, sauvegarde dans une variable double ou float static ), fonction appelée myrandd() ou myrandf()
add/mul/div : 2 appels à myrand, 2 cast int->float ou double, addition/mulitplication/division des deux nombres et sauvegarde dans une variable float/double.
cos/sin : 1 appel à myrand(), 1 cast, 1 sauvegarde
offset abs : 1 appel à abs( myrand( ) ) 1 cast et 1 sauvegarde
sqrt : 1 appel à sqrt(abs(myrand( ))) 1 cast et 1 sauvegarde

Le problème est que l’on ne peut pas vraiment soustraire l’offset des opérations … car le temps d’exécution n’est pas vraiment linéaire. Un autre point qui m’étonne est le nombre de cycles qu’il faut pour faire une simple addition …

Bref je vous donne les résultats as is, car j’ai du mal à les interpréter. On retiendra qu’une opération arithmétique sur un double ou un float est du même ordre de grandeur : quelques dizaines de µs.

Vendredi 16

J’ai écrit le code pour les télémètres, ce qui était assez rapide puisque j’ai juste eu à m’intégrer dans le code de FX pour les gyros. Mais comme les télémètres étaient bloqués au Royaume Uni à cause de l’arrêt du traffic aérien, une fois mon code fini, j’en ai profité pour me mettre à jour sur le code qu’avaient écrit les autres.

J’ai aussi commencé à nettoyer mon code et à agencé un peu l’architecture globale du code.

Samedi , Dimanche, Lundi 19

Vacances

Mardi 20

Retour en A405 par une magnifique journée de printemps. Aujourd’hui j’ai écrit le squelette de notre version propre alpha pour l’heliokter avec des prises de sémaphore bien claires, des delays là où il faut et une beau découpage en fichier pour la compilation séparée. Ca m’a pris un peu de temps dans le sens où j’ai du lire le code un peu de tout le monde et réfléchir à comment wrapper les fonctions de chacun pour ne pas avoir à réécrire trop de choses.

Ensuite comme les télémètres étaient arrivés, j’ai pu vérifier mon code, qui marchait du premier coup ! ( merci FX ) ensuite j’ai pu étalonner le télémètre et maintenant je récupère une distance en cm. Celle-ci est bonne pour une distance entre 8cm et 70 cm environ ( comme nous n’avions qu’une règle de 30 cm ( merci Flavia ) , les mesures au delà de 30 cm n’étaient pas très précises).

La méthode pour étalonner : regarder la valeur en « int12″ renvoyée par l’ADC à 8 cm, celle renvoyée à 30 cm, regarder la courbe sur la datasheet qui ressemble fortement à a/x+b, ensuite trouver a et b à partir de ces deux points. Ci dessous le résultat sous forme de courbe ( axes : distance en cm )

  • En rouge : y=x
  • En vert : y= f(x) où f est la fonction d’interpolation. Dans l’idéal elle devrait donner l’identité.

C’est assez satisfaisant !

Mercredi 21

Aujourd’hui on a réussi à avoir une version « sale » mais complète du code qui compile ( c’est à dire senseurs, filtrage, asservissement et moteurs ).

On a ensuite fait un vol d’essai pour faire une acquisition et un test du filtre de Kalman depuis notre propre carte. Ce qu’on a constaté dans ce premier vol et qui m’inquiète un peu c’est que les angles mis en jeu pour le roll et le pitch sont très faibles, je me demande aussi l’amplitude des valeurs utilisées par les moteurs ( +- 3 sur un unsigned char ou bien +- 100 ? ).

Quand l'incroyable se produit

Après quelques essais infractueux, je m’étais un peu résigné sur cette histoire de filtre de Kalman.

Mais là ce soir, dans un éclair d’espérance, j’ai relu mon code. Après la correction d’une erreur de signe, d’une erreur de calcul (je faisais la somme d’une matrice et de sa transposée « en place », très mauvaise idée …) et avec un pas de discrétisation plus petit, mon filtre s’est soudainement mis à marcher.

Yeeepeee !

Si, si !  et même que le changement des paramètres de covariance influe de manière à peu près logique sur le système. Si je fais trop confiance aux gyros, j’ai un petit effet « flamby inertiel » (lhélico tourne un peu plus loin qu’il aurait dû et revient un peu après le mouvement pour se trouver au bon endroit) mais si je fais trop confiance aux accelerometre/magnétomètre, il reprend un peu la tremblotte (comme sans filtre en fait).

Il faut encore que je comprenne comment régler la puissance de la correction sur l’offset des gyros, même si ça marche vraiment pas trop mal pour le moment.

Normalement, on va essayer de voir ce que ça donne demain mercredi sur un vrai vol : L’helicoptère devrait être attaché par des cordes de sorte qu’il ne puisse pas trop se déplacer mais en laissant de la marge sur son assiette pour voir si notre asservissement fonctionne.

Je vous aurais bien montré des vidéos de la visualisation de l’orientation de la carte mais je n’ai pas d’iPhone 3GS :’( (ou d’autres outils de prises de vue animées non propriétaires)

Résumé des épisodes précédents

Après avoir perdu sur la représentation des rotations de la carte en OpenGL, je me suis finalement mis à lire directement la DCM (matrice qui détermine l’attitude de la carte) pour savoir si l’algo FQA était bon. Ca permet surtout de vérifier pas mal de cas et de se convaincre que ça marche (quand la matrice n’a que des 0 et des +/- 1). Et le FQA a bien l’air de marcher !!

Sur la carte Heliokter, j’ai repris la méthode de Samuel pour déterminer comment les composants détectaient les axes et ça a presque tout de suite marché avec l’algorithme FQA. Vendredi soir j’ai juste eu le temps d’implémenter le FQA sur la carte et le filtre de Kalman sans pouvoir tout tester.

Aujourd’hui lundi, j’ai d’une part repris le code de visualisation OpenGL pour recevoir par Bluetooth directement les coefficients de la DCM calculés sur la carte et les représenter (sans se tromper ce coup-ci). C’est OK de ce point de vue là et ça marche bien. Il n’y a pas trop de singularité mais les changements d’attitude manquent de fluidité. C’etait prévu, mais ça permet aussi de voir que c’est effectivement très sensible aux vibrations. Après, peut-être que les vibrations faites avec la main sont différentes (notamment en terme de fréquence) de celles ressenties sur l’Heliokter.

D’autre part, j’ai regardé le comportement de mon filtre de Kalman  en fonction de différentes valeurs sur les incertitudes de mesures et de prédiction. J’ai surtout fait du cas extrême pour voir s’il n’y avait pas de grosses erreurs. C’est un peu le flou puisque je ne sais pas non plus si le modèle cinématique que j’ai implémenté dans le filtre est juste  mais j’ai observé des choses qui me semblaient cohérentes.

  1. Quand l’incertitude de prédiction est beaucoup plus faible (donc plus fiable !) que celle de mesure, j’ai mon état qui se décale lentement mais surement de l’état réel. C’est visiblement du au biais des gyroscopes, qui n’est pas corrigé dans l’état. C’est cohérent avec les incertitudes qu’on fixe.
  2. Quand l’incertitude de mesure est beaucoup plus faible que celle de prédiction, j’ai un filtre qui diverge très rapidement (en quelques itérations) sans que je puisse vraiment l’expliquer.
  3. Dans les autres cas (c’est à dire quand les incertitudes sont du même ordre de grandeur), mon état commence par se décaler de l’état réel puis le biais calculé dans l’état du filtre commence à se rapprocher du biais réél, ramenant l’état vers sa valeur réelle. Jusque là tout va bien. Mais le problème c’est que cela va de plus en plus vite et le biais calculé finit par dépasser rapidement la valeur du biais réel et s’écrase dans les choux (genre dépasement de capacité) quelques itérations plus tard.

Le jeu sur les valeurs de l’incertitude influe sur le temps que met le filtre à diverger. Ca va de jamais (cas extreme 1) à tout de suite (cas extreme 2). Je n’ai pas encore trouvé de valeurs qui marchent.

Voilà, après j’imagine qu’il faudra régler les incertitudes plus finement (c’est à dire avoir des coefficients différents pour chaque mesure et chaque prédiction, pour le moment) mais je ne sais pas si le comportement que j’observe présentement est typique d’un filtre de Kalman correct mal calibré ou d’un filtre complètement foireux …

Encore une victoire de canard !

Suite à mon dernier article j’ai tenté toutes sortes de manières de debug pour trouver d’où le petit problème venait. Je suis passé par l’implémentation d’un programme en C sur mon ordinateur pour tester le filtre sur un échelon pour comparer à ce que donnait MATLAB (pareil), tester l’échelon sur la carte logique, j’en passe et des meilleures.

Après de longues heures à m’arracher les cheveux, j’ai enfin réussi à corriger le code. Le problème venait de l’accéléromètre. La récupération des valeurs via SPI posait des soucis et désynchronisait le filtre. J’ai donc créé une tâche  spécifique qui se charge d’updater une variable locale au programme avec ce qu’elle récupère via SPI. Ca permet d’éviter de perturber le bon déroulement du filtrage.

Voici le résultat obtenu lors du benchmark. Cela correspond pile poil aux mouvements donné à la carte par le servomoteur. Et l’axe des ordonnées indique pile poil l’angle d’inclinaison de la carte en centièmes de degrés. C’est trop cool, et surtout, ça va permettre de bosser sur la suite en ayant une totale confiance dans les angles que l’on va manipuler.

Filtres complémentaires : mission accomplie

Bon, suite aux travaux d’aujourd’hui, j’ai réussi à obtenir de très bonnes courbes. Notre problème d’offset sur les valeurs et de temps de réponse du filtre a été résolu en choisissant une valeur beaucoup plus grande pour la fréquence de coupure.

J’ai ensuite mis en place un benchmark assez complet : oscillation rapide, oscillation lente avec plateau ou non, oscillations rapides de demi amplitudes.

J’ai ensuite calibré le coefficient relatif entre l’accéléromètre et le gyroscope en utilisant des oscillations de différentes fréquence, l’un et l’autre se manifestant ainsi différemment. Le bon coefficient relatif est celui qui permet d’avoir une même mesure de l’angle pour des oscillations de fréquence différente.

Voici le graphique obtenu, avec en haut la commande d’angle donnée aux servomoteurs, et en dessous, l’inclinaison, calculée en degré, en fonction des données recueillie par les capteurs lors du mouvement. Le seul défaut que l’on peut constater est un angle calculé un chouilla en dessous de 0 lorsque l’on vient de remonter la carte (aucun soucis lorsqu’elle vient de descendre). Cela est probablement du à un petit défaut des servo vis à vis de la gravité, et n’est donc a priori pas lié à notre travail.

J’ai ensuite implémenté le nouveau filtre en C, modifié le code du gyroscope pour intégrer après filtrage (et non avant comme c’était fait), et créé deux fonctions get_angle_degree et get_angle_radian qui intègrent les bons coefficients.

Enfin, j’ai testé l’execution sur la carte de tout ça, envoyant via bluetooth le résultat de la commande get_angle_degree*100 (pour avoir de la précision malgré le cast en entier pour l’itoa).

J’obtiens des résultats à l’image de la simulation MATLAB, plutôt satisfaisants. Hormis pour le troisième type d’impulsion, ou ça ne colle pas. Ca n’atteint pas les 10°, et surtout, ça fait deux oscillations au lieu d’une.

Compte tenu que j’ai bien vérifié que le filtre et les données traitées (vitesse donnée par le gyroscope et accélération donnée par l’accéléromètre) sont exactement les mêmes je ne comprends pas trop. Si quelqu’un à une piste je suis preneur, en attendant, je continue à chercher…

Quand Wheely devient iWheely

Alors, en parallèle du reste, ces derniers jours j’ai aussi finalisé l’application iPhone et le serveur relais en Python. Voici un petit résumé de la joyeuse situation : L’application iPhone fournit des boutons pratiques (pause et connexion) et une IHM sympa (du moins du point de vue de son créateur) pour visualiser la consigne de pilotage données par l’accéléromètre (avec filtrage passe bas pour lisser et une dead zone pour envoyer facilement une consigne « ne bouge pas). Le serveur TCP Python lui attend une connexion de la part de l’application iPhone, typiquement au travers d’un réseau Wifi (ad-hoc ou pas). Il ouvre ensuite une liaison bluetooth profil SPP (RFCOMM), via la fabuleuse bibliothèque lightblue, vers Wheely, et lui transmet les consignes perçues de la part de l’iPhone.

En bref, toute la partie pilotage via iPhone-Wifi-Laptop-Bluetooth du robot est a priori bouclée. Il reste à s’occuper du traitement des consignes coté robot. J’ai aussi fait des tests de portée du Bluetooth, qui sont largement concluants, puisque on obtient une portée de 15m à vue, 10m derrière un mur. Tout ça pour dire que la vie est belle, ou iBelle devrais-je dire.

Kalman farçi, Kalman à l'américaine ...

Aujourd’hui, j’ai fait beaucoup de recherches sur le filtrage de Kalman, afin que ce soit bien clair dans ma tête et pour enfin voir concrètement ce qu’on va faire.

C’est un peu frustrant (surtout à ce stade du projet) de passer une journée où on ne produit rien de concret mais je pense que ça aidera quand même .

J’ai eu de gros moment de doutes dans la journée (notamment sur le calcul d’erreur et la manière dont on l’utilisait pour mettre à jour la variable d’état) qui se sont dissipés au fur et à mesure des lectures. Ca ne fera surement pas de mal non plus de confronter ce que j’en ai compris avec Miguel demain et d’en parler aussi avec Alexis.

Tout d’abord, le mot clé  AHRS (pour Attitude and Heading Reference Systems) m’a ouvert pas mal de portes pour trouver d’autres articles que ceux d’Alexis, certains plus théoriques et aussi des exemples de code. J’ai aussi découvert le projet OpenPilot (dont les sources sont normalement libres, mais je n’ai pu trouver les sources de leur AHRS (!) dans leur dépôt SVN ) et Rotomotion, asez présent sur le net.

Deux liens ici vers des exemples de code que je ne trouve pas parfait non plus (notations peu claires, syntaxe hésitante, utilisation de flottants) mais qui permettent de se donner une idée de ce qu’on devra faire.

Et un article, qui reprend l’idée de considérer le gyroscope comme commande et les accéléromètre comme mesure. Ils utilisent également les quaternions  et donnent les matrices utilisées dans le filtre de Kalman à part les matrices de covariance.

Voilà, j’espère qu’on pourra demain décider finalement ce qu’on met dans notre filtre, éclaircir les dernières zones d’ombre avec Alexis et commencer à le coder !

Quant au code de Mikrokopter, il m’a l’air très artisanal, avec des tests sur les écarts entre gyros et accélérometres et autres. Bref, pas de filtre de Kalman de ce côté là.

Gyroscope et filtre anti dérive d'ordre 2

Aujourd’hui j’ai continué à travailler sur le gyroscope. J’ai commencé par chercher le mystérieux bug d’hier et j’ai découvert à mes dépend que l’opérateur % en C (modulo) ne renvoit pas le modulo n canonique (le nombre compris entre 0 et n-1) mais le reste de la division euclidienne. Ce qui signifie que -1%3 = -1.

D’où des underflow dans mes buffers… D’où des résultats bien étrange. Mais ce temps est révolu !

J’ai donc ensuite testé le filtre MATLAB de Fabien, après l’avoir implémenté en C. Les résultats sont cohérents avec les simulations MATLAB de Fabien, comme vous pouvez le voir dans les diagrammes ci-dessous, que j’ai tracé avec l’excellent freeware Mac qu’est Plot, découvert pour l’occasion.

X et Y intégrés (représentent l'angle de rotation de la carte) sans filtrage. La carte est immobile. La partie agitée correspond à une agitation de la carte qui ne fait qu'accentuer gravement la dérive.

X et Y intégrés (représentent l'angle de rotation de la carte) avec filtrage. Cette fois ci, une agitation de la carte qui ne déclenche toujours pas de dérive significative ! C'est un peu normal vu le filtre, mais ça devrait produire des résultats intéressants une fois combiné à l'accéléromètre.

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 !