Une centrale inertielle est un instrument de mesure de l'accélération et de la vitesse angulaire composé de trois accéléromètres et de trois gyroscopes. Estimer l'accélération et la vitesse angulaire pourrait sembler trivial dans la mesure où un accéléromètre fournit une mesure d'accélération et un gyroscope, une mesure de vitesse angulaire. Néanmoins, nous allons voir que dessous cette apparente facilité se cache des difficultés bien réelles.
Le but de cet article n'est pas de recréer une centrale inertielle, mais simplement d'essayer d'estimer l'angle d'inclinaison ainsi que la vitesse angulaire d'un objet selon un axe uniquement à l'aide d'un filtre de Kalman. Ce problème nous permettra non seulement de toucher du doigt la complexité de la mise en œuvre d'une centrale inertielle, mais aussi et surtout d'utiliser un filtre de Kalman afin de faire de la prédiction d'information et de la fusion de données multi-capteurs.
Prérequis : Pour lire cet article le plus confortablement possible, je vous conseille de vous familiariser avec le filtre de Kalman. Vous trouverez d'autres articles parlant de ce filtre sur ce site :
- Le filtre de Kalman : intérêts et limites
- De l'estimateur optimal au filtre de Kalman (article mathématiques)
I. Les capteurs
Nous disposons de deux capteurs pour cet exemple :
- Un accéléromètre détectant l'accélération d'un mobile selon l'axe Y. Cet accéléromètre nous renvoie des valeurs d'accélérations bruitées.
- Un gyroscope détectant la vitesse angulaire selon l'axe X (donc la vitesse à laquelle le mobile s'incline). Ce gyroscope nous renvoie des valeurs de vitesses angulaires bruitées et biaisées.
Toute la difficulté du problème vient du fait que le gyroscope possède un biais évoluant dans le temps ! C'est à cause de ce biais que l'on dit que les gyroscopes dérivent. Ce qui est problématique, c'est que l'on ne connait pas la forme de la dérive. Du coup, si on se contente d'un seul gyroscope, on aura l'impression que le mobile s'incline lentement alors qu'en réalité, celui-ci ne bouge pas !
Avant d'entamer une modélisation du système, nous allons émettre quelques hypothèses concernant le mobile et les capteurs.
- Tout d'abord, on considère que le mobile ne subit aucune accélération
- Ensuite, on considère les bruits des deux capteurs comme gaussiens
- On considère que l'accéléromètre nous revoie des valeurs d'accélérations directement en mètres par seconde carré
- On considère que le gyroscope nous renvoie des valeurs de vitesses angulaires directement en radians par seconde
- Enfin, on considère le biais du gyroscope à l'instant initial comme nul. (ce qui n'est pas trop compliqué à mettre en place en pratique)
II. Les paramètres à estimer
Comme vous vous en êtes rendu compte, l'accéléromètre ne va pas servir à mesurer une accélération du mobile car on a supposé que le mobile n'accélérait pas ! En réalité, l'accéléromètre va nous servir à obtenir une mesure de l'angle d'inclinaison du mobile ! Comment est-ce faisable ? Tout simplement parce que la force de gravitation n'est rien d'autre qu'une accélération !
Mettez votre accéléromètre selon l'axe Y et vous ne verrez aucune accélération. Mais orientez le maintenant complètement vers le sol (selon l'axe Z). Vous obtiendrez une valeur non nul en sortie de votre accéléromètre. Cette valeur sera d'environ 9.8 mètres par seconde carré, soit l'équivalent de la constante de pesanteur sur Terre (un g).
Pour obtenir une mesure de l'angle d'orientation du mobile, il suffit donc de prendre moins l'arc sinus du rapport entre l'accélération fournis par l'accéléromètre et la constante de pesanteur g.
Côté gyroscope, nous pouvons obtenir une mesure de la vitesse angulaire du mobile (certes biaisée). Pour obtenir l'angle d'orientation du mobile, il suffit donc d'intégrer la vitesse angulaire par rapport au temps. On peut donc remarquer que cette information n'est pas très précise, car le fait d'intégrer dans le domaine discret (on n'a que des échantillons de la vitesse angulaire) va inéluctablement engendrer une dérive (en plus de celle apporté par le biais du gyroscope lui-même).
Nous chercherons donc, dans cet exemple, à estimer l'angle du mobile, sa vitesse angulaire ainsi que le biais du gyroscope à l'aide d'un accéléromètre et d'un gyroscope.
III. La modélisation du problème
Pour appliquer un filtre de Kalman, il faut d'abord et avant tout modéliser le problème en fonction des paramètres à estimer et des mesures des capteurs.
La mesure de l'angle d'inclinaison du mobile par l'accéléromètre (
) s'obtient en ajoutant le bruit de l'accéléromètre (
) à la vraie valeur de l'angle d'inclinaison du mobile (
)
La mesure de la vitesse angulaire du mobile par le gyroscope (
) s'obtient en ajoutant le bruit du gyroscope (
) et le biais de celui-ci (
) à la vraie valeur de la vitesse angulaire du mobile (
)
On en déduit les équations suivantes :
On peut écrire ces équations sous la forme matricielle 
que l'on appelle l'équation de mesure
- Y s'appelle le vecteur de mesure
- X est le vecteur d'état
- B, le vecteur de bruit
- H, la matrice d'observation
Il faut aussi modéliser l'évolution des paramètres à estimer dans le temps.
Ici, ne connaissant pas le modèle d'évolution du biais du gyroscope, on va le considérer comme fixe : 
Le mobile pouvant faire n'importe quoi, nous ne connaissons pas non plus le modèle d'évolution de la vitesse angulaire du mobile. On va donc aussi la considérer comme étant fixe : 
Enfin, on peut exprimer l'inclinaison du mobile en fonction de la vitesse angulaire à l'instant précédent : 
étant la période d'échantillonnage du système (on récupère les mesures des capteurs de manière discrète).
Ici encore, on peut réécrire ces équations sous la forme matricielle 
appelée l'équation d'état
est le nouveau vecteur d'état
est l'ancien vecteur d'état- A est la matrice de transition
Pour pouvoir utiliser le filtre de Kalman, il nous manque deux matrices apparaissant dans les calculs : la matrice de covariance du bruit de mesure et la matrice de covariance du bruit de commande.
Comme on a deux capteurs avec des bruits gaussiens, la matrice de covariance du bruit de mesure (matrice d'auto-corrélation) sera une matrice diagonale de dimension deux avec, pour valeurs des termes sur la diagonale, le carré de l'écart-type du bruit des capteurs. (En effet, les bruits des capteurs sont considérés comme gaussiens et décorrélés entre eux)
est l'écart type du bruit du gyroscope
est l'écart type du bruit de l'accéléromètre
Enfin, la matrice de covariance du bruit de commande représente les erreurs de modélisation du système. C'est grâce à cette matrice que les erreurs commises lors de la modélisation (ici, on a estimé que le biais et la vitesse angulaire étaient constants alors qu'en réalité, c'est faux) seront "gommées". Cette matrice sera une matrice diagonale de dimension trois (on a un vecteur d'état de taille 3). Les termes sur la diagonale correspondent au carré des écart-types maximaux de l'erreur que l'on autorise pour chacun des paramètres à estimer. Cette matrice est à déterminer empiriquement en fonction des données du problème.
Il faut avoir conscience que si on définie des termes d'erreur trop petit par rapport à la réalité, alors le filtre de Kalman n'arrivera pas à rectifier les erreurs du modèle et fera des estimations biaisés. Par contre, si les termes d'erreurs sont trop importants par rapport à la réalité, alors le modèle ne sera pas biaisé, mais les estimations produites seront de piètre qualité (la covariance de l'erreur sera importante)
La difficulté du filtre de Kalman est donc de bien estimer cette matrice de covariance du bruit de commande afin d'avoir une estimation la plus précise possible sans pour autant avoir de biais.
est la variance maximale de l'erreur autorisée sur la modélisation de la vitesse angulaire (équation d'état)
est la variance maximale de l'erreur autorisée sur la modélisation de l'inclinaison (équation d'état). Dans notre exemple, cette valeur doit être contrainte à 0. En effet, le modèle pour l'inclinaison a été parfaitement déterminé en fonction de l'inclinaison précédente et de la vitesse angulaire précédente. Il n'y a pas d'erreur possible.
est la variance maximale de l'erreur autorisée sur la modélisation du biais du gyroscope (équation d'état)
4. Application du filtre de Kalman
Maintenant que l'on a modélisé le système et déterminé toutes les matrices indispensables, nous pouvons appliquer le fameux filtre de Kalman :
La phase de prédiction
La phase de mise à jour
Vous reconnaissez donc 
comme étant l'estimation du nouveau vecteur d'état et 
comme étant la nouvelle matrice de covariance de l'état estimé. Cette dernière matrice vous renseignera sur la précision de l'estimation de chaque paramètre.
Il vous suffit donc d'appliquer ces deux phases à chaque fois que vous obtenez de nouveaux échantillons provenant des capteurs.
5. Simulation sous MatLab
Voici le résultat de la simulation de cet exemple sous Matlab.
J'ai pris une vitesse angulaire réelle sinusoïdale, j'ai calculé l'angle d'inclinaison réel du mobile, j'ai fait des mesures de la vitesse angulaire et de l'angle d'inclinaison en rajoutant le bruit très important et le biais du gyroscope afin de simuler les données capteurs reçu. Enfin, j'ai passé ces données capteurs dans la moulinette de Kalman et j'ai obtenu les résultats suivants :
On s'aperçoit que malgré le bruit important, le filtre de Kalman a réussi à estimer assez correctement le biais du gyroscope, ce qui nous permet donc de retrouver la vitesse angulaire du mobile précisément !!
Beaucoup de personnes me demande le code de ma simulation. C'est pourquoi j'ai décidé de le distribuer, bien que le code ne soit pas très propre, ni bien commenté. Pour afficher le code, il suffit de cliquer ici.
6. Conclusion
Nous avons donc vu un exemple illustrant l'utilisation du filtre de Kalman pour faire de la fusion de donnée multi-capteurs afin d'estimer les paramètres de notre système.
Bien sûr, dans un système réel, bien souvent, le mobile subit des accélérations, ce qui fait que cette méthode n'est pas applicable tel quelle. En effet, dans le cas d'une accélération non nul du mobile, les données obtenues par l'accéléromètre sera donc la somme de l'accélération et de la gravité terrestre, ce qui ne nous permet plus d'estimer l'inclinaison du mobile en prenant l'arc sinus de cette valeur !
Là est toute la difficulté lorsque l'on souhaite créer une centrale inertielle performante.
(Pour encore aller plus loin, découvrez le filtre de Kalman étendu : http://www.ferdinandpiette.com/blog/2011/05/le-filtre-de-kalman-etendu-principe-et-exemple/)
Bonjour,
Tout d'abord merci pour ces différents post qui m'on permis d'y voir beaucoup plus clair dans kalman.
Il serait par contre intéressant que vous ajoutiez dans votre exemple un paramètre "control input" (correspondants aux matrice Bk.uk) pour voir comment il s'intègre dans un kalman.
Cordialement, Peamon.
Bonjour,
Merci pour vos remarques.
En effet, ça serait intéressant d'ajouter un tel exemple.
, il faudrait que je comprenne à quoi sert exactement un tel vecteur !
Je n'ai jamais eu besoin d'intégrer une entrée de commande
La matrice P_k n'est pas documentée il me semble.
Sinon merci pour cet article très bien écrit!
En effet, je n'ai pas parler de l'initialisation du filtre de Kalman dans cet exemple.
Je rajouterais une partie dessus prochainement.
Merci pour le retour !
Ferdi
Bonjour,
merci pour ces posts sympatiquement rédigés 🙂
Est ce qu'il serait possible de récupérer les fichiers matlab pour tester le filtre avec des données réelles?
Merci,
vince
merci
et merci une autre fois
très bon comme article
Bonjour ,
Merci pour votre superbe article . C'est très clairement expliqué ! est-il possible de récupérer vos simulations ?? et une question : est-il possible de faire le meme modélisation en fixant l'angle et cherchant l'accélération ??
Bonsoir,
Vous pouvez appliquer un filtre de Kalman dans un modèle ou vous fixez l'angle et où vous chercher l'accélération.
La question à vous poser sera : "Est-ce que les capteurs que j'utilise apporte une quelconque information en relation avec mes paramètres à estimer ?"
Donc à vous de voir si vous trouvez un lien entre le gyroscope et l'accélération du mobile dans votre application (ce qui m'étonnerais à première vue)
Bonsoir,
Très intéressé par votre présentation, j'aimerais par contre ''sauter'' sur la réponse donnée à jasuh qui soulève un point d'ombre à mon niveau. Très novice dans le domaine et travaillant avec un accéléromètre couplé à un gyroscope (milieu sportif et bio-médical), l'affirmation que suscite une telle réponse est que cela ne sert à rien au final d'avoir un matériel pareil vu qu'on ne sait pas ce que l'on mesure finalement. Parler de la précision de mesure d'un tel outils serait ''pur fantasme'' pour le moment difficilement concevable ???!!!???
Bonjour,
Je ne comprend pas vraiment la question que vous posez.
Bien sur qu'un tel matériel est utile ! Mais pour pouvoir en tirer les informations utiles, il faut passer par une phase de modélisation. Plus la modélisation du système est précise et plus les résultats seront pertinent. C'est d'ailleurs le point le plus dur dans le filtre de Kalman. Une fois que le système est modélisé, le reste, c'est du gâteau.
La précision d'un filtre de Kalman dépend donc de la précision de la modélisation. Néanmoins, il est possible de mesurer le degré de précision d'un tel filtre grâce à la matrice de covariance Pk.
De plus, le filtre de Kalman n'est qu'une méthode d'estimation parmi d'autre. Pour une centrale inertielle par exemple, il est aussi possible d'utiliser le filtrage complémentaire qui permet d'obtenir de bon résultat.
Un grand merci pour ce topo sur le filtre de Kalman que je considérais, après l'avoir étudié il y a environ 6 ans, être un nom d'oiseau : la bête noire par excellence et cela malgré quelques fouilles sur la toile pour me soigner mais qui sont restées vaines. Et là, c'est la révélation : l'euphorie d'une réconciliation avec un outil fort puissant qu'il peut être utile d'avoir sous le coude et dont l'utilisation éventuelle se révèle désormais possible.
Un lien à diffuser sans modération.
Un Grand Merci !
Damien
PS pour passer de la modélisation à l'intégration :
http://invensense.com/mems/gyro/mpu6000.html
Bonsoir,
Merci pour toute ces informations sur le filtre de Kalman.
Je suis étudiante de maths,je fais une simulation avec Matlab sur le filtre de kalman pour estimer la position d'un véhicule sachant qu'il n y a pas de contrôle (c'est juste une application pour voir la validité du filtre), mon problème c'est : comment je dois choisir le bruit d'état?
Dans mes résultats j'ai trouvé que la courbe de la matrice de covariance de l'erreur de l'estimation est croissante et je pense qu'elle doit être décroissante, non?!!
Bonjour,
En effet, si l'erreur augmente avec les itérations, c'est qu'il y a un problème.
Pour le bruit du modèle, la matrice Q va permettre de rectifier l'estimation si jamais l'objet ne suit pas scrupuleusement le modèle. Ainsi, cette matrice de covariance Q se verra attribuer les erreurs (variance) maximale que l'on autorise sur le modèle.
(dans notre cas, on sait par exemple que le biais du gyroscope ne variera pas de plus de 0.1 rad/s par minute, à partir de ça, on peut donc déterminer le coefficient epsilon_b de la matrice Q)
Il n'y a pas de règle générale pour déterminer les coefs de cette matrice. ça se fait souvent empiriquement.
Merci beaucoup ,c'est vraie que j'ai eux un problème dans mon modèle.
Mon vecteur d'état est {position vitesse}^t est dans R^2
c'est-à-dire que la covariance de l'erreur d'estimation de la position P est une matrice de dimensions 4, donc j'ai simulé juste la première composante (1,1) de la matrice P je crois que c'est la variance de l'erreur d'estimation de la position.Es ce que ceci est vrai?
Si oui, dès que j'ai modifié les matrices de covariances des bruits d'état et d'observation, j'ai obtenu de très bons résultats est la de covariance de l'erreur de l'estimation s'approche de 0.
Un grand merci...(et pardon parce que c'est des questions pour une débutante dans la recherche)
C'est tout à fait ça.
Si le vecteur à estimé contient la position en première composante et la vitesse en seconde, alors la matrice de covariance de l'erreur contiendra la variance de l'erreur de l'estimation de la position en position (1,1) et la variance de l'erreur en vitesse en position (2,2) (sur la diagonale donc)
Bonjour,
j'ai lu vos différent articles avec grand intérêt car je cherche à améliorer la qualité de l'angle d'inclinaison. Ce dernier ayant pour l'application la consigne d'un correcteur pour piloter les moteurs d'un quadricopter.
Si j'ai bien compris votre exemple :
On a :
H = (1 0 1)
(0 1 0)
Γ = (var_noise_acc 0 ) que je vais noter : Γ = (X 0)
( 0 var_noise_gyro) (0 Y)
Ainsi on peut calculer Pk :
Pk = Matrice inverse[ transposée(Hk) . Matrice inverse( Γk ) . Hk ]
cf : article De l'estimateur optimal au filtre de Kalman
Matrice inverse(Γk ) = (1/X 0 )
( 0 1/Y)
Je suppose que ces matrices sont fixes dans le temps.
Le résultat de cette multiplication : [ Transposée(Hk) . Matrice inverse( Γk ) . Hk ] est égal à :
( 1/X 0 1/X )
( 0 1/Y 0 )
( 1/X 0 1/X )
Or cette matrice n'est pas inversible.
Ai-je fais une erreur ?
Pouvez-vous me donner plus de détails sur le calcul de Pk.
Merci pour votre réponse.
Bonjour,
Le problème vient du fait que la formule Pk = Matrice inverse[ transposée(Hk) . Matrice inverse( Γk ) . Hk ] est valable pour l'estimateur optimal, mais plus pour le filtre de Kalman (Hk est différents).
Pk doit être calculé récursivement. On le prédit grâce à la modélisation de l'évolution du modèle (Pk+) et on le met à jour en fonction des mesures capteurs (Pk+1)
Bonjour, je suis un débutant en ce qui concerne le filtre de Kalman, pouvez-vous m'envoyer la simulation sous MatLab s'il vous plaît? Merci énormément pour ces explications.
Hello,
Merci pour ces superbes tutoriaux qui rafraîchissent bien la mémoire et remettent les neurones en place ;-).
Etant donné que l on utilise un gyroscope et un accéléromètre je me disait si le mobile se déplace le long d un cercle en gardant la tangente de ce cercle comme direction la composante d accélération comportera l acc gravitationnelle plus acc même du mobile au bout d un moment l erreur dut a l accélération du mobile ne sera plus une erreur.
Donc d ou l utilité d un magnétomètre comme capteur!?
Mais les donnes du magnétomètre ne sont pas linéaire avec la position angulaire du mobile!?
Si elle ne le sont pas peut on rester sur le filtre de Kalman ou plutôt le filtre de Kalman étendu?
Dsl çà fait déjà plusieurs questions 😉
Encore merci
Ghis
Bonjour,
Le magnétomètre renvoi une mesure d'angle. Le filtre de Kalman reste donc linéaire puisque l'angle est un paramètre à estimer.
Après, il se peut que ce magnétomètre renvoient des données numériques non proportionnelles avec l'angle réel (ce qui me semble normal pour un capteur IR par exemple, mais bizarre pour un magnétomètre). Il suffit dans ce cas de "linéariser" la mesure avant de l'injecter dans Kalman, en s'aidant de la datasheet du capteur.
En ce qui concerne votre problème du déplacement en cercle, il est vrai que l'hypothèse (très contraignante) que j'ai faite, à savoir négliger l'accélération propre du mobile, n'est plus vrai. Il faut donc modéliser le problème autrement. Par exemple, appliquer un filtre passe haut < 0.1Hz sur les données du gyroscope, pour supprimer la dérive de celui-ci, on a ainsi un paramètre de moins à estimer. Le problème, c'est que pour des mouvements de rotation uniforme, le gyroscope détectera plus rien (à cause du filtre passe haut). Dans ce cas, l'ajout d'un magnétomètre peut aider ! Mais là, c'est un autre problème, et donc il faut une autre modélisation ^^
bonjour,je m’intéresse au filtre de kalman pour évaluer le pib potentiel des pays de la cemac sous une allocations optimale des dépenses publiques. mais j ai du mal à formaliser mon modèle. pouvez vous m'aider SVP. merci
Bonjour,
La modélisation est toujours la partie la plus compliqué... et je ne peux pas vraiment vous aider, car je ne connais/comprendrai pas votre problème.
La première chose à déterminer, c'est le paramètre à estimer (ici le PIB je suppose).
Ensuite, il faut définir quelles sont les informations (les mesures) que l'on reçoit du système ? (d'autres indicateurs ?)
Troisième étape, il faut réussir à relier les mesures aux paramètres inconnus à estimer. (le PIB inconnu aux indicateur du niveau de richesses connu par exemple)
Quatrièmement, il faut modéliser l'évolution des paramètres à estimer en fonction du temps (y a-t-il un modèle de l'évolution du PIB en fonction du temps ?)
Enfin, il faut se demander si Kalman est la méthode la plus approprié pour résoudre votre problème.
Bonjour,
merci beaucoup pour cet exemple avec le graphique, c'est très encourageant pour une première utilisation du filtre de Kalman (très souvent théorique dans les tutoriels et articles scientifiques).
Pour mon cas, j'ai implémenté le filtre de Kalman pour faire le suivi d'objets multiples en temps réel dans une séquence vidéo mais malheureusement l'algorithme de Kalman ne fonctionne que pour un seul objet (le 1er détecté) et pas pour les restes des objets.
A mon avis, le problème est forcément au niveau de l'initialisation du vecteur d'état X0 et la matrice de covariances P0 pour les autres objets.
Auriez-vous une idée de la manière dont je pourrais initialiser ces matrices, en sachant que des objets peuvent apparaître ou disparaître de la scène?
Merci
Merci
Bonjour,
Sans connaître le problème, non, je n'en ai aucune idée.
Par contre, comme ça, je dirais qu'il suffit de modéliser le comportement du système pour un seul objet, de définir les coefficients de Kalman pour cet objet et d'appliquer autant de fois Kalman qu'il y a d'objets sur la scène. (plutôt que de vouloir tout modéliser d'un coup avec un seul kalman... surtout si les objets n'ont pas d’interactions connues entre eux)
Bonjour,
Merci beaucoup pour ces explications qui m'ont permis de comprendre (du moins en partie) le filtre de Kalman.
Je ne suis pas un grand fan des maths ni des calculs matriciels mais j'ai tout de même réussi à ébaucher un script MATLAB reproduisant cet exemple. Je ne suis toutefois pas au bout de mes peines.
Mon script trace des courbes ressemblant à votre exemple afin de pouvoir les comparer et voir si mon filtre fonctionne : il ne fonctionne pas très bien. Voir ici les courbes en question : http://imageshack.us/f/845/kalman.png/
- L'estimation du biais forme une sinusoïde. Cela signifie donc qu'une partie du signal du gyro est considéré comme du biais donc que l'estimation de la vitesse angulaire est faussée.
- Les estimations d'angle et de vitesse angulaire sont encore peu précises, pourtant j'ai tiré les valeur de la matrice de covariance du bruit de commande au plus bas.
Est-ce juste une question valeur des différentes constantes ou une erreur dans le calcul ?
Plusieurs personnes ont demandé dans les commentaires le code MATLAB. Il pourrait effectivement se révéler très instructif, en l’occurrence je pourrais le comparer avec le mien pour identifier les causes de mes résultats.
Autre point :
Je ne suis pas sûr d'avoir bien compris cet exemple. Ici l'accéléro et le gyro semblent complètement indépendants. Si je supprime dans mon script la mesure du gyro l'estimation de l'angle n'est pas impactée et si je supprime la mesure de l'accéléro l'estimation de la vitesse angulaire n'est pas non plus touchée.
De même si j'ajoute des variations sur la mesure de l'accéléro qui sont décorrélées de la mesure du gyro (Imaginons que le système subisse une accélération linéaire), l'estimation de l'angle est perturbée.
L'intérêt du filtre de Kalman n'est il pas une fusion des capteurs accéléro + gyro qui permet de mesurer l'angle indépendamment de toute accélération "parasite" ?
Peut-être est-ce simplement mon scipt qui ne fonctionne pas comme il le devrait ?
Je souhaite réaliser un système de mesure d'angle autour d'un seul axe mais qui pourrait être embarqué dans un véhicule de terrain ou aérien. Cela me permettrait de me faire la main avant de m'attaquer à un projet plus ardu : réaliser une centrale inertielle. Vous avez commencé à en faire une, le projet est il toujours en cours ?
Merci
Thomas
Bonjour,
Cela est sûrement du au fait que vous avez fixé la variance maximale de l'erreur autorisée sur la modélisation de l'inclinaison ϵα (de la matrice de covariance du bruit de commande Q) à une valeur différente de 0.
En réalité, on est obligé de l'imposer à 0 car on est sûr que la modélisation est parfaite. En effet, l'angle, c'est bien l'intégration de la vitesse angulaire.
Si cette valeur est différente de 0, alors ça implique que le signal va venir modifier en partie la modélisation.
N'étant pas parfaitement bien codé, je fournie le code par mail à la demande.
Pas exactement non.
L'accéléromètre sert à mesurer l'angle du mobile alors que le gyroscope mesure la vitese angulaire de ce même mobile.
Ces deux données sont liés dans la modélisation de la transition (c'est là que l'on introduit la contrainte entre les deux mesures).
Les mesures sont donc indépendantes, mais on viens les corréler pour estimer le vecteur d'état, et donc l'angle. (bien que l'on pourrait obtenir ce vecteur d'état en utilisant un seul capteur, mais dans ce cas là, il n'y a plus d'intérêt d'utiliser Kalman)
Tout à fait, c'est pour ça que j'ai fait l'hypothèse en début d'article que le mobile n'a pas d'accélération propre ! Sinon, il faut trouver une autre modélisation qui prenne en compte cette accélération (et là, ça complique beaucoup les choses)
Attention, dans ce cas, le véhicule possède une accélération propre et donc il faut modéliser le système de façon bien plus complète que ce qui est présenté dans l'article.
Non, le projet est toujours en stand-by (et il le restera encore pendant quelques années malheureusement)
Bonsoir,
Pourrais-je avoir votre code source ?
J'ai beaucoup de mal à coder sur matlab et cela me serait vraiment très utile.
Merci d'avance
Bonjour,
Juste pour que l'article soit encore mieux 🙂
Petite erreur: "On considère que l'accéléromètre nous revoie des valeurs d'accélérations directement en mètres par seconde".
Merci pour votre travail en tout cas 😉
Ooops, merci pour la correction 😉
De rien!
J'ai une petite question.. Je ne comprends pas vraiment comment tu calcules l'angle d'incidence dans ton script Matlab? Pourrais-tu me l'expliquer stp 🙂
Merci
Autre question, y a-t-il une explication à l'allure du biais choisi?
J'ai pris une allure au pif (qui n'était pas linéaire) pour l'évolution du biais.
Qu'appelles-tu angle d'incidence ?
Désolé, je travaille sur un projet de mécanique des fluides en même temps 😉 Angle d'inclinaison du mobile..
Mais c'est ok j'ai compris 😉
Bonjour
Merci pour cet article qui m'a aidé à comprendre un peu mieux le filtre de Kalman. Cependant il y a une partie encore assez obscure dans mon esprit; à savoir l'implémentation dans un système embarqué.
Si j'ai bien compris, le gain de Kalman K et les matrices de covariance Pk et Pk+1 sont indépendants des mesures yk et de l'état xhk et xhk+1. Dès lors, et si les matrices Q, R et A sont constantes, le gain de Kalman K converge vers une valeur fixe. Ainsi, il est plus simple de calculer le gain K en simulation avant de l'implémenter dans notre système embarqué, ce qui nous permet de ne pas le recalculer à chaque étape.
C'est tout de moins ce que j'en déduis, et je voudrais savoir si cela est correcte.
En effet, quel est l'intérêt d'implémenter un algorithme assez lourd en calcul dans un système embarqué si on peut déterminer en simulation le gain K ?
Ou bien, les bruits peuvent évoluer dans le temps et il faudrait estimer la matrice R au fur et à mesure ?
Merci,
Baptiste
Bonjour,
J'ai eu la même idée à une certaine époque, mais malheureusement, ça ne marche pas.
En effet, le gain de Kalman (K) converge, certes, mais ce n'est pas la valeur finale qui nous intéresse, mais son évolution !! Ce gain s'adapte automatiquement en fonction de la justesse de la modélisation.
Si on fixe une fois pour toute le gain de Kalman, alors, d'une part, la covariance de l'erreur n'a plus aucune signification (le calcul de la covariance de l'erreur est alors faussé), et d'autre part, cela reviendrait à estimer une estimation du vecteur d'état.
Avec ce type de filtre, il est difficile de converger vers la borne de Cramer-Rao alors que Kalman garanti cette convergence. Néanmoins, un tel filtre existe, c'est un filtre alpha-beta : http://en.wikipedia.org/wiki/Alpha_beta_filter
Pour une implémentation sur un dsp, il y a plusieurs méthode.
La plus simple (mais pas la moins longue) serait de développer les équations matricielles afin d'obtenir des équations linéaires. C'est faisable si l'on cherche à estimer peu de paramètres (2 ou 3 max)
Une autre façon de faire est d'utiliser d'autre formulation du filtre de Kalman comme par exemple le filtre d'information : http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Le_filtre_d.27Information
Cela permet de se passer de l’inversion matricielle, qui très coûteuse
Sinon, il existe aussi des astuces de calculs comme la décomposition LU d'une matrice (qui facilite son inversion) : https://fr.wikipedia.org/wiki/Factorisation_de_Cholesky
Bonne journée,
Ferdinand
Bonjour
Merci pour votre réponse, néanmoins j'ai du mal à assimiler votre remarque :
De plus, il me semble que la covariance de l'erreur converge elle aussi vers une constante. J'admets que si l'on fixe le gain K, la covariance de l'erreur est faussée jusqu'au temps nécessaire à la convergence.
J'ai simulé un processus avec des bruits blancs aléatoires que j'ai ensuite filtré afin de déterminer le gain de Kalman final, puis j'ai simulé ce procédé avec le filtre une deuxième fois en filtrant dès le début avec le gain de Kalman fixe trouvé précédemment. Les résultats sont parfaitement corrects, à savoir quasiment identique, et totalement identique dès que l'on atteint le temps nécessaire à la convergence du gain choisi (que j'avais simulé précédemment). De plus, et cela est compréhensible, les résultats au tout début de la simulation sont meilleurs avec un gain fixe.
Ce pourquoi je ne comprends pas bien votre remarque sur l'intérêt que l'on doit porter à l'évolution du gain et non à la valeur finale.
Je précise que j'accorde peu d'importance à la covariance de l'erreur. En effet, je travail sur des systèmes en temps réel où je souhaite me servir d'un filtre de Kalman pour reconstruire certaines mesures bruitées afin d'asservir ou de réguler mon procédé. (Je comprends pas bien quelle serait l'utilité de la covariance de l'erreur dans cette situation).
De plus, admettons que l'on utilise un filtre de Kalman appliqué à un procédé durant un période très longue. Le gain et la covariance de l'erreur vont converger très vite par rapport à la durée de fonctionnement, dès lors je ne comprends pas bien quels sont les avantages à l'implémentation de l'algorithme du filtre de Kalman à un système temps réel.
Enfin, savez-vous si il existe des procédés où la matrice d'évolution A, ou les matrices de covariances des bruits Q et R ne sont pas constantes ?, et où il semble que l'implémentation de l'algorithme du filtre de Kalman en temps réel prend tout sont sens.
J'espère avoir été assez clair dans mes explications.
Je vous remercie pour vos indications sur le filtre alpha-beta, information et la décomposition LU.
Bonne journée,
Baptiste
Hum... Voyons ça d'un autre angle et essayons d'interpréter les équations.
On a deux phases : prédictions et mise à jour.
Durant la phase de prédiction, le gain de Kalman n'intervient pas, ni la mesure d'ailleurs.
Tout ce que l'on fait, c'est que l'on prédit la prochaine valeur du vecteur d'état en fonction d'un modèle plus ou moins juste.
Néanmoins, on remarque l'ajout de la matrice Q à l'estimation de la covariance de l'erreur. L'ajout de cette matrice signifie : On considère qui le modèle peut être erroné, on augmente la covariance de l'erreur pour traduire cette incertitude sur la modélisation.
Si on se contentait uniquement de cette étape, la covariance de l'erreur ne ferait qu'augmenter (en effet, il n'y a aucun lien entre cette matrice et le vecteur d'état prédit ! cette matrice ne servirait donc à rien).
Jusque là, pas de soucis. Passons à la phase de mise à jour.
Ici, on calcule le gain de Kalman. Ce gain indique l'importance de la prédiction par rapport aux erreurs (bruits et erreurs de modélisation)
Si on regarde l'équation de la mise à jour du vecteur d'état X.
X = Xp + K.(y-H.Xp)
Si le gain K vaut 0, alors on estime que la prédiction Xp est fiable à 100% !
Au contraire, si K est différent de 0, alors on donne de l'importance au terme (y-H.Xp).
H.Xp correspond à ce que devrait valoir le vecteur de mesure si la modélisation était parfaite et le bruit inexistant. (y-H.Xp) représente donc l'erreur (de bruits et de modélisation) de la mesure.
K.(y-H.Xp) est donc un terme correctif de la prédiction du vecteur d'état en fonction des erreurs engendrées par le bruit et la modélisation. Plus K est grand, plus le terme correctif aura de l'influence et moins on accordera de crédibilité à la prédiction.
Imaginons maintenant un scénario : on souhaite calculer la trajectoire d'une balle de football. On modélise donc les équations de la chute d'un corps : m.a = 0
On implémente le Kalman et on lance une simulation. Le gain K converge vers une valeur (non nul, mais très petite).
A un moment t quelconque, on ajoute une bourrasque de vent. La trajectoire est déviée, mais ce changement de trajectoire n'est pas prévu dans la modélisation. Le gain de Kalman va donc augmenter et converger vers une autre valeur, plus importante que précédemment ! La prédiction aura donc moins d'influence. Le produit erreur/gain de Kalman va alors peser plus lourd dans la balance et ainsi corriger en partie l'erreur de modélisation.
Le vent retombe, alors K reconverge vers une valeur faible : la prédiction (et donc la modélisation) reprend plus d'importance.
Je pense qu'avec cet exemple vous vous rendez compte que si l'on fixe le gain de Kalman dès le départ, on perd tout l'avantage du filtre de Kalman qui est de s'adapter automatiquement aux perturbations du modèle.
Un gain K fixé suppose que l'incertitude du modèle ne changera pas au cours du temps (par exemple, une modélisation parabolique de trajectoire d'une balle (en théorie), et l'ajout d'une vent INVARIANT au cours du temps (en pratique)).
Attention, la covariance de l'erreur ne vous sert pas directement, soit. Mais ce n'est pas une raison pour retirer les équations !!!! Cette covariance sert à calculer le gain de Kalman, sans lui, pas de correction possible (que ce soit de la modélisation, ou du bruit des capteurs !) car le filtre lui-même n'aura pas conscience de la pertinence de son estimation !
Le calcul de cette matrice est un peu le coeur de l'algorithme !
En général, non, ces matrices sont définies une fois pour toute et on n'a pas besoin modifier.
R correspond au bruit des capteurs. A la limite, c'est la seule matrice qui peut changer au cours du temps (on peut imaginer un capteur dont le niveau de bruit change avec la température). Il faudrait juste vérifier qu'un changement dynamique des valeurs de cette matrice n'affecte pas la valeur d'autres matrices.
La matrice A permet de lier l'état précédent à l'état suivant. Normalement, vous modélisez une fois pour toute votre système au début. Cette matrice n'a pas de raison de changer car, si vous la modifiez, ça veut dire que vous avez des infos en plus. Autant directement définir cette matrice directement du premier coup.
A la limite, si vous avez deux cas vraiment différents, il suffit d'implémenter deux Kalman et d'utiliser un filtre si certaines conditions sont réunies, l'autre sinon. De toute manière, c'est à toi de définir la modélisation. Kalman ne sert pas à estimer la modélisation ^^
Q correspond à l'incertitude que l'on accorde à la modélisation. Comme la modélisation ne change pas, cette matrice n'a pas de raison de changer.
J'espère que j'ai été plus clair.
Ferdinand
Bonjour,
Merci pour votre réponse détaillée. Cependant il y a encore un détail que je ne comprends pas :
Etant donné que les calculs du gain de Kalman ainsi que celui de la covariance de l'erreur ne sont pas fonction de la mesure ou de l'état, comment le gain peut-il augmenter lorsque la différence entre la mesure et la prédiction augmente ?
D'après ce que je comprends, tous les coefficients du calcul du gain K sont constants, je ne vois aucune variable qui traduirait de l'évolution de l'état ou de la mesure, et qui permettrai de faire augmenter K lorsque la prédiction n'égale pas la mesure.
Merci pour vos explications.
Baptiste
@Baptiste : C'est une bonne question, je n'ai malheureusement pas la réponse.
Mais dans le même style, on peut se demander comment la matrice P arrive à converger vu qu'elle n'a aucun lien avec la mesure, ni avec l'estimation de l'état
Merci pour ces explications claires et concrètes sur le filtre de Kalman. La plupart des articles du net ne détaillent que la partie mathématiques ou restent flous quant à l'application de cette méthode d'estimation
je me demande si tu peut m'aider a comprendre l'algorithme DCM afin de modélisé un quad-rotor pour faire une estimation d'attitude en appliquant KALMAN et merci d'avance (dans ce exemple, on utilise trois types de capteurs gyro et accéléro et magnétomètre)
Bonsoir,
Je travaille actuellement sur un projet de localisation de robot mobile dans le cadre d'un projet en école d'ingénieur.
Mon problème est très proche de votre exemple.
Après avoir passer pas mal de temps sur la compréhension du filtre et de mon problème ,j'ai beaucoup de mal à coder celui-ci sur matlab.
Serait-il possible de voir votre code source svp ? (cela pourrait peut-être m'éclairer un peu plus)
Merci d'avance
Bonjour,
Il y a un lien vers le code source juste avant la conclusion.
Bonjour,
Tout d’abord je vous remercie pour ses très bons articles sur le filtre Kalman. En ce moment je dois faire un filtre Kalman avec un gyroscope et un accéléromètre comme dans cet article.
Pour utiliser le filtre Kalman, (avec un accéléromètre et un gyroscope) est-on obligé d’enlever le bruit du à l’accélération du système ? Si on ne le fait pas qu’obtient-on lorsqu’il y a des accélérations du système (vu qu’on n’a pas de bruit de type blanc)?
Est-ce possible, si on connait les équations du système d’éliminer le bruit sur l’accéléromètre a chaque instant de mesure ?
Et ma dernière question, comment font les IMU industrielles pour enlever ses accélérations vu qu’ils ne connaissent pas le système sur lequel va être utilisé cette IMU pour mesure l’angle d’inclinaison ?
En vous remerciant
merci bcp pr cet article, trés interessant
Merci pour ce superbe article
en fait est ce que vous pouvez faire une simulation sous simulink en integrant par exp un modele pour le gyro et l'accelerometre
si vous avez des document utiles pour faire cela veuillez m'envoyer un email svp
merci
Merci
Bonjour,
je te remercie tout d'abord pour cet exemple et ce cours très clair.
J'essaye maintenant de créer mon modèle pour fusionner des données issu du gyroscope, du magnétomètre et de l’accelerometre pour obtenir les angles d'Euler.
J'aimerai intégrer les donnes du magnétomètre pour réduire les perturbations magnétiques et je voulais réduire la dérive du gyroscope.
Je me suis basée sur la démarche de Roetenberg (https://www.xsens.com/images/stories/PDF/Roetenberg_inertial_and_magnetic_sensing_TNSRE_2005.pdf) avec dans mon modele :
X = (a g m d w b) avec Y = acc mag gyro tel que voici mes equations :
- yAcc = a - g + BruitBlanc
- yMag = m +deriveMag +BruitBlanc
- yGyro = w + biaisGyro + BruitBlanc
j'ai comme hypothese :
- biaisGyro (t) = biaisGyro (t-1) + BruitGaussien
- g la gravite est considérée constante - g(t) = g(t-1)
- a(t) = ca * a(t-1) + BruitGaussien
- deriveMag(t) = cd * deriveMag(t-1) + BruitGaussien
- w(t) = w(t-1) considerée constante car je ne connais pas le modèle
- m(t) = m(t-1)
Malheureusement, je n'ai cependant pas l impression que ce modèle est pertinent et je n'arrive aps l’implémenter car je ne connais pas cd et ca qui sont des constantes.
As tu d'autres idées pour moi pour intégrer le magnétomètre au modèle
Merci d'avance, a+
Bonjour,
Je ne comprend pas à quoi ça sert d'estimer la constante g. Celle-ci est connue et constante (dans 99.99% des applications).
Si le modèle d'évolution des paramètres n'est pas connu, l'avantage du filtre de Kalman sera de faire de la fusion de données. Pour moi, le vecteur X des paramètres à estimer serait plutôt (accélération; vitesse angulaire; angle; biais_gyro; dérive_magneto).
Si c'est possible, il vaut mieux à mon avis convertir les données du magnétomètre directement en angle (avec un peu de trigo 3D). Le vecteur de mesure Y sera alors (Accélération, Vitesse angulaire, Angle)
Concernant les équations de mesure, c'est pas mal, mis à part la mesure de l'accéléromètre qui est fonction de la vrai accélération plus "un bout" de la constante g plus du bruit. ce "bout de g" dépend de l'inclinaison x,y,z de la centrale inertielle. Ici, il y a un peu de trigo 3D à faire 😉
Concernant les équations d'états, par contre, on peut améliorer un peu ça.
On a pas d'information sur l'accélération, la vitesse angulaire, le biais, ni la dérive. Du coup, on considère ça comme constant.
Par contre, concernant l'angle à t, c'est égale à l'angle à t-1 + w(t-1) * le pas de temps (échantillonnage).
Tout ça permet de lier les informations du magnéto, et de l'accéléro via les équations de mesure, et celles du gyro et du magnéto dans les équations d'états. On fusionne tout ça afin de mieux estimer chacun des paramètres.
Les constantes ca et cd n'ont rien à faire dans la modélisation de l'évolution du vecteur d'état à mon avis (à moins que tu saches que ta centrale est en accélération constante, ce qui à mon avis n'est pas le cas !!)
Tout ça me parait plutôt bien. Il y a juste un peu de trigo 3D à faire, mais pour le reste, ça m'a l'air plutôt facile à réaliser. Il ne faut pas oublié de considérer les 3 axes x,y et z pour chacune des grandeurs à estimer !
a+ F.
bonjour j'ai un peu compris Kalman mais par contre j'ai du mal à comprendre la phase d'initialisation Pk=P0
P= inverse de matrice (matrice tranpose (H) *inverse de matrice ( R)*matrice (H) )
quel est le rapport avec votre code ci-dessous ? (pourriez vous expliquer brievement chaque ligne ?, la question semble légitime car Pk pose probleme a pas mal d'internautes apparemment) :
P = zeros(3, 3);
P(1, 1) = 0;
P(2, 2) = 0;
P(3, 3) = 0;
P(1, 2) = 0;
P(2, 1) = P(1, 2);
P(2, 3) = P(1, 2);
P(3, 2) = P(1, 2);
P(1, 3) = 0;
P(3, 1) = P(1, 3)
P_svg = zeros(temps*fe, 3);
P_svg(1, 1) = P(1, 1);
P_svg(1, 2) = P(2, 2);
P_svg(1, 3) = P(3, 3);
je vous remercie d'avance ,
jean
Bonjour,
Tout d'abord je tenais à vous remercier pour vos explications très claires.
Pourriez-vous m'envoyer le code du fitlre sous matlab je vous prie ?
En vous remerciant
Bonjour !
Pourrais-je avoir votre code matlab svp ?
Bonjour,
Tout d'abord, merci pour vos articles sur le kalman et cet exemple, car ils sont très claires et instructifs. Ce qui m'a permis de bien comprendre le fonctionnement de ce type de filtrage.
Aussi, en regardant votre exemple, j'aurais aimé savoir d'où proviennent vos données d'inclinaisons et de vitesses angulaires.
Du faite que pour un travail, j'implémente un kalman qui reçoit des mesures de l'accéléromètre et du giroscope sur 3 axes. Mais j'aurais aimé avoir un modèle de ceux-ci sur 3 axes pour connaître au préalable ce que je dois obtenir en sortie du kalman et comparer avec l'entré, comme dans votre exemple. Seulement je ne trouve pas de modèle. Bien sur il y a possibilité de calculer ces données avec des matrices de rotation, Cependant cela occupe beaucoup de temps. Alors je voudrais savoir si vous n'avez pas déjà des modèles de prêt, ou bien une solution qui serait envisageable.
En l'attente de votre retour.
Thibault
Bonjour,
Je vous remercie pour cet article très intéressant.
D'une manière générale, comment construit-on les matrices de covariance Q et R?
Bien cordialement,
Auxence
stp pouvez vous me donner le Code Matlab du System?
merci d avance