Le Filtre Kalman Étendu Extended Kalman Filter

Le filtre étendu de
Kalman :un didacticiel

interactif pour les non-
experts
Source : https://home.wlu.edu/~levys/kalman_tutorial/
Traduit par Abderrahim TAOUNI
18
Le filtre étendu de Kalman: un didacticiel interactif pour

les non-experts
Traduit par A.TAOUNI
En travaillant avec des systèmes de pilote automatique comme OpenPilot et Pixhawk, j'ai souvent
rencontré des références à quelque chose appelé un filtre étendu de Kalman (EKF).
Googling ce terme m'a conduit à plusieurs pages Web et documents de référence différents, dont
la plupart j'ai trouvé trop difficile à suivre. [1] J'ai donc décidé de créer mon propre tutoriel pour
enseigner et apprendre sur l'EKF à partir des premiers principes. Ce didacticiel suppose uniquement
des mathématiques de niveau secondaire et il présente des concepts issus de domaines plus avancés,
tels que l'algèbre linéaire, au besoin, plutôt que de supposer que vous les connaissez déjà. En
commençant par quelques exemples simples et le filtre Kalman standard (linéaire), nous travaillons à
la compréhension des implémentations EKF réelles à la fin du tutoriel.
Partie 1: Un exemple simple

Imaginez un avion qui arrive pour un atterrissage. Bien qu'il y ait beaucoup de choses dont nous
pourrions nous inquiéter, comme la vitesse, le carburant, etc., la chose la plus évidente pour se
concentrer sur son altitude de l'avion (hauteur au-dessus du niveau de la mer). Comme une
approximation très simple, nous pouvons considérer l'altitude actuelle comme une fraction de
l'altitude précédente. Par exemple, si l'avion perd 2% de son altitude chaque fois que nous
l'observons, alors son altitude à l'heure actuelle est de 98% de son altitude à l'instant précédent:
Les ingénieurs utilisent le terme récursif pour se référer à une formule comme celle-ci où une
quantité est définie en fonction de sa valeur précédente: pour calculer la valeur actuelle, nous
devons «revenir» à la précédente. Finalement, nous revenons à un certain "cas de base" initial,
comme une altitude de départ connue.
Essayez de vous déplacer dans le curseur au-dessus pour voir comment l'altitude de l'avion
change pour différents pourcentages.
A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 1

Partie 2: Faire face au bruit

Bien sûr, les mesures du monde réel comme l'altitude sont obtenues à partir d'un capteur comme
un GPS ou un baromètre. De tels capteurs offrent différents degrés de précision. [2] Si le capteur est
éteint d'une quantité constante, nous pouvons simplement ajouter ou soustraire cette quantité pour
déterminer notre altitude. Généralement, cependant, la précision du capteur varie de façon
imprévisible d'un moment à l'autre, ce qui fait que le capteur observé lit une version "bruyante" de
l'altitude vraie:
Essayez de vous déplacer autour du curseur pour voir l'effet du bruit sur l'altitude observée. Le
bruit est représenté en pourcentage de la gamme des altitudes observables.
Partie 3: S’y mettre ensemble

Nous avons donc maintenant deux équations décrivant l'état de notre avion:
altitudecurrent_time = 0.98 * altitudeprevious_time
observed_altitudecurrent_time = altitudecurrent_time + noisecurrent_time
Ces équations sont assez faciles à comprendre, mais elles ne sont pas assez générales pour traiter
d'autres systèmes que l'exemple de notre avion-altitude. Pour rendre les équations plus générales,
les ingénieurs adoptent la convention mathématique familière d'utiliser des noms comme x, y et z
pour les variables et a et b pour les constantes, et l'indice k pour représenter le temps. [3] Donc, nos
équations deviennent:
où x est l'état actuel de notre système, xk-1 est son état précédent, a est une constante (0,98
dans notre exemple), zk est notre observation actuelle du système, et vk est la mesure de bruit
actuelle. Une raison pour laquelle le filtre de Kalman est si populaire est qu'il nous permet d'obtenir

une très bonne estimation de l'état actuel xk étant donné l'observation zk, la constante a, et la
quantité totale de bruit de mesure v.
Pour compléter l'image, il faut aussi considérer que l'altitude réelle de l'avion peut ne pas décrire
une trajectoire parfaitement lisse. Comme tous ceux qui ont déjà volé peuvent vous le dire, les
avions subissent généralement une certaine turbulence lorsqu'ils descendent pour atterrir. Cette
turbulence est par définition bruyante, et peut donc être traitée comme un autre signal de bruit:
Plus généralement:
où wk est appelé le bruit de processus, parce que, comme la turbulence, c'est une partie
inhérente du processus, et non un artefact d'observation ou de mesure. Nous allons ignorer le bruit
de processus pendant un certain temps afin de nous concentrer sur d'autres sujets, mais nous y
reviendrons dans la section sur Fusion de capteurs. [1]
Partie 4: Estimation de l'État

Ici encore (en ignorant le bruit de processus) sont nos deux équations décrivant l'état d'un
système que nous observons:
Puisque notre objectif est d'obtenir les états x à partir des observations z, nous pourrions réécrire
la deuxième équation comme suit:
Le problème est bien sûr que nous ne connaissons pas le bruit actuel vk: il est par définition
imprévisible. Heureusement, Kalman a eu l'idée que nous pouvons estimer l'état en prenant en
compte à la fois l'observation actuelle et l'état estimé précédent. Les ingénieurs utilisent un petit
curseur ou «chapeau» sur une variable pour montrer qu'elle est estimée: donc ^x k est l'estimation
de l'état actuel. Ensuite, nous pouvons exprimer l'estimation comme un compromis entre
l'estimation précédente et l'observation actuelle:
Où g est un terme «gain» exprimant le compromis. [2] J'ai mis en évidence cette équation en
rouge car c'est celle que nous utiliserons directement dans la mise en œuvre de notre filtre de
Kalman.
Maintenant, tout cela semble plutôt compliqué, mais pensez à ce qui se passe pour deux valeurs
extrêmes du gain gk. Pour gk = 0, nous obtenons

En d'autres termes, lorsque le gain est nul, l'observation n'a aucun effet et nous obtenons
l'équation originale reliant l'état actuel au précédent. Pour gk = 1, nous obtenons
En d'autres termes, lorsque le gain est un, l'état précédent n'a pas d'importance, et nous
obtenons l'estimation de l'état actuel entièrement à partir de l'observation actuelle.
Bien sûr, la valeur de gain réelle se situera probablement entre ces deux extrêmes. Essayez de
déplacer le curseur ci-dessous pour voir l'effet du gain sur l'estimation de l'état actuel:
Partie 5: Calculer le gain

