Kalman sur centrale inertielle

Bonjour à tous,
Tout d'abord je ne sais pas si il existe une endroit ou se présenter sur ce forum, mais bon voila je suis bird12358 et je suis nouveau sur ce forum.

Le sujet que je voudrais abordé porte sur les centrales inertielles et plus particulierement la chr6-dm. Cette centrale inertielle est livrée avec son code source.
Je suis familiarisé avec l'utilisation du filtrage de kalman et des notions mathématiques qui sont utiles pour ce sujet.
J'ai commencé a dépouiller le code fourni et je me heurte à un soucis.
- Donc la première étape est compréhensive, les gyros sont utilisés pour avoir un état prédit du système sur les 3 axes.
- Dans l'étape suivante, les accéléros sont utilisés pour apporter une correction. Je vous met en dessous le code source dont je parle:

                  // Now subtract the reference vector
		  acc_vect.data[0] = acc_vect.data[0] - acc_hat.data[0];
		  acc_vect.data[1] = acc_vect.data[1] - acc_hat.data[1];
		  acc_vect.data[2] = acc_vect.data[2] - acc_hat.data[2];
		  
		  // Multiply by Kalman gain
		  MatVectMult3( &L, &acc_vect, &correction );
		  
		  // Apply correction
		  estimated_states->phi = estimated_states->phi + correction.data[0];
		  estimated_states->theta = estimated_states->theta + correction.data[1];
		  estimated_states->psi = estimated_states->psi + correction.data[2];

Les données brutes des capteurs accéléros sont extraits (acc_vect.data[x]) et sont soustraits à l'estimation de l'accélération (acc_hat.data[x]). Cette accélération estimé est le vecteur de référence ([0,0,g] dans le repére terrestre) rotaté de la triple rotation R= Rphi*Rtheta*Rpsi. Par contre je ne comprends pas le dernière partie:

// Apply correction
		  estimated_states->phi = estimated_states->phi + correction.data[0];
		  estimated_states->theta = estimated_states->theta + correction.data[1];
		  estimated_states->psi = estimated_states->psi + correction.data[2];

Comment peut-on appliqué à chaqu'un des angles, une correction qui est une différence entre un vecteur d'accélération et sa référence? Il y a pas un problème d'unité?

Je suis consciens que c'est un sujet pas évident, j'espére que quelqu'un pourra m'aider.
D'avance merci.

bird12358.

Bonjour à tous,
Tout d'abord je ne sais pas si il existe une endroit ou se présenter sur ce forum, mais bon voila je suis bird12358 et je suis nouveau sur ce forum.

Le sujet que je voudrais abordé porte sur les centrales inertielles et plus particulierement la chr6-dm. Cette centrale inertielle est livrée avec son code source.
Je suis familiarisé avec l'utilisation du filtrage de kalman et des notions mathématiques qui sont utiles pour ce sujet.
J'ai commencé a dépouiller le code fourni et je me heurte à un soucis.
- Donc la première étape est compréhensive, les gyros sont utilisés pour avoir un état prédit du système sur les 3 axes.
- Dans l'étape suivante, les accéléros sont utilisés pour apporter une correction. Je vous met en dessous le code source dont je parle:

                  // Now subtract the reference vector
		  acc_vect.data[0] = acc_vect.data[0] - acc_hat.data[0];
		  acc_vect.data[1] = acc_vect.data[1] - acc_hat.data[1];
		  acc_vect.data[2] = acc_vect.data[2] - acc_hat.data[2];
		  
		  // Multiply by Kalman gain
		  MatVectMult3( &L, &acc_vect, &correction );
		  
		  // Apply correction
		  estimated_states->phi = estimated_states->phi + correction.data[0];
		  estimated_states->theta = estimated_states->theta + correction.data[1];
		  estimated_states->psi = estimated_states->psi + correction.data[2];

Les données brutes des capteurs accéléros sont extraits (acc_vect.data[x]) et sont soustraits à l'estimation de l'accélération (acc_hat.data[x]). Cette accélération estimé est le vecteur de référence ([0,0,g] dans le repére terrestre) rotaté de la triple rotation R= Rphi*Rtheta*Rpsi. Par contre je ne comprends pas le dernière partie:

// Apply correction
		  estimated_states->phi = estimated_states->phi + correction.data[0];
		  estimated_states->theta = estimated_states->theta + correction.data[1];
		  estimated_states->psi = estimated_states->psi + correction.data[2];

Comment peut-on appliqué à chaqu'un des angles, une correction qui est une différence entre un vecteur d'accélération et sa référence? Il y a pas un problème d'unité?

Je suis consciens que c'est un sujet pas évident, j'espére que quelqu'un pourra m'aider.
D'avance merci.

bird12358.