Nous avons donc maintenant une formule que nous pouvons utiliser pour calculer l'estimation de
l'état actuel x ^ k sur la base de l'estimation précédente x ^ k-1, de l'observation actuelle zk et du
gain actuel gk:
Alors, comment calculons-nous le gain? La réponse est: indirectement, du bruit. Rappelons que
chaque observation est associée à une valeur de bruit particulière:
Nous ne connaissons pas la valeur du bruit individuel pour une observation, mais nous
connaissons généralement le bruit moyen: par exemple, la précision publiée d'un capteur nous
indique approximativement à quel point sa sortie est bruyante. Appelez cette valeur r; il n'y a pas
d'indice car il ne dépend pas du temps, mais est une propriété du capteur. Ensuite, nous pouvons
calculer le gain actuel gk en termes de r:

où pk est une erreur de prédiction calculée de manière récursive: [3]
Comme pour la formule d'estimation d'état, réfléchissons à ce que signifient ces deux formules
avant de continuer.
Disons que l'erreur pk-1 sur notre prédiction précédente était nulle. Alors, notre gain actuel gk
sera 0 / (0 + r) = 0, et notre prochaine estimation d'état ne sera pas différente de notre estimation
d'état actuelle. Ce qui est logique, car nous ne devrions pas ajuster notre estimation d'état si notre
prédiction était exacte. À l'autre extrême, disons que l'erreur de prédiction est un. Alors le gain sera
1 / (1 + r). Si r est nul, c'est-à-dire s'il y a très peu de bruit dans notre système, alors le gain sera un,
et notre nouvelle estimation d'état xk sera fortement influencée par notre observation zk. Mais
quand r grandit, le gain peut devenir arbitrairement petit. En d'autres termes, lorsque le système est
assez bruyant, une mauvaise prédiction devra être ignorée. Le bruit dépasse notre capacité à corriger
les mauvaises prédictions.
Qu'en est-il de la troisième formule, calculant l'erreur de prédiction pk récursivement à partir de

sa valeur précédente pk-1 et du gain actuel gk? Encore une fois, il est utile de penser à ce qui se
passe pour les valeurs extrêmes du gain: quand gk = 0, nous avons pk = pk-1. Ainsi, tout comme avec
l'estimation d'état, un gain nul signifie qu'il n'y a pas de mise à jour de l'erreur de prédiction. Quand
par contre gk = 1, on a pk = 0. Par conséquent, le gain maximum correspond à zéro erreur de
prédiction, l'observation actuelle étant utilisée seule pour mettre à jour l'état actuel.
Partie 6: Prédiction et mise à jour

Nous sommes presque prêts à lancer notre filtre de Kalman et à voir quelques résultats. D'abord,
cependant, vous pouvez vous demander ce qui est arrivé à la constante a dans notre équation d'état
d'origine:
qui semble avoir disparu dans notre équation pour l'estimation de l'état:
La réponse est, nous avons besoin de ces deux équations pour estimer l'état. En effet, les deux
équations représentent une estimation de l'état, basée sur différents types d'informations. Notre
équation originale représente une prédiction sur ce que l'état devrait être, et notre deuxième
équation représente une mise à jour de cette prédiction, basée sur une observation. [4] Nous
réécrivons donc notre équation originale avec un petit chapeau sur le x pour indiquer une
estimation:

Enfin, nous utilisons la constante a dans une prédiction de l'erreur: [5]
Ensemble, ces deux formules en rouge représentent la phase de prédiction de notre filtre de
Kalman. L'idée est que le cycle prédire / mettre à jour, prédire / mettre à jour, ... soit répété pour
autant de pas de temps que nous voulons.
Partie 7: Exécution du filtre

Nous avons maintenant tout ce dont nous avons besoin pour faire fonctionner notre filtre
Kalman:
Prédire:
Mettre à jour:
Notez que j'ai utilisé le symbole d'affectation (flèche), au lieu du signe égal standard, dans les
deux dernières lignes de la mise à jour. Bien que cet usage soit non traditionnel, il traverse l'idée que
la mise à jour de x ^ k et pk modifie leurs valeurs actuelles (à partir de l'étape de prédiction) plutôt
que de les définir en termes d'une étape précédente (comme le fait la prédiction) . [6]
Pour essayer notre filtre, nous aurons besoin de:
 Une séquence d'observations zk

 Une valeur initiale (cas de base) x ^ 0 pour les estimations d'état. Cela peut juste être
notre première observation z0.
 Une valeur initiale p0 pour l'erreur de prédiction. Il ne peut pas être 0, sinon pk
resterait 0 pour toujours par multiplication. Nous l'avons donc arbitrairement fixé à 1.
Pour nos observations, plutôt que d'essayer d'observer un système réel (comme un avion arrivant
pour un atterrissage), nous allons fausser quelques observations basées sur l'ajout de bruit aléatoire
[7] vk dans l'intervalle [-200, + 200] à les valeurs idéalisées xk = 0,75xk-1, en commençant par x0 =
1000:

Une fois que vous êtes prêt à lancer le filtre, appuyez sur le bouton Exécuter pour voir comment
le filtre de Kalman produit une version lisse (verte) du signal bruyant (rouge) qui est souvent
remarquablement proche du signal propre d'origine (bleu). Vous pouvez également essayer
différentes valeurs pour x0, r et a.
Partie 8: Un modèle plus réaliste

Rappelez les deux équations décrivant notre système:
où xk est l'état actuel de notre système, xk-1 est son état précédent, a est une constante, zk est
notre observation actuelle du système, et vk est le bruit actuel (inexactitude) associé à l'observation.
Bien que ces deux équations s'appliquent bien à de nombreux types de systèmes, elles ne
constituent parfois pas toute l'histoire. D'une part, nous n'avons pas pris en compte le contrôle
variable du temps que le pilote exerce sur l'avion, en déplaçant (par exemple) la colonne de
commande vers l'avant et vers l'arrière. Pour prendre en compte le contrôle, nous introduisons une
autre variable indicée uk, représentant la valeur actuelle du signal de contrôle que le pilote envoie à
l'avion. Tout comme l'état précédent xk-1 a été mis à l'échelle par une quantité constante a, ce signal
de contrôle peut être mis à l'échelle d'une quantité constante si on le souhaite; appelez-le b. Donc,
notre équation complète pour l'état devient

avec le nouveau composant surligné en bleu.
En général, tout signal autre que le bruit peut être mis à l'échelle par une certaine constante, de
sorte que notre équation pour l'observation zk peut être réécrite ainsi: [8]
Partie 9: Modifier les estimations

Voici encore nos équations plus réalistes / plus générales pour les variables d'état et
d'observation de notre système:
Comme on peut s'y attendre, l'introduction de ces nouveaux composants dans notre modèle
nécessite une modification correspondante des équations de prédiction et de mise à jour:
Prédire:
Mettre à jour:
Voici une extension de notre démo d'avion, montrant une plus longue durée de temps et ajoutant
un signal de contrôle représentant le pilote tirant régulièrement sur la colonne de contrôle pour
élever l'altitude de l'avion. Essayez de vous déplacer dans les curseurs pour ajuster les valeurs des
différentes constantes. Comme dans la démo précédente, le signal original est affiché en bleu, le
signal observé en rouge et le signal filtré par Kalman en vert.

Partie 10: Ajouter de la vélocité au système

Rappelons notre équation d'origine pour l'altitude d'un avion:
avec la forme plus générale:
En repensant aux maths et à la physique que vous avez appris au lycée, ce genre de formule
semble un peu étrange. L'altitude, après tout, est une sorte de distance (au-dessus du niveau de la
mer, ou au-dessus du niveau du sol), pour laquelle nous avons appris la formule