Bonsoir
pas évident oui, loin de mes compétences mais intéressant; j'ai donc parcouru la datasheet et j'ai compris que les signaux délivrés sont déjà traités par Kalman An Extended Kalman Filter (EKF) combines data from onboard accelerometers, rate gyros, and magnetic sensors to produce yaw, pitch, and roll angle estimates. et donc tu veux "seulement" comprendre le code (pour pouvoir éventuellement le modifier) c'est bien ça?
celà dit ça en fait un produit très interressant car ces traitements m'effraient!

Correction j'ai lu la suite de la data et donc je sais maintenant que le code est open source; j'ai donc ma réponse!

Salut et bienvenue sur le forum.

Tout d'abord je ne sais pas si il existe une endroit ou se présenter sur ce forum, mais bon voila je suis bird12358 et je suis nouveau sur ce forum.

Pour se présenter, il y a une rubrique dédiée : http://www.robot-maker.com/index.php?/forum/27-et-si-vous-vous-presentiez/


Tu pourrais nous mettre un lien vers le code source complet ?

Comment peut-on appliqué à chaqu'un des angles, une correction qui est une différence entre un vecteur d'accélération et sa référence? Il y a pas un problème d'unité?

Kalman est un filtre qui ne prend en compte que des modélisation linéaire. Ici pour passer d'une accélération à un angle, tu as juste une opération d'arcsinus ou d'arctg à effectuer. Et le développement limité à l'ordre 1 de ces 2 opérations, c'est x (l'accélération elle-même).
ça pourrait expliquer cette étape de calcul, mais sans avoir une vue d'ensemble du filtre, difficile de répondre.

Surtout que si c'est un filtre de Kalman étendu, comme j'ai pu le comprendre en lisant le post de hmnrobots, la linéarisation locale se serait faite lors du calcul de la jacobienne ...

Bonjour,

Pour le lien du code, les données sont présentent dans le lien suivant:
http://www.pololu.com/catalog/product/1253/resources

Les codes sources sont présent dans "CHR-6dm AHRS firmware v1.0". Et le calcul dont je parle se situe dans le fichier "chr6dm_states.c", dans les fonctions EstimateStates et EKF_Predict.
La version du kalman utilisé est effectivement le calcul étendu, il y a donc une linéarisation des données tout au lon du calcul. J'avais trouvé dans le ducument ci-dessous, une facon d'extraire les 2 angles (tangage et roulis ) en fonction des 3 accéléromètres:
http%3A%2F%2Fcache.freescale.com%2Ffiles%2Fsensors%2Fdoc%2Fapp_note%2FAN3461.pdf

C'est visiblement ce qui est fait dans la fonction EKF_InitFromSensors pour initialiser les angles à partir des accéléromètres uniquement:

float theta_a = -(180/3.14159)*atan2( (float)sensor_data->accel_x, (float)sensor_data->accel_z );
float phi_a = -(180/3.14159)*atan2( (float)sensor_data->accel_y, (float)sensor_data->accel_z );

Mais je ne comprend pas la partie estimation à partir des accéléromètres, il utilise les accélérations brutes pour compenser les angles d'euler...

Ok, je regarderai ça demain

Mais je ne comprend pas la partie estimation à partir des accéléromètres, il utilise les accélérations brutes pour compenser les angles d'euler...