Pouvons-nous réconcilier ces deux manières différentes de penser à la distance? La réponse est
oui, mais cela nous obligera à faire deux pas.
Tout d'abord, nous devons introduire les concepts de l'heure actuelle et de l'heure précédente
dans notre formule de lycée, et penser à la distance en pas de temps discrets plutôt qu'à la distance
globale:
En d'autres termes, là où nous sommes maintenant, c'est là où nous étions il y a un instant, plus la
distance que nous venons de parcourir. Si nous effectuons ce calcul à intervalles de temps réguliers,
ou timesteps (une seconde, 100 nanosecondes, six mois, etc.), alors nous pouvons simplifier cela
pour:
Cette équation nous rapproche de notre forme générale
mais nous semblons toujours avoir deux systèmes très différents: l'un concerne un produit simple
et l'autre un produit et une somme. Venir avec une équation générale pour les deux nous amène à la
deuxième étape: l'algèbre linéaire.
Partie 11: Algèbre linéaire

Nous avons donc une équation exprimant la distance en termes de vitesse et de temps:
que nous essayons de réconcilier avec une équation plus générale
Heureusement pour nous, les mathématiciens ont depuis longtemps imaginé «un truc bizarre»
pour représenter les deux types d'équations de la même manière. L'astuce consiste à penser à une
situation (comme l'état d'un système) non pas comme un seul nombre, mais plutôt comme une liste
de nombres appelée un vecteur, qui est comme une colonne dans une feuille de calcul Excel. La taille
du vecteur (nombre d'éléments) correspond au nombre de choses que nous voulons encoder à
propos de l'état. Pour la distance et la vitesse, nous avons deux éléments dans notre vecteur:

Ici, j'ai utilisé le "triple égal ≡" pour indiquer que c'est une définition: l'état actuel est défini
comme un vecteur contenant la distance actuelle et la vitesse actuelle.
Alors, comment cela nous aide-t-il? Eh bien, une autre chose que nous obtenons de l'algèbre
linéaire est une matrice. Si un vecteur est comme une colonne de valeurs dans une feuille de calcul,
alors une matrice est comme une feuille de calcul entière ou une table de valeurs. Quand nous
multiplions une matrice par un vecteur, nous obtenons un autre vecteur de la même taille:
Par exemple:
Les vecteurs et les matrices peuvent être de n'importe quelle taille, à condition qu'ils
correspondent:
Nous pouvons également multiplier deux matrices ensemble pour obtenir une autre
L'ajout
de deux matrices est plus simple. nous ajoutons simplement chaque paire
Revenons à la tâche à accomplir, nous définissons une matrice
Suivant la convention d'utiliser une lettre majuscule pour représenter une matrice. Alors notre
équation générale est à peu près la même:

ce qui fonctionne comme nous voulons:
En d'autres termes, la distance actuelle est la distance précédente plus la vitesse précédente fois
le temps, et la vitesse actuelle est la même que la vitesse précédente. Si nous voulons modéliser un
système dans lequel la vitesse change avec le temps, nous pouvons facilement modifier notre
vecteur et notre matrice pour inclure l'accélération: [12]
Partie 12: Prévision et mise à jour révisées

Voici à nouveau notre formule modifiée pour l'état du système:
où x est un vecteur et A est une matrice. Comme vous vous en souvenez, la forme originale de
cette équation était
où uk est un signal de contrôle, et b est un coefficient pour le mettre à l'échelle. Nous avons aussi
eu l'équation
où zk est un signal de mesure (observation, capteur), et vk est un bruit ajouté à ce signal résultant
de l'imprécision du capteur. Alors, comment pouvons-nous modifier ces formes originales pour
travailler avec notre nouvelle approche vecteur / matrice? Comme vous pouvez vous en douter,
l'algèbre linéaire rend cela très simple: nous écrivons les coefficients b et c en majuscules, ce qui en
fait des matrices plutôt que des valeurs scalaires (simples):

Ensuite, toutes les variables (état, observation, bruit, contrôle) sont considérées comme des
vecteurs, et nous avons un ensemble de multiplications de vecteurs * matricielles. [13]
Alors qu'en est-il de nos prédictions et mises à jour des équations? Rappelez-vous qu'ils sont:
Prédire:
Mettre à jour:
Nous aimerions juste capitaliser toutes les constantes a, b, c et r pour qu'elles deviennent les
matrices A, B, C et R, et en finir avec elles. Mais les choses sont un peu plus compliquées pour
l'algèbre linéaire! Vous vous souvenez peut-être quand j'ai promis d'expliquer pourquoi je n'ai pas
simplifié apk-1a à a2pk-1. La réponse est que la multiplication d'algèbre linéaire n'est pas aussi simple
que la multiplication ordinaire. Pour obtenir la réponse correcte, nous devons souvent effectuer la
multiplication dans un certain ordre; par exemple, AxB n'est pas nécessairement égal à BxA. Nous
pouvons également avoir besoin de transposer la matrice, en indiquant un petit exposant T à côté de
la matrice. Transposer une matrice est fait en tournant chaque rangée dans une colonne et chaque
colonne dans une rangée. Voici quelques exemples:
Avec cette connaissance à l'esprit, nous pouvons réécrire nos équations de prédiction comme
suit:

Notez que nous avons réécrit A et Pk en majuscules, indiquant qu'il s'agit de matrices. Nous
savons déjà pourquoi A est une matrice; nous différerons pour un moment de comprendre pourquoi
Pk est aussi une matrice.
Qu'en est-il de nos équations de mise à jour? La deuxième formule de mise à jour, pour
l'estimation d'état x ^ k, est simple:
mais la mise à jour de gain implique une division:
Puisque la multiplication et la division sont si étroitement liées, nous rencontrons un problème

similaire avec la division matricielle comme nous l'avons fait avec la multiplication. Pour voir
comment nous traitons ce problème, réécrivons d'abord notre première équation de mise à jour en
utilisant l'exposant familier -1 pour indiquer la division comme inversion (a-1 = 1/a):
Maintenant, nous transformons c, r, et g et la constante 1 en matrices, en transposant C comme

nécessaire - et en différant pour un moment, une fois de plus, notre compréhension de la raison pour
laquelle ils doivent être des matrices:
Alors comment calculer , l'inverse de la matrice ? Tout comme

-1
pour l'inversion ordinaire, où a * a = 1, nous avons besoin de l'inversion matricielle pour que la
multiplication d'une matrice par sa propre inverse produise une matrice identitaire qui, lorsqu'elle
est multipliée par une autre matrice laisse cette matrice inchangée:
où (pour une matrice 3x3)

Calculer une matrice inverse n'est pas aussi simple que d'inverser chaque élément de la matrice,
ce qui donnerait la mauvaise réponse dans la plupart des cas. Bien que les détails de l'inversion de
matrice dépassent la portée de ce tutoriel, il existe d'excellentes ressources telles que MathWorld
pour en apprendre davantage. Mieux encore, vous n'avez généralement pas à le coder vous-même; il
est construit dans des langages comme Matlab, et est disponible via un paquet en Python et d'autres
langages populaires.
Partie 13: Introduction au Fusion des capteurs

Nous avons maintenant un ensemble complet d'équations pour notre filtre de Kalman en forme
d'algèbre linéaire (vecteur, matrice):
Modèle:
Prédire:
Mettre à jour:
Cela semble beaucoup de travail juste pour pouvoir ajouter quelques éléments supplémentaires à
notre variable d'état! En fait, l'utilisation de l'algèbre linéaire supporte une capacité extrêmement
importante du filtre de Kalman, appelée fusion de capteurs.
En revenant à notre exemple d'avion, nous constatons que les pilotes ont accès à plus
d'informations (observations) qu'à l'altitude: ils disposent également de jauges indiquant la vitesse,
la vitesse sol, le cap, la latitude et la longitude, la température extérieure, etc. trois capteurs dont
chacun correspond à une partie donnée de l'état: un baromètre pour l'altitude, une boussole pour le
cap et un tube de Pitot pour la vitesse. Supposons pour le moment que ces capteurs sont
parfaitement précis (pas de bruit). Alors notre équation d'observation

devient
Maintenant imaginez que nous avons un autre capteur d'altitude, disons un GPS. Le
Nous avons ici notre premier exemple de système sans une simple correspondance bi-univoque
entre les capteurs et les valeurs d'état. [14]. Un tel système nous offre l'opportunité de fusionner les
capteurs; c'est-à-dire, la capacité de combiner les lectures de plus d'un capteur (baromètre, GPS)
pour déduire quelque chose au sujet d'une composante (altitude) de l'état.
Comme lorsque nous demandons l'avis d'un deuxième médecin sur un problème de santé, notre
intuition nous dit qu'il vaut mieux avoir plus d'une source d'information sur quelque chose
d'important. Dans la section suivante, nous verrons comment le filtre de Kalman utilise la fusion de
capteurs pour nous donner une meilleure estimation de l'état que nous pouvons obtenir avec un
capteur unique.
Partie 14: Exemple de fusion de capteurs

Pour avoir une idée de la façon dont la fusion de capteurs fonctionne, limitons-nous à nouveau à
un système avec une seule valeur d'état. [15] Pour simplifier encore les choses, nous supposerons
que nous n'avons aucune connaissance du modèle de transition d'état (matrice AA) et que nous ne
devons donc compter que sur les valeurs des capteurs. Peut-être que nous mesurons la température
à l'extérieur avec deux thermomètres différents. Nous allons donc simplement définir notre matrice
de transition d'état à 1:
En l'absence d'un modèle de transition d'état pour notre thermomètre, nous supposons
simplement que l'état actuel est le même que l'état précédent.
Pour la fusion de capteurs, nous aurons bien sûr besoin de plus d'une valeur de capteur dans
notre vecteur d'observation zk, que nous pouvons traiter ici comme les lectures actuelles de nos

deux thermomètres. Nous supposerons que les deux capteurs contribuent également à notre
estimation de la température, donc notre matrice C est juste une paire de 1:
Nous avons maintenant deux matrices (A, C) des trois (A, C, R) dont nous avons besoin pour les
équations de prédiction et de mise à jour. Alors, comment obtenons-nous R?
Rappelons que pour notre exemple à un seul capteur, nous avons défini rr comme la variance du
signal de bruit d'observation vk; c'est-à-dire, combien cela varie autour de sa valeur moyenne
(moyenne). Pour un système avec plus de deux capteurs, RR est une matrice contenant la covariance
entre chaque paire de capteurs. Les éléments sur la diagonale de cette matrice seront la valeur rr
pour chaque capteur, c'est-à-dire la variance de ce capteur avec lui-même. Les éléments situés en
dehors de la diagonale représentent la variation du bruit d'un capteur par rapport à celui d'un autre.
Pour cet exemple, et de nombreuses applications du monde réel, nous supposons que ces valeurs
sont nulles. Disons que nous avons observé nos deux thermomètres dans des conditions climatiques
de température constante, et observé que leurs valeurs fluctuent en moyenne de 0,8 degré; c'est-à-
dire que l'écart type de leurs lectures est de 0,8, rendant la variance 0,8 * 0,8 = 0,64. Cela nous
donne la matrice R:
Maintenant, nous pouvons aussi voir pourquoi Pk et Gk doivent être des matrices: comme
mentionné dans une note plus bas, Pk est la covariance du processus d'estimation à l'étape k; Ainsi,
comme la matrice de covariance de capteur R, Pk est également une matrice. Et, puisque Gk est le
gain associé à ces matrices à chaque étape, Gk doit également être une matrice, contenant une
valeur de gain pour chaque valeur de covariance dans ces matrices. Les tailles de ces matrices Pk et
Gk sont bien entendu déterminées par ce qu'elles représentent. Dans notre exemple, la taille de Pk
est 1 × 11 × 1 (c'est-à-dire, une seule valeur), car elle représente la covariance de la valeur
d'estimation à valeur unique x ^ k avec elle-même. Et le gain Gk est une matrice de 1 × 2 (une ligne,
deux colonnes), car il relie l'estimation d'état unique x ^ k deux les deux observations de capteur
dans zk.
Avec cette compréhension de la fusion des capteurs, laissons de côté nos thermomètres et
revenons à notre exemple d'avion. En mettant tout cela ensemble, nous obtenons les équations
suivantes pour la prédiction et la mise à jour pour notre avion (en utilisant des valeurs de bruit de
covariance entre 0 et 200 pieds, comme avant):
Prédire:

Mettre à jour:
En fin de compte, notre modèle de transition d'état appauvri peut nous causer des problèmes si
nous ne réintroduisons pas quelque chose que nous avons mentionné plus tôt: le bruit de processus.
Rappelons que notre équation complète pour la transition d'état dans un système à une seule
variable était
où wk est le bruit de processus à un moment donné. Avec notre connaissance de l'algèbre

linéaire, nous allons maintenant écrire cette équation
mais le fait demeure que nous n'avons toujours pas pris en compte le bruit de processus dans
notre modèle de prédiction / mise à jour. Faire cela s'avère plutôt facile. Tout comme nous avons
utilisé R pour représenter la covariance du bruit de mesure vk, nous utilisons Q pour représenter la
covariance du bruit de processus wk. Ensuite, nous apportons une légère modification à notre
prédiction Pk, en ajoutant simplement cette covariance:
Ce qui est intéressant, c'est que même de très petites valeurs pour les éléments non nuls de cette
matrice QQ s'avèrent très utiles pour garder nos valeurs d'état estimées sur la bonne voie.
Voici enfin une petite démonstration de fusion de capteurs, qui vous permet d'expérimenter avec
les valeurs de R et Q, et également de changer la quantité de biais (inexactitude constante ou valeur
moyenne du bruit) dans chacun des deux capteurs. Comme vous pouvez le voir, lorsque les capteurs
sont polarisés dans des directions différentes, la fusion du capteur peut fournir une approximation
plus proche du «vrai» état du système que ce que vous pouvez obtenir avec un seul capteur.


Partie 15: Non-linéarité

A présent, il devrait être évident que l'algèbre linéaire est assez génial, nous permettant
d'exprimer un algorithme sophistiqué comme le filtre de Kalman dans une forme très compacte.
L'algèbre linéaire n'est cependant pas toute l'histoire. Comme son nom l'indique, l'algèbre linéaire se
limite à représenter des relations qui sont linéaires, c'est-à-dire caractérisées par une ligne droite.
Pour voir ceci, considérons encore notre exemple simple de multiplication vectorielle de matrice *:
La démo ci-dessous vous permet d'ajuster les valeurs des constantes a, b, c et dd pour voir
comment le vecteur résultant change, pour un segment de ligne arbitraire contenant des points (x,
y). tous les paramètres donnent un autre segment de droite:
D'un autre côté, sortez et faites une promenade, et vous apprécierez que très peu dans la nature
est linéaire. Les choses que vous verrez avec les lignes droites - les bâtiments, les routes, les poteaux

téléphoniques, et autres - sont pour la plupart d'origine humaine, et la plupart de tout le reste
(arbres, oiseaux, nuages) est courbé ou fractal. Et parce que les capteurs et les moteurs et autres
artefacts sont fabriqués à partir de matériaux physiques, leur comportement tend également vers la
non-linéarité, en dehors d'une gamme limitée.
À peu près toutes les fonctions que vous avez apprises en maths de lycée, comme f (x) =
2
ax + bx + c, f (x) = sin (x), log (x), etc., sont non linéaires, donc il y a une grande variété de choix. La
démo ci-dessous vous permet de passer le segment de la démo précédente à travers quelques
fonctions non linéaires différentes que j'ai choisies pour leur aspect intéressant:
Partie 16: Traiter la non-linéarité

Bien que la non-linéarité introduise un tout nouveau monde de variations possibles dans
n'importe quel système, l'espoir n'est pas perdu. Comme vous l'avez peut-être remarqué dans la
démo précédente, les trois premiers exemples non linéaires ont une propriété utile en commun: ils
sont tous courbés en douceur, contrairement à l'exemple final, qui a un virage serré. Les
mathématiciens se réfèrent à de telles fonctions courbes incurvées comme différenciables. Comme
l'attestent tous ceux qui ont étudié le calcul, les fonctions différentiables (courbes lisses) peuvent
être modélisées à un degré de précision arbitraire par une suite de segments de droite
successivement plus petits. Même sans calcul, vous pouvez le voir dans la démo ci-dessous, ce qui
vous permet d'approximer la fonction f(x) = x2 en ajustant la taille du segment de droite Δx:

Alors, comment cette astuce de segment de ligne aide-t-elle à gérer les relations non linéaires
dans notre filtre de Kalman? Considérons un filtre très simple avec l'équation linéaire suivante pour
son capteur:
Ces équations indiquent que la lecture du capteur est toujours trois fois la valeur d'état
correspondante, plus deux: quelle que soit la valeur de l'état, la lecture du capteur est toujours trois
fois cette valeur plus deux. Considérons maintenant une équation de capteur non linéaire:
Cette équation dit que la lecture du capteur est le logarithme (à la base 2) de la valeur d'état: une
relation typique, par exemple, dans notre sensation de tangage en fonction de la fréquence. Même si
vous n'avez jamais entendu parler d'un logarithme auparavant, un rapide coup d'œil sur le tableau
suivant des valeurs approximatives montre que la relation entre l'état xk et la lecture du capteur zk
n'est pas aussi simple que la précédente:
Ici, vous pouvez voir qu'il n'y a pas de constantes a et telles que zk = axk + b. Nous pouvons,
cependant, utiliser notre aperçu de segment de ligne ci-dessus pour dériver un ensemble différent de
valeurs ak et bk, un pour chaque pas de temps, qui se rapproche d'une telle relation. Si vous avez
étudié le calcul, vous pouvez vous rappeler que nous pouvons calculer directement la première
dérivée de nombreuses fonctions non linéaires (log, sin, cos, etc.). Si vous n'avez pas étudié le calcul,
ne vous sentez pas mal: la dérivée première d'une fonction est vraiment la meilleure approximation
linéaire (segment de ligne) de cette fonction à chaque point donné. Un peu de googler révèle que la
première dérivée de log2 (x) est d'environ 1 / 0.693x, ce qui est logique: lorsque xx augmente, la
valeur de log2 (x) augmente et plus progressivement.
Partie 17: Un filtre de Kalman non linéaire

Alors que pouvons-nous faire avec cette première dérivée? Eh bien, voici l'ensemble des
équations pour notre filtre de Kalman linéaire, en utilisant un modèle sans transition d'état ou un
signal de contrôle, un peu de bruit de processus, un seul capteur et une seule valeur d'état:
Modèle:
Prédire:

Mettre à jour:
Nous allons maintenant modifier ces équations pour refléter la non-linéarité de notre capteur. En
utilisant une fonction h pour représenter n'importe quelle fonction non linéaire (comme log2 dans
notre exemple) [16], et ck pour représenter sa première dérivée à timestep k, on obtient:
Modèle:
Prédire:
Mettre à jour:
Comme notre démo de fusion de capteurs, la démo ci-dessous montre une fluctuation de
température variant dans le temps, mais avec un seul capteur ayant une réponse non linéaire et
aucune polarisation. Vous pouvez choisir parmi trois fonctions de capteur non linéaires différentes et
comparer notre filtre de Kalman non linéaire à la version linéaire. Comme vous pouvez le voir, aucun
ajustement du paramètre cc dans la version linéaire n'est suffisant pour s'adapter au signal d'origine
comme vous pouvez le faire avec la version non linéaire. Le graphique à droite montre la forme de la
fonction non linéaire à titre de référence:


Partie 18: Calculer le dérivé

Si vous l'avez fait jusqu'ici, vous êtes dans une très bonne position pour comprendre le filtre
étendu de Kalman. Il y a juste deux autres choses à considérer:
 Comment calculer la dérivée première à partir d'un signal réel, sans connaître sa
fonction sous-jacente.
 Comment généraliser notre modèle d'observation / état non linéaire à valeur unique
aux systèmes à valeurs multiples que nous avons envisagés.
Pour répondre à la première question, nous notons que la première dérivée d'une fonction est
définie comme la limite de la différence entre les valeurs successives de cette fonction, divisée par le
pas de temps, au fur et à mesure que le pas de temps approche de zéro:
Si vous ne comprenez pas cette équation, ne vous inquiétez pas: il suffit de penser à soustraire les
différences successives d'un signal yy pour approcher sa dérivée première:
En effet, comme le montre la démo ci-dessous, cette formule de différence finie est souvent une
très bonne approximation de la dérivée première. La démo vous permet de choisir parmi les mêmes
trois fonctions que sur la page précédente (affichée dans l'intervalle [0,1]), mais cette fois vous
pouvez choisir entre la dérivée et la différence finie:

Si un signal (comme la valeur du capteur zk) est une fonction d'un autre signal (comme l'état xk),
on peut diviser les différences successives du premier signal par des différences successives du
second signal:
Partie 19: Le Jacobien