C'est parce qu'une accélération correspond à un angle autour de 0. (angle_z = sinus(accélération_y / g) = accélération_y / g si l'accélération est proche de 0)

Un petit lien qui pourra-t-être surement utile : http://www.ferdinandpiette.com/blog/les-dossiers/dossier-le-filtre-de-kalman/

Merci déja pour les réponses.

C'est parce qu'une accélération correspond à un angle autour de 0. (angle_z = sinus(accélération_y / g) = accélération_y / g si l'accélération est proche de 0)

Par contre je ne comprend pas ce que vous voulez dire??
Je sais que l'on peut linéairiser autour de zéro donc sin(angle) ~= angle et cos(angle) ~= 1 mais dans ce cas le calcul fait juste:
C = accélération de mesure - angle de référence rotaté --> pour moi même en linearisant ca donne pas ca...

Les équations présentent dans le document dont j'ai mis le lien (cf AN3461.pdf)qui permettent d'obtenir les angles à partir des accéléros sont les suivantes:
soit les valeur issues des accéléros Gp = [Gpx Gpy Gpz]'
Les angles sont obtenus par : Theta = atan2(-Gpx/Gpz) et phi = atan2(Gpy/Gpz) ce sont les calculs présents dans EKF_InitFromSensors pour initialiser le capteur a sa valeur initiale.
Après j'ai certainement dû louper quelquechose en chemin ...

Salut (et bonne année ^^)

Par contre je ne comprend pas ce que vous voulez dire??
Je sais que l'on peut linéairiser autour de zéro donc sin(angle) ~= angle et cos(angle) ~= 1 mais dans ce cas le calcul fait juste:
C = accélération de mesure - angle de référence rotaté --> pour moi même en linearisant ca donne pas ca...

Je viens de lire le code source et en effet, ça ne correspond pas à ce que j'ai dit plus haut.
Tout le code à l'air correct. ça correspond bien aux équations de kalman.

estimated_states->Racc est la matrice de covariance du bruit de mesure.
C est la matrice d'observation (c'est la jacobienne plus précisément)

L c'est le gain de Kalman (L= PC^t.(R+CPC^t)^-1)
La matrice de covariance de l'erreur P est bien mis à jour comme il le faut (P=(I-LC)P)

La mise à jour de l'estimation de l'état courant semble aussi être correcte.
Normalement on prend l'état précédent auquel on ajoute le produit entre le gain de Kalman et l'innovation.
C'est ce qui semble être fait :

// Multiply by Kalman gain
	  	MatVectMult3( &L, &acc_vect, &correction );	// Ici le gain de kalman est bien multiplié par quelquechose qui doit correspondre à l'innovation
	  	
	  	// Apply correction
  		// Ici on ajoute bien à l'état prédit la nouvelle correction calculée
	  	estimated_states->phi = estimated_states->phi + correction.data[0];
	  	estimated_states->theta = estimated_states->theta + correction.data[1];
	  	estimated_states->psi = estimated_states->psi + correction.data[2];

acc_vect doit donc normalement correspondre à l'innovation (innovation = mesure - C.Etat_predit)
Or si on examine le code :

// Now subtract the reference vector
	  	acc_vect.data[0] = acc_vect.data[0] - acc_hat.data[0];
	  	acc_vect.data[1] = acc_vect.data[1] - acc_hat.data[1];
	  	acc_vect.data[2] = acc_vect.data[2] - acc_hat.data[2];

acc_hat.data devrait correspondre à C.Etat_predit

// Compute expected accelerometer output based on yaw, pitch, and roll angles
	  	MatVectMult3( &R, &accel_ref, &acc_hat );

Or ici, on se rend compte que ce n'est pas le cas et qu'on multiplie en réalité la matrice R par une référence d'accélération qui ne dépend pas de l'état prédit.
J'avoue donc que cette dernière ligne me laisse perplexe car R, même si elle intègre le informations du vecteur d'état prédit n'a pas de lien trivial avec la matrice d'observation C (mis à part que c'est bien une matrice 3x3).

Il n'y a probablement pas de problèmes avec le code, mais c'est juste que je n'ai pas compris comment est calculée cette matrice.

Sinon pour le reste du calcul, aucun soucis, ça correspond bien à un Kalman étendu

Comment peut-on appliqué à chaqu'un des angles, une correction qui est une différence entre un vecteur d'accélération et sa référence? Il y a pas un problème d'unité?

Non non, aucun problème d'unité.
La correction n'est pas la différence entre une accélération et sa référence, mais c'est le produit entre le gain de Kalman et l'innovation.

Bonjour et bonne année à vous aussi.Merci d'avoir pris le temps de me répondre.

Pour en revenir au sujet, lorsque l'on fait un filtre de kalman pour moi, il doit se faire de cette façon.
- prediction de l'etat : Xpred = A*Xk-1
- estimation Xest = Xpred + L(Xobs - Xpred) avec innovation =(Xobs - Xpred).

Bon imaginons que les deux XPred n'utilisent pas les memes données , une utilisant les gyros pour le premier et l'autre utilisant le vecteur d'accélération de référence rotaté, c'est ce qui est fait ici. Il faut quand meme que le Xpred (gyro), Xpred(accéléro reference), Xobs(accéléro) aient la même unité sinon ca marche pas dans le calcul de kalman.

Meme si innovation est multiplié par le gain de Kalman, dans le programme l'innovation reste une différence entre deux vecteurs d'accélération et non une différence entre les angles d'euler ... Pour moi, il manque pour Xpred(accéléro reference) et Xobs(accéléro), la transformation en angle d'euler de ces valeurs issues des accéléromètres et de l'accélération de référence. C'est ce qui est fait dans le fonction EKF_InitFromSensors:

float theta_a = -(180/3.14159)*atan2( (float)sensor_data->accel_x, (float)sensor_data->accel_z );float phi_a = -(180/3.14159)*atan2( (float)sensor_data->accel_y, (float)sensor_data->accel_z );

J'ai peutêtre mal compris le kalman mais pour moi lorsque l'on fait une estimation il faut que la mesure utilisé dans l'inovation (et l'inovation elle-même) ait la même unité que la prédiction que vous voulez corriger.
La c'est comme si (en exagérant) on voulait estimer une position et on utilisait une température pour corriger la position. />/>/>

Bon imaginons que les deux XPred n'utilisent pas les memes données , une utilisant les gyros pour le premier et l'autre utilisant le vecteur d'accélération de référence rotaté, c'est ce qui est fait ici. Il faut quand meme que le Xpred (gyro), Xpred(accéléro reference), Xobs(accéléro) aient la même unité sinon ca marche pas dans le calcul de kalman.

Euu, c'est peut-être ici la confusion.

Il n'y a qu'un seul Xpred, et ça correspond aux angles d'euler, qui ne sont pas observables, c'est l'inconnue du problème.
Xobs n'est pas un vecteur d'état, mais un vecteur de mesure que j'aurais appelé Yobs pour éviter la confusion.

Xpred et Yobs sont reliés grâce à la matrice d'observation C (et un bruit en plus)

Bon imaginons que les deux XPred n'utilisent pas les memes données , une utilisant les gyros pour le premier et l'autre utilisant le vecteur d'accélération de référence rotaté, c'est ce qui est fait ici.

Absolument pas
Xpred ne change pas. ça reste toujours le vecteur à estimer, c'est à dire les angles, rien d'autre. On ne cherche pas à estimer les "vrais valeurs" des gyro/accéléro ou magnéto. On utilise les mesures de ces capteurs pour estimer les angles d'euleurs, rien de plus.

Meme si innovation est multiplié par le gain de Kalman, dans le programme l'innovation reste une différence entre deux vecteurs d'accélération et non une différence entre les angles d'euler ...

Je trouve ça intéressent de raisonner avec les grandeurs. Je n'ai jamais vu le problème sous cet angle Pour moi, Kalman, c'est juste un estimateur optimal linéaire gaussien que l'on a écrit en récursif et auquel on a ajouter une évolution aléatoire des paramètres dans le temps. Donc pour moi, je ne remet pas en cause les équations vu qu'elles découlent de l'estimateur optimal

Si on regarde bien les équation, oui, la prédiction est bien la différence entre les mesures (accélération par exemple) et le produit entre la matrice d'observation et le vecteur d'état (ce produit est donc bien homogène au vecteur de mesure)
Reste à voir si le gain de Kalman est sans unité ou au contraire, si son produit avec l'innovation correspond bien au vecteur d'état. (le soucis, c'est qu'on peut mettre des valeurs non homogènes dans ce vecteur, on peut très bien essayer d'estimer en même temps des angles avec des vitesses, etc.)

On a donc le gain de Kalman L = P.H^t.(Racc+HPH^t)^-1
Racc est une matrice de covariance (du bruit de mesure), et donc une erreur
(Racc+HPH^t)^-1 représente donc une variance (on remarque ici que c'est la borne de Cramer-Rao de l'estimateur optimal (H^t.Γ^-1.H)^-1 auquel on a rajouter l'erreur de mesure !)

On a finalement P.H^t.P2 (p2 représentant la variance précédemment calculée)
Et si on regarde les équations de l'estimateur optimal :

P=(H^t.Γ^-1.H)−1
X=P.H^t.Γ^-1.Y


On retrouve bien la borne de Cramer-Rao P et dans la seconde équation, on remarque que P.H^t.P2 (qui représente le gain de kalman) permet de relier le vecteur de mesure (accéléro) au vecteur d'état, l'angle d'euler.

Conclusion, tout va bien

Pour moi, il manque pour Xpred(accéléro reference) et Xobs(accéléro), la transformation en angle d'euler de ces valeurs issues des accéléromètres et de l'accélération de référence. C'est ce qui est fait dans le fonction EKF_InitFromSensors

Ce vecteur accélération de référence n'est pas un vecteur d'état. On ne cherche pas à le prédire. C'est juste un terme correctif constant.

J'ai peut être mal compris le kalman mais pour moi lorsque l'on fait une estimation il faut que la mesure utilisé dans l'inovation (et l'inovation elle-même) ait la même unité que la prédiction que vous voulez corriger.
La c'est comme si (en exagérant) on voulait estimer une position et on utilisait une température pour corriger la position.

Ton exemple d'estimer une position en fonction d'une température est parfaitement réaliste et fonctionnerait avec un Kalman.
Si tu as un modèle de l'évolution de la position en fonction de la température, alors tu as ta matrice d'observation C qui relie l'observation de la température au vecteur d'état (la position.
(exemple simpliste, un parachutiste qui se rapproche du sol. Si on sait modéliser le gradient de température, alors on peut estimer la hauteur du parachutiste en fonction de la température)

En fait dans le code, on a un Xpredit qui est en angle d'euler et une innovation qui est une différence de vecteur d'accélération et ca ne pose pas de soucis?

Si on continu sur l'exemple du parachutiste et du gradiant de température, on aurait eu une prédiction de la hauteur en fonction de la hauteur de départ et du temps depuis la chute et une gradiant de température.
Pour moi si on voulait utiliser le gradiant de température il faudrait avoir une fonction f(gradiant de T) = hauteur, et à ce moment la seulement on aurait pu incorporer le résultant.
La si on continu dans l'exemple (en parallèle de ce qui est fait dans le programme), ce qu'ils font c'est:
- ils calculent une hauteur prédite hpred
- ils calculent un gradiant de témpérature prédit (gpred) à partir de la hauteur prédite hpred
- ils font la différence entre gradiant prédit et le gradiant observé (gobs-gpred), c'est l'innovation.
- et ils appliquent cette innovation (en le multipliant au gain) à la hauteur prédite hpred. hest = Hpred + Lg(gobs-gpred)

C'est ca ou j'ai pas encore compris? Et cette facon de faire ne pose pas de soucis? C'est le gain qui se charge de remettre les deux quantités (Xpred et l'innovation) dans le bonne unité pour que ca colle?

A ma facon j'aurai fait:
- calcul une hauteur prédite hpred
- calcul de la hauteur observé à partir du gradiant observé
- faire la différence entre hauteur prédite et le hauteur observée (Hobs-Hpred)
- et ils appliquent cette innovation (en le multipliant au gain) à la hauteur prédite hpred. hest = Hpred + Lh(Hobs-Hpred)

En fait dans le code, on a un Xpredit qui est en angle d'euler et une innovation qui est une différence de vecteur d'accélération et ca ne pose pas de soucis?

Non, pas de soucis, c'est bien ce que tu dois avoir (les angles étant bien l'état et les accélérations, les mesures).

Si on continu sur l'exemple du parachutiste et du gradiant de température, on aurait eu une prédiction de la hauteur en fonction de la hauteur de départ et du temps depuis la chute et une gradiant de température.
Pour moi si on voulait utiliser le gradiant de température il faudrait avoir une fonction f(gradiant de T) = hauteur, et à ce moment la seulement on aurait pu incorporer le résultant.

Mais c'est exactement ce que l'on a ici
Ta fonction f correspond à la fonction d'observation qui est ici linéarisé localement grâce à sa matrice jacobienne C.
La hauteur est ton vecteur d'état, le gradiant est le vecteur de mesure et C est la jacobienne de f qui permet de relier l'état à la mesure.

La si on continu dans l'exemple (en parallèle de ce qui est fait dans le programme), ce qu'ils font c'est:
- ils calculent une hauteur prédite hpred
- ils calculent un gradiant de témpérature prédit (gpred) à partir de la hauteur prédite hpred
- ils font la différence entre gradiant prédit et le gradiant observé (gobs-gpred), c'est l'innovation.
- et ils appliquent cette innovation (en le multipliant au gain) à la hauteur prédite hpred. hest = Hpred + Lg(gobs-gpred)

C'est ca ou j'ai pas encore compris? Et cette facon de faire ne pose pas de soucis? C'est le gain qui se charge de remettre les deux quantités (Xpred et l'innovation) dans le bonne unité pour que ca colle?

Exactement, c'est tout à fait ça.

A ma facon j'aurai fait:
- calcul une hauteur prédite hpred
- calcul de la hauteur observé à partir du gradiant observé
- faire la différence entre hauteur prédite et le hauteur observée (Hobs-Hpred)
- et ils appliquent cette innovation (en le multipliant au gain) à la hauteur prédite hpred. hest = Hpred + Lh(Hobs-Hpred)

ça ne marche pas. Car le gain de kalman fait le lien entre l'état et l'observation. Tu ne peux donc pas appliquer ce gain à la hauteur, mais bien au gradiant de température afin de retrouver ton vecteur d'état, la hauteur.

ça ne marche pas. Car le gain de kalman fait le lien entre l'état et l'observation. Tu ne peux donc pas appliquer ce gain à la hauteur, mais bien au gradiant de température afin de retrouver ton vecteur d'état, la hauteur.

Je ne comprends pas trop on peut tres bien se créer une fonction f(gradiant température) = Hobs, ca ferait un "pseudo capteur" et donc l'etat observé serait Hobs et non plus gobs?? Comme on aurait pu transformer les accélérations en angles d'euler observé et avoir un gain de kalman modifié dans le deux cas?
Je dis ca parle que j'ai déja vu dans certain autre programme des choses comme ca. Ils prédisent avec les gyros (on a donc les angles prédits) et ils estiment avec les valeurs d'accéléros (transformé en euler par les formules que j'ai cité la les posts précédent).
Ca ne serait donc pas la bonne méthode?

Je suis en train de consulter le wiki sur le kalman, c'est effectivement dans l'autre seul:
http://fr.wikipedia.org/wiki/Filtre_de_Kalman

Dans l'innovation, on part de l'état prédit et on arrive dans la même unité que Z l'état observé par la matrice H.

--> Si on en revient au programme, C serait la forme linéarisé (sous wiki H linéarisé) de la matrice de rotation R? Mais par rapport à quoi?

Je ne comprends pas trop on peut tres bien se créer une fonction f(gradiant température) = Hobs, ca ferait un "pseudo capteur" et donc l'etat observé serait Hobs et non plus gobs?? Comme on aurait pu transformer les accélérations en angles d'euler observé et avoir un gain de kalman modifié dans le deux cas?

Aleale, je ne te suis plus du tout sur ce coup-ci !
Pourquoi parles-tu de pseudo-capteur ?

Si on reprend à la base, que ce soit pour n'importe quelle estimateur (moindre carrée comme Kalman). La première chose à faire, c'est de modéliser le système.
Tu as ton inconnue qui est le vecteur d'état, noté x.
Tu as ton observation qui est le vecteur de mesure, noté y.
Tu sais qu'une fonction h relie le vecteur d'état au vecteur de mesure.
En plus de ça, tu as du bruit b qui s'ajoute à tes capteurs.

Si ton système est linéaire, tu peux modéliser tout ça de manière matricielle : Y = H.X+B (sinon, ça reste une équation du type y = h( x , b ))

Par exemple, y est l'accélération selon l'axe y de l'accéléromètre, x est l'angle que forme l'accéléromètre par rapport au sol.
Pour relier les deux tu sais qu'il faut prendre l'arcsinus de la mesure d'accélération divisé par la constante de gravité terrestre.
Tu as donc y = arcsin(x/g) + b
Voila comment tu transformes une accélération en angle. Tout est contenue dans la fonction h.
Dans ton code source, cette fonction h est représenté par la matrice C qui correspond à la jacobienne de h (ça permet de linéariser localement cette fonction !)

Je dis ca parle que j'ai déja vu dans certain autre programme des choses comme ca. Ils prédisent avec les gyros (on a donc les angles prédits) et ils estiment avec les valeurs d'accéléros (transformé en euler par les formules que j'ai cité la les posts précédent).

Tu ne peux pas avoir plusieurs vecteur à estimer avec un seul filtrage de Kalman. Soit tu as plusieurs filtres de Kalman, soit tu en a un seul unique et gros.
Après, il se peut que certaines composantes de ce vecteur n'entrent pas en jeux lors du calcul de la prédiction... mais ça, c'est une histoire de modélisation.

Je suis en train de consulter le wiki sur le kalman, c'est effectivement dans l'autre seul:
http://fr.wikipedia....iltre_de_Kalman

Dans l'innovation, on part de l'état prédit et on arrive dans la même unité que Z l'état observé par la matrice H.

--> Si on en revient au programme, C serait la forme linéarisé (sous wiki H linéarisé) de la matrice de rotation R? Mais par rapport à quoi?

Oui, je préfère les notations de wiki
Non, pas exactement. Une matrice, c'est déjà du linéaire ! (avec des matrices, tu ne peux faire que des combinaisons linéaires)
C (ou H) est la forme linéarisée de la fonction d'observation h (dans la modélisation y = h( x , b )
On linéarise par rapport à quoi ? Par rapport aux composantes des vecteurs d'états.

Pour rappel, si ton système est linéaire (que l'équation y = h( x , b ) peut s'écrire directement sous forme matricielle) alors tu appliques simplement un Kalman.
Sinon, si ton équation y = h( x , b ) ne peux pas s'écrire sous forme linéaire, alors tu utilises un Kalman étendu (c'est ton cas). Ce filtre va venir linéariser localement ta fonction h (ainsi que la fonction de transition f). C'est le but de la jacobienne



Ce n'est pas de la pub, mais j'ai essayé d'expliqué tout ça dans mes articles, je te conseil de les lires.
Outre le premier article qui est une vue d'ensemble du filtre et le second qui est mathématiques, tu as justement un exemple de Kalman simple pour un début de centrale inertielle. Ainsi qu'un second exemple du même problème, mais avec un Kalman étendu.
http://www.ferdinand...ltre-de-kalman/

Ok donc H ou C est bien la fonction d'observation linéarisé. Dans la formule que vous avez donné:
y = arcsin(x/g) + b
A quoi correspond b? Et cette formule est valable qu'en 2D pas en 3D? parce que vu la complexité de la matrice C (qui est donc la fonction d'observation H linéarisé), on a une formule plus complexe de H qui lie les angles aux valeurs d'accélérations?
La matrice C vaut dans le code:

 // Compute C matrix for Kalman gain calculation
		  /*
[  - az*cos(theta) - ax*cos(psi)*sin(theta) - ay*sin(psi)*sin(theta),    ay*cos(psi)*cos(theta) - ax*cos(theta)*sin(psi)]
[ ax*(sin(phi)*sin(psi) + cos(phi)*cos(psi)*sin(theta)) - ay*(cos(psi)*sin(phi) - cos(phi)*sin(psi)*sin(theta)) + az*cos(phi)*cos(theta), ax*cos(psi)*cos(theta)*sin(phi) - az*sin(phi)*sin(theta) + ay*cos(theta)*sin(phi)*sin(psi), - ax*(cos(phi)*cos(psi) + sin(phi)*sin(psi)*sin(theta)) - ay*(cos(phi)*sin(psi) - cos(psi)*sin(phi)*sin(theta))]
[ ax*(cos(phi)*sin(psi) - cos(psi)*sin(phi)*sin(theta)) - ay*(cos(phi)*cos(psi) + sin(phi)*sin(psi)*sin(theta)) - az*cos(theta)*sin(phi), ax*cos(phi)*cos(psi)*cos(theta) - az*cos(phi)*sin(theta) + ay*cos(phi)*cos(theta)*sin(psi),   ax*(cos(psi)*sin(phi) - cos(phi)*sin(psi)*sin(theta)) + ay*(sin(phi)*sin(psi) + cos(phi)*cos(psi)*sin(theta))]
		  */