Pour répondre à notre deuxième question - comment généraliser notre modèle d'observation /
état non linéaire à valeur unique à un système à valeurs multiples - il sera utile de rappeler l'équation
pour la composante du capteur de notre modèle linéaire:
Pour un système avec deux valeurs d'état et trois capteurs, nous pouvons réécrire ceci:

Bien que vous n'ayez peut-être pas vu de notation de ce genre auparavant, c'est assez simple:
l'index numérique double sur les éléments de la matrice C indique la position des lignes et des
colonnes de chaque élément, mais surtout la relation exprimée. Par exemple, c12 est le coefficient
(multiplicateur) reliant la valeur courante zk1 du premier capteur au deuxième composant xk2 de
l'état courant.
Pour un modèle non linéaire, il y aura également une matrice dont le nombre de rangées est égal
au nombre de capteurs et le nombre de colonnes est égal au nombre d'états; cependant, cette
matrice contiendra la valeur actuelle de la première dérivée de la valeur du capteur par rapport à
cette valeur d'état. Les mathématiciens appellent une telle dérivée une dérivée partielle, et la
matrice de tels dérivés qu'ils appellent le jacobien. Le calcul du jacobien dépasse le cadre du présent
tutoriel [17], mais ce tutoriel EKF basé sur Matlab et cette implémentation basée sur Matlab avec des
exemples GPS montrent qu'il implique relativement peu de code.
Si ces concepts semblent confus, pensez à une enquête dans laquelle on demande à un groupe de
personnes d'évaluer deux produits différents sur une échelle (disons 1 à 5). Le score global attribué à
chaque produit sera la moyenne de toutes les évaluations des personnes sur ce produit. Pour voir
comment une personne a influencé la note globale d'un produit unique, nous examinons la note de
cette personne pour ce produit. Chaque notation de personne / produit est comme une dérivée
partielle, et la table de ces évaluations de personne / produit est comme le jacobien. Remplacez les
personnes par des capteurs et des problèmes d'état, et vous comprenez le modèle de capteur du
filtre Kalman étendu.
Tout ce qui reste à ce stade est de généraliser notre modèle de capteur / état non linéaire au
modèle de transition d'état. En d'autres termes, notre modèle linéaire
devient
où A est remplacé par le jacobien de la fonction de transition d'état f. En fait, la convention est
d'utiliser Fk pour ce Jacobien (puisqu'il correspond à la fonction f et change dans le temps), et
d'utiliser Hk pour le Jacobien de la fonction de capteur h. En incorporant le signal de contrôle uk dans
la fonction de transition d'état, nous avons obtenu le "Monty complet" pour le filtre étendu de
Kalman que vous rencontrerez probablement dans la littérature:
Modèle:

Prédire:
Mettre à jour:
Partie 20: TinyEKF

Si vous êtes arrivé si loin, vous êtes prêt à expérimenter avec une implémentation EKF réelle.
TinyEKF est une implémentation C / C ++ simple que j'ai écrite principalement pour fonctionner
sur un microcontrôleur comme Arduino, Teensy, et la ligne STM32 utilisée dans les contrôleurs de vol
populaires comme Pixhawk, Multiwii32 et OpenPilot. Ayant examiné le code EKF dans certains de ces
contrôleurs de vol, j'ai trouvé le code difficile à mettre en relation avec la compréhension exprimée
dans ce tutoriel. J'ai donc décidé d'écrire une implémentation EKF simple qui serait pratique à utiliser
sur un microcontrôleur réel, prenant une "petite" quantité de mémoire, tout en restant
suffisamment flexible pour être utilisé sur différents projets. J'ai aussi écrit une implémentation
Python, donc vous pouvez prototyper votre EKF avant de l'exécuter sur un microcontrôleur réel.
TinyEKF vous oblige à écrire une seule fonction de modèle, en remplissant les valeurs de la
fonction de transition d'état f (x) f (x), sa matrice jacobienne F (x) F (x), la fonction de capteur h (x) h
(x), et son H Jacobien (x) H (x). La prédiction et la mise à jour sont ensuite traitées automatiquement
en passant le vecteur d'observation zz à la fonction d'étape.
Trois exemples sont fournis:
1. un exemple pur en utilisant des données satellitaires GPS stockées dans un fichier,
basé sur l'implémentation Matlab de You Chong
2. un exemple de fusion de capteur C ++ qui s'exécute sur Arduino
3. un exemple de suivi de souris Python (nécessite OpenCV)