Elle serait égale à quoi cette fonction H qui lie les angles aux accélérations? ca serait H = R*Accref (ou R est la matrice de rotation sur les 3 axes et Accref l'accélération de référence)?

Ok donc H ou C est bien la fonction d'observation linéarisé. Dans la formule que vous avez donné:
y = arcsin(x/g) + b
A quoi correspond b?

On peut se tutoyer (comme tout le monde ici)
b, c'est du bruit du as tes capteurs. C'est à cause de ça que l'on ne connaitra jamais précisément l'inconnue x.
Le but de Kalman, c'est d'estimer au mieux cette inconnue, c'est à dire essayer de supprimer au maximum ce bruit.

Et cette formule est valable qu'en 2D pas en 3D? parce que vu la complexité de la matrice C (qui est donc la fonction d'observation H linéarisé), on a une formule plus complexe de H qui lie les angles aux valeurs d'accélérations?

Oui, mon exemple est super simpliste et en 2D uniquement. C'était pour faire comprendre le principe.

Elle serait égale à quoi cette fonction H qui lie les angles aux accélérations? ca serait H = R*Accref (ou R est la matrice de rotation sur les 3 axes et Accref l'accélération de référence)?

Bonne question, ça doit pouvoir se trouver sur internet, ou se refaire à la main si tu n'as pas peur de faire un peu de trigo 3D.
Toutes les formules de changement de repères cartésienne vers sphériques sont utiles.
Ou alors en intégrant la jacobienne C tu retrouvera ta fonction, mais est-ce vraiment utile de savoir ça ?

Ah ok b c'est le bruit gaussien d'écart type que l'on peut trouver en faisant des mesures directement sur le capteur.

Pour moi la matrice d'observation serait simplement R*Accref. R contient la rotation selon les 3 axes, donc les 3 angles d'Euler et le calcul de H donne la valeur d'accélération selon les 3 axes ? Sachant que Accref est [0 0 - ou + g].

De plus quand je commence à linéairiser selon phi,theta et psi j'ai l'air de tomber sur les mêmes résultats que ceux contenus dans la matrice C.

En fait je suis curieux et je voulais comprendre l'intégralité du code pour pouvoir me refaire une centrale inertielle basique. J'ai pu rencontrer quelques exemples basés sur des algos beaucoup plus simpliste. Mais c'est toujours plus intéressant de se casser les dents sur des exemples plus complexe.

Visiblement, les concepteurs de cette IMU vont améliorer encore leur algo en incorporant la correction des biais, de la température, ... J'espère qu'il remettrons à disposition leur code source ...

Visiblement, les concepteurs de cette IMU vont améliorer encore leur algo en incorporant la correction des biais, de la température, ... J'espère qu'il remettrons à disposition leur code source ...

Oui, c'est tout le problème avec les centrale inertielle. Aucun capteur n'est parfait.
Les gyro ont une dérive dans le temps qu'il faut annuler. Les accéléro prennent en compte à la fois l'accélération propre de la centrale et la gravité terrestre, il faut réussir à séparer ses deux grandeurs, les magnéto ne sont pas précis du tout et possèdent énormément de bruits.

Tellement de contraintes qu'il faut faire des compromis...
En tout cas, c'est un bon exo, mais sache qu'il n'y a pas que Kalman pour faire une bonne centrale intertielle. Il existe d'autres techniques comme les filtres complémentaires ou encore DCM.
Après tout dépend de l'application que tu comptes faire de la centrale.

Merci encore d'avoir passer du temps sur mon problème.

Dernières questions concernant la mise en oeuvre du kalman:
- Comment fait-on pour creer une kalman à 100Hz quand on a des données à 10Hz à 50Hz et à 100Hz? Pour les données en dessous de 100Hz, on utilise la dernière donnée reçu pour le filtre à 100Hz ou faut-il faire une prédiction de cette valeur en fonction du comportement des n derniers points?

- Imaginons que l'on ait plusieurs données observé pour estimer un état. Je suis tombé sur internet sur le filtrage de position par accéléromètre, vision et GPS. On a donc de ces 3 capteurs une informations de vitesse que l'on intégre. Comme va-t-on devoir traiter ces données.

Pour en revenir aux alternatives du Kalman, les autres possibilités, le DCM et le filtre complémentaire sont-ils vraiment plus intéressant en terme de validité des résultat? J'ai vu que le DCM était utilisé pour estimer l'attitude d'un hélicoptère par exemple.

- Comment fait-on pour creer une kalman à 100Hz quand on a des données à 10Hz à 50Hz et à 100Hz? Pour les données en dessous de 100Hz, on utilise la dernière donnée reçu pour le filtre à 100Hz ou faut-il faire une prédiction de cette valeur en fonction du comportement des n derniers points?

Bonne question.
Déjà, tu ne peux pas réappliquer un cycle de Kalman sur des données déjà traités pour la simple et bonne raison que dans la phase de prédiction, tu considères que la donnée a été calculée pour t+dt. Donc il y aura un problème lors de la phase de mise à jour (vu que la donnée n'a pas été prise à t+dt)
A la limite, si tu as des données à 10Hz, tu peux faire uniquement l'étape de prédiction à une fréquence de 100Hz puis, à chaque fois que tu as une nouvelle information, tu applique en plus la mise à jour pour rectifier tes prédictions.

Autre solution, c'est de décomposer ton Kalman en plusieurs petits Kalman et d'appliquer les cycles à la même fréquence que tu reçois tes données. Je n'ai jamais fait comme ça, mais c'est ce qui semble être fait dans ton code (l'accéléro et le magnéto sont traités à l'aide de deux Kalman différents)

- Imaginons que l'on ait plusieurs données observé pour estimer un état. Je suis tombé sur internet sur le filtrage de position par accéléromètre, vision et GPS. On a donc de ces 3 capteurs une informations de vitesse que l'on intégre. Comme va-t-on devoir traiter ces données.

Là, à mon avis le mieux, c'est d'utiliser les donées brutes des capteurs, sans essayer de faire des prétraitements pour calculer la vitesse.
Par exemple, on a un GPS qui te donne une position en x et un accéléromètre qui te donne une accélération selon x
Ton vecteur de mesure est donc [x_m ; ax_m] (le _m pour dire que ce sont des valeurs mesurées)
Tu cherches à estiver accélération, vitesse et position. Ton vecteur d'état sera donc [ax ; vx ; x]

A partir de ça, tu trouves ta fonction d’observation qui est linéaire ici : ax = ax_m + b_ax et x = x_m + b_x
Tu réécrit ça sous forme matricielle
[x_m ; ax_m] = [0 0 1 ; 1 0 0] . [ax ; vx ; x] + [b_x; b_ax]

Maintenant, pour finir la modélisation, il ne te manque plus que la fonction de transition qui elle aussi est linéaire :
a(k+1) = a(k)
v(k+1) = v(k) + a(k).dt
x(k+1) = x(k) + v(k).dt + a(k).dt²/2

Sous forme matricielle, ça donne
[ax ; vx ; x](k+1) = [1 0 0 ; dt 1 0 ; dt²/2 dt 1].[ax ; vx ; x](k)

Après, il ne te manque plus qu'a déterminer les matrices de covariances du bruit de mesure R = [sigma_mesure_position² 0 ; 0 simga_mesure_acceleration²] et du bruit de commande Q = [simga_modelisation_acceleration² 0 0 ; 0 simga_modelisation_vitesse² 0 ; 0 0 simga_modelisation_position² ]

Et le tour est jouée
Reste plus qu'a initialiser correctement et appliquer Kalman.

Pour en revenir aux alternatives du Kalman, les autres possibilités, le DCM et le filtre complémentaire sont-ils vraiment plus intéressant en terme de validité des résultat? J'ai vu que le DCM était utilisé pour estimer l'attitude d'un hélicoptère par exemple.

Même si je trouve que mathématiquement parlant, Kalman, c'est super séduisant, il faut bien avoué que dans la pratique, ça consomme énormément de ressources et que d'autres méthodes comme le filtrage complémentaire (beaucoup plus simple) fournis des résultats équivalent au Kalman.
Je n'ai jamais fait de comparatifs moi-même, mais ça se trouve sur internet, et il est souvent difficile de classer ces algos par performances...