Bibliographie
[ "I have also ignored the control-signal component of the state equation, because it is
1] tangential to most of the Kalman Filter equations and can be easily added when needed.".
[ "La variable k est généralement utilisée pour le gain, car elle est connue sous le nom de gain
2] de Kalman. Avec tout le respect dû à Rudolf Kalman, je trouve confus d'utiliser la même lettre
pour une variable et un indice, donc j'ai opté pour la lettre gg à".
[ " Techniquement, r est vraiment la variance du signal de bruit; c'est-à-dire l'écart, ou la

3] distance moyenne au carré des valeurs de bruit individuelles à partir de leur moyenne. Le filtre de
Kalman fonctionnera également bien si cette variance de bruit est permise de changer avec le
temps, mais dans la plupart des applications, il peut être supposé constant. De même, pk est
techniquement la covariance du processus d'estimation à l'étape k; c'est la moyenne de l'erreur
quadratique de nos prédictions. En effet, comme l'a souligné Tim Wilkin, l'état est une variable /
un vecteur stochastique [aléatoire] (une valeur instantanée d'un processus stochastique) et il n'a
pas de valeur «vraie» du tout! L'estimation est simplement la valeur la plus probable pour le
modèle de processus décrivant l'état.".
[ " Techniquement, la première estimation est appelée a priori, et la seconde a posteriori, et la

4] plupart des traitements introduisent un exposant ou un indice supplémentaire pour montrer la
distinction. Parce que j'essaie de garder les choses simples (et facile à coder dans votre langage
de programmation préféré!), J'évite de compliquer la notation plus loin.".
[ "Comme Zichao Zhang m'a aimablement fait remarquer, nous multiplions deux fois par aa car
5] l'erreur de prédiction pk est elle-même une erreur au carré; par conséquent, il est mis à l'échelle
par le carré du coefficient associé à la valeur d'état xk. La rais".
[ "En effet, si vous regardez le code source de cette page, vous verrez que le JavaScript pour la
6] prédiction et la mise à jour est encore plus simple que les formules:".
// Predict
xhat = a * xhat;
p = a * p * a;
// Update
g = p / (p + r);
xhat = xhat + g * (z - xhat);
p = (1 - g) * p;
Je remercie Marco Camurri et John Mahoney d'avoir souligné les incohérences dans
l'utilisation de ces équations, dans les versions antérieures de ce tutoriel.
[7] J'ai ajouté le bruit d'une distribution uniforme plutôt que la distribution gaussienne
(normale) assumée par le filtre de Kalman, mais cela ne fait pas beaucoup de différence du point
de vue de cette démo.
[8] Par souci de cohérence avec l'exemple original, j'ai choisi d'utiliser ici les variables a, b et c
pour les constantes, au lieu des plus communes f, b et h.

Utilisation d'un filtre Kalman étendu pour le suivi

des objets dans Simulink
Source : http://www.goddardconsulting.ca/simulink-extended-kalman-filter-tracking.html
Ce tutoriel présente un exemple d'implémentation d'un filtre Extended Kalman dans Simulink. Si
vous n'êtes pas familier avec Simulink, regardez ici quelques tutoriels génériques sur Simulink pour
savoir comment créer et exécuter des modèles simples. Si vous n'êtes pas familier avec les
mathématiques derrière le filtre Kalman ou le filtre Kalman étendu, consultez les didacticiels liés.
L'une des premières applications du filtre Kalman étendu consistait à résoudre le problème du
suivi des objets volants. Le problème de base est illustré à la figure 1.
Figure 1: Relation entre les déplacements et la portée.
A chaque instant, l'objet suivi a une portée et un relèvement donnés par l'observateur. Souvent,
l'observateur est considéré comme l'emplacement d'une antenne radar qui suit l'objet. La portée et
le relèvement sont générés à partir des déplacements (c'est-à-dire des distances par rapport à
l'observateur) dans les directions x et y. (Ce tutoriel ne considère pas l'altitude de l'objet.)
Le problème de suivi consiste à estimer non seulement les déplacements x et y de l'objet, mais
aussi ses vitesses x et y. Ces quatre états doivent être estimés avec seulement des mesures bruitées
de portée et de relèvement. Etant donné que les déplacements et les vitesses sont liés de manière
non linéaire à la plage, il est idéal de résoudre ce problème en utilisant un filtre de Kalman étendu.
L'algorithme Extended Kalman Filter nécessite le calcul de matrices jacobiennes pour l'état et les
équations de mesure. Ceux-ci prennent les formes suivantes. Sur une petite période, le déplacement
peut être considéré comme évoluant selon l'approximation du premier ordre,
Jacobien pour l'équation de mise à jour d'état.

L'équation ci-dessus indique que sur une courte période de temps, la position change de Δt fois la
vitesse (dans les directions x et y) et que la vitesse reste constante (dans les deux directions x et y).
Fk est la matrice jacobienne requise.
L'équation de mise à jour des mesures est légèrement plus complexe - en s'appuyant sur la
différenciation d'une identité trigonométrique. La portée et le relèvement sont liés aux
déplacements x et y par les équations,
Déplacement x-y vers l'équation de portée et de relèvement.
(Notez que l'indice de temps d'échantillonnage k a été supprimé du vecteur d'état pour éviter
d'encombrer les équations.) D'où le jacobien pour les équations de mesure est donné par,
Jacobien pour l'équation de mesure.
 Une implémentation Simulink
Un modèle Simulink qui implémente le problème de suivi de base discuté ci-dessus et qui utilise
un filtre de Kalman étendu pour estimer la trajectoire de l'objet est illustré à la figure 2.
Modèle Simulink pour le suivi d'un objet volant à l'aide d'un filtre étendu de Kalman.
Figure 2: Modèle Simulink pour le suivi d'un objet volant à l'aide d'un filtre Kalman étendu.
Ce modèle comporte deux parties principales: premièrement, les blocs qui modélisent la
trajectoire réelle de l'objet suivi; et deuxièmement, le filtre étendu de Kalman utilisé pour estimer la

trajectoire de l'objet à partir des données mesurées. Chacun de ceux-ci est décrit plus en détail ci-
dessous.
La trajectoire réelle
Les blocs colorés en noir sont utilisés pour modéliser la trajectoire réelle d'un objet volant dans un
espace bidimensionnel. En fin de compte, les propriétés mesurées sont la gamme et le relèvement.
Ceux-ci sont calculés à partir des déplacements x et y, qui sont générés par des vitesses d'intégration
qui, à leur tour, sont générées par l'intégration d'accélérations.
Les accélérations sont générées par le modèle d'accélération présenté à la figure 3. Un modèle
plus simple pourrait utiliser soit une vitesse constante (soumise à des perturbations aléatoires) soit
une accélération constante (soumise à des perturbations aléatoires). Ce modèle suppose des
accélérations aléatoires en série. Cela correspond à un objet manœuvrant avec une vitesse qui ne
peut pas changer trop rapidement. (Le gain dans le filtre du premier ordre détermine la vitesse à
laquelle la vitesse peut changer.)
Figure 3: Modèle d'accélération.
Dans cet exemple, l'objet est supposé commencer dans le nord-ouest et se déplacer vers l'est à
100 m / s. Cela correspond aux paramètres donnés dans le tableau suivant,
Quantité Valeur
Position initiale x -1000 m
Position initiale y -1000 m
Vitesse initiale x 100 m / s
Vitesse initiale y 0m/s
x puissance de bruit de déplacement 10
y puissance de bruit de déplacement 0,1
Le filtre étendu de Kalman
Le filtre étendu de Kalman utilise un algorithme prédicteur-correcteur pour estimer les états non
mesurés d'un processus discret. Une discussion des mathématiques derrière le filtre de Kalman
étendu peut être trouvée dans ce tutoriel.
Pour le problème de suivi considéré, les données mesurées sont la portée et le relèvement réels
de l'objet corrigés avec un bruit gaussien de moyenne nulle et échantillonnés à des intervalles de
0,1s. Le bruit de gamme a une variance de 50 tandis que le bruit de roulement a une variance de
0,005.

Le filtre Kalman étendu lui-même a été implémenté à l'aide d'un bloc fonctionnel MATLAB
intégré. Le bloc est discret avec un temps d'échantillonnage de 0,1 seconde. Le code du bloc est
indiqué ci-dessous.
Notez que le filtre a été délibérément initialisé avec des données erronées pour refléter le fait que
la trajectoire réelle n'est pas connue à l'avance.
function xhatOut = ExtKalman(meas,dt)

% This Embedded MATLAB Function implements an extended Kalman filter used
% for object tracking.
%
% The states of the process are given by
% x = [x_position; x_velocity; y_position; y_velocity];
%
% and the measurements are given by
% y = [range; bearing]
%
% where
% range = sqrt(x_position^2 + y_position^2)
% bearing = atan2(y_position/x_position)
% Author: Phil Goddard (phil@goddardconsulting.ca)

% Date: Q2, 2011.
% Define storage for the variables that need to persist

% between time periods.
persistent P xhat Q R
if isempty(P)
% First time through the code so do some initialization
xhat = [-900;80;950;20];
P = zeros(4,4);
Q = diag([0 .1 0 .1]);
R = diag([50^2 0.005^2]);
end
% Calculate the Jacobians for the state and measurement equations
F = [1 dt 0 0;0 1 0 0;0 0 1 dt;0 0 0 1];
rangeHat = sqrt(xhat(1)^2+xhat(3)^2);
bearingHat = atan2(xhat(3),xhat(1));
yhat = [rangeHat; bearingHat];
H = [cos(bearingHat) 0 sin(bearingHat) 0;
-sin(bearingHat)/rangeHat 0 cos(bearingHat)/rangeHat 0];
% Propogate the state and covariance matrices
xhat = F*xhat;
P = F*P*F' + Q;
% Calculate the Kalman gain
K = P*H'/(H*P*H' + R);
% Calculate the measurement residual
resid = meas - yhat;
% Update the state and covariance estimates
xhat = xhat + K*resid;
P = (eye(size(K,1))-K*H)*P;
% Post the results
xhatOut = xhat;
Résultats de la simulation
Les résultats de simulation sont montrés sur la Figure 4. Chacun des états réel et estimé est
montré sur un axe séparé, avec la trajectoire réelle montrée en bleu et la trajectoire estimée en
rouge.

Notez encore que les estimations ont été délibérément initialisées avec des valeurs non exactes.
Cela signifie que le filtrage prend du temps pour converger vers une estimation acceptable.
Cependant, après 10-15 secondes, les estimations suivent les trajectoires réelles avec une précision
raisonnable. Les estimations pourraient être encore améliorées en ajustant les estimations de la
covariance du bruit avant (et pendant) l'application du filtrage.
Figure 4: Positions et vitesses réelles et estimées.
Il est souvent utile d'effectuer un suivi de trajectoire pour afficher les résultats sur un tracé
polaire. Ceci est illustré à la figure 5. L'objet se déplace du nord-ouest directement à l'est et se
termine au nord-est. À partir d'une portée d'environ 1400 mètres, l'objet se trouve à moins d'un
kilomètre de l'observateur et se déplace à nouveau à une distance d'environ 1400 mètres. Cela
confirme que le filtre prend peu de temps pour estimer correctement la position de l'objet, mais qu'il
fait ensuite un bon travail en suivant sa trajectoire.

Figure 5: Tracé polaire des trajectoires réelles et estimées.

Le Filtre Kalman Étendu Extended Kalman Filter

Transféré par

Informations du document

Titre original

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Le Filtre Kalman Étendu Extended Kalman Filter

Transféré par

Droits d'auteur :

Formats disponibles

Le filtre étendu de

Kalman :un didacticiel

Traduit par Abderrahim TAOUNI

Le filtre étendu de Kalman: un didacticiel interactif pour

Partie 1: Un exemple simple

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 1

Partie 2: Faire face au bruit

Partie 3: S’y mettre ensemble

altitudecurrent_time = 0.98 * altitudeprevious_time

observed_altitudecurrent_time = altitudecurrent_time + noisecurrent_time

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 2

Partie 4: Estimation de l'État

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 3

Partie 5: Calculer le gain

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 4

Qu'en est-il de la troisième formule, calculant l'erreur de prédiction pk récursivement à partir de

Partie 6: Prédiction et mise à jour

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 5

Partie 7: Exécution du filtre

Pour essayer notre filtre, nous aurons besoin de:

 Une séquence d'observations zk

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 6

Partie 8: Un modèle plus réaliste

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 7

avec le nouveau composant surligné en bleu.

Partie 9: Modifier les estimations

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 8

Partie 10: Ajouter de la vélocité au système

avec la forme plus générale:

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 9

Cette équation nous rapproche de notre forme générale

Partie 11: Algèbre linéaire

que nous essayons de réconcilier avec une équation plus générale

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 10

Revenons à la tâche à accomplir, nous définissons une matrice

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 11

ce qui fonctionne comme nous voulons:

Partie 12: Prévision et mise à jour révisées

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 12

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 13

mais la mise à jour de gain implique une division:

Puisque la multiplication et la division sont si étroitement liées, nous rencontrons un problème

Maintenant, nous transformons c, r, et g et la constante 1 en matrices, en transposant C comme

Alors comment calculer , l'inverse de la matrice ? Tout comme

où (pour une matrice 3x3)

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 14

Partie 13: Introduction au Fusion des capteurs

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 15

Partie 14: Exemple de fusion de capteurs

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 16

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 17

où wk est le bruit de processus à un moment donné. Avec notre connaissance de l'algèbre

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 18

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 19

Partie 15: Non-linéarité

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 20

Partie 16: Traiter la non-linéarité

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 21

Partie 17: Un filtre de Kalman non linéaire

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 22

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 23

A.TAOUNI | Le filtre étendu de Kalman: un didacticiel interactif pour les non-experts 24

Partie 18: Calculer le dérivé