« Différents » filtres de Kalman : 1. Fusion de mesures

(1)

UPJV, Département EEA Master 2 3EA, EC53 UE obligatoire alternants

Fabio MORBIDI

Laboratoire MIS

Équipe Perception Robotique fabio.morbidi@u-picardie.fr

AU 2019-2020

Mardi, Mercredi et Jeudi

9h30-12h00 (CM ou TD, salle CURI 305) 9h00-12h00 (TP, salle TP204)

(2)

Ch. 2: Traitement des mesures

Réseaux multi-capteurs

Fusion des mesures Partie 2

Partie 1

(3)

« Différents » filtres de Kalman : 1. Fusion de mesures

2. Fusion de mesures dans le temps

3. Fusion de mesures de capteurs en mouvement

• Combinaison des évidences (mesures) de façon optimale

• Moyenne récursive

• Filtre de Kalman linéaire (systèmes dyn. linéaires)

• Filtre de Kalman étendu ou EKF (systèmes dyn. non linéaires)

(4)

Notation:

: état du système (scalaire, pour plus de simplicité) : mesure du capteur

: variance de la mesure du capteur

, : estimation (optimale) de et variance de

1. Fusion de mesures

x

Nos hypothèses:

Deux mesures non corrélées:

Bruits blancs gaussiens à moyenne zéro:

où

x = z

¹

+ K ( z

²

− z

¹

)

σ

²

= (1 − K ) σ

1²

K = σ

1²

σ

₁²

+ σ

₂²

z

_i

σ_i²

z

_i

= x + r

_i

, i ∈ { 1 , 2 } r

_i

∼ N (0 , σ

_i²

)

i i

σ

²

x

est le gain de Kalman

Estimation de l’état optimum:

(5)

Dans le problème de localisation d’un robot nous avons

combiné les deux mesures de distance et en utilisant l’équation:

Remarque:

z = (1 − w ) z

¹

+ w z

²

= z

¹

+ w ( z

²

− z

¹

)

avec le poids optimal:

w = σ

1²

σ

1²

+ σ

2²

z

¹

z

²

1. Fusion de mesures

(6)

En outre, nous avons vu que:

Var[ z ] = (1 − w )

²

σ

1²

+ w

²

σ

2²

=

1 − σ

1²

σ

1²

+ σ

2²

²

σ

1²

+

σ

1²

σ

1²

+ σ

2²

²

σ

2²

=

1 − σ

1²

σ

1²

+ σ

2²

σ

1²

= (1 − w ) σ

1²

1. Fusion de mesures

(7)

est le gain de Kalman

z

¹

z

²

z ( t

1

) , z ( t

2

) t

¹

< t

²

Estimation de l’état optimum à l’instant :

t

²

x ( t

²

) = z ( t

¹

) + K ( z ( t

²

) − z ( t

¹

))

σ

²

= (1 − K ) σ

_z²₍_t₁₎

où

K = σ

_z²₍_t₁₎

σ

_z²₍_t₁₎

+ σ

_z²₍_t₂₎

• Introduisons maintenant le temps

• Les mesures et sont prises de façon

séquentielle, à savoir nous avons avec

t

2. Fusion de mesures dans le temps

(8)

• Que se passe-t-il si nous prenons une autre mesure à l’instant ?

Estimation de l’état optimum à l’instant générique

où

z ( t

³

) t

³

> t

²

t

k

K ( t

k−1

) = σ

²

( t

k−1

)

σ

²

( t

_k−¹

) + σ

_z²₍_t_k₎

x ( t

k

) = x ( t

k−1

) + K ( t

k−1

)( z ( t

k

) − x ( t

k−1

))

σ

²

( t

_k

) = (1 − K ( t

_k−¹

)) σ

²

( t

_k−¹

)

_“Moyenne

recursive”

est le gain de Kalman (variable dans le temps)

2. Fusion de mesures dans le temps

(9)

Entrée du filtre de Kalman: la commande et les mesures

Sortie du filtre de Kalman: l’estimation de l’état du système et la matrice de covariance de l’erreur d’estimation associée

A B Q R

Matrices du système dynamique:

x

u

z ^P

x

Étant donné un système dynamique linéaire à temps discret d’état

H

z u

P

x

3. Systèmes dynamiques

(10)

Filtre de Kalman linéaire

• Soit le système dynamique à temps discret, :

x

_k

= A

_k−¹

x

_k−¹

+ B

_k−¹

u

_k−¹

+ w

_k−¹

• Le bruit de modèle et le bruit de mesure sont supposés à moyenne zéro, blancs (sans mémoire) et gaussiens avec matrices

de covariance et , respectivement. Les bruits de modèle et de mesure sont supposés aussi non corrélés. En résumé, nous avons:

w

k−1

Qk

R

k

k ∈ {1, 2, . . .}

Entrée de commande (connue)

z

_k

= H

_k

x

_k

+ r

_k

r

k

Équation d’état Équation de mesure

E [ w

k

] = 0 E [ r

k

] = 0

E [ w

_k

w

_j^T

] = 0, j = k E [ r

_k

r

^T_j

] = 0, j = k E [ r

_k

r

^T_k

] = R

_k

E [ r

_k

w

^T_j

] = 0, ∀ j, k

E [ w

_k

w

_k^T

] = Q

_k

(11)

• Le filtre de Kalman linéaire à temps discret est un estimateur récursif

Pour estimer l'état courant , on a besoin uniquement de connaître l'estimé de l'état précédent et les mesures courantes: l'historique des mesures et des estimés n'est pas nécessaire

: estimé de l'état du système à l'instant , en utilisant l’information jusqu’à l’instant

x

_k|k

P

_k|k

= E [( x

_k

− x

_k|k

)( x

_k

− x

_k|k

)

^T

]

: matrice de covariance de l'erreur d’estimation

de l’état à l'instant , en utilisant l’information jusqu’à l’instant

Erreur d'estimation de l’état du système

x

_k|k xk

1.

2.

xk

Filtre de Kalman linéaire

x

_k

k k

• L' « état » du filtre se compose de deux variables:

(12)

Initialisation du filtre:

Prédiction (ou propagation):

x

_k|k−¹

= A

k−1

x

_k−¹_|k−¹

+ B

k−1

u

k−1

P

_k|k−1

= A

_k−¹

P

_k−1|k−1

A

^T_k−1

+ Q

_k−¹

Pour :

k ∈ { 1 , 2 , . . .}

x

⁰_|⁰

= E [ x

⁰

]

où est l’état initial du système

Filtre de Kalman linéaire

x⁰ ∈ Rⁿ

P

⁰_|⁰

= E [( x

⁰

− x

⁰_|⁰

)( x

⁰

− x

⁰_|⁰

)

^T

] ∈ R

^n×n

(13)

Correction (ou mise à jour):

est la matrice d'estimation a posteriori de la covariance de l'erreur

x

_k|k

= x

_k|k−¹

+ K

k

( z

k

− H

k

x

_k|k−¹

)

où

K

k

= P

_k|k−1

H

^T_k

( H

k

P

_k|k−1

H

^T_k

+ R

k

)

⁻¹ est le gain de Kalman

est la matrice d'estimation a priori de la covariance de l'erreur

P

_k|k

P

_k|k−1

Filtre de Kalman linéaire

Remarque:

P

_k|k

= ( P

⁻_k|k−¹ ₁

+ H

^T_k

R

⁻_k ¹

H

_k

)

⁻¹

= ( I

_n

− K

_k

H

_k

) P

_k|k−1

(14)

Soit le système dynamique non linéaire à temps discret :

• Comme pour le filtre de Kalman linéaire:

Le bruit de modèle et le bruit de mesure sont supposés blancs (sans mémoire), à moyenne zéro, et gaussiens avec matrices de covariance et ,

respectivement. Les bruits de modèle et de mesure sont aussi supposés non corrélés

w

_k−¹

Qk

R

_k k ∈ {1,2, . . .}

Entrée de commande (connue)

r

_k

Équation d’état Équation de mesure

x

_k

= f [ x

_k−¹

, u

_k−¹

] + w

_k−¹

z

_k

= h [ x

_k

] + r

_k

où et sont deux fonctions différentiables arbitraires f[· , ·] h[· ]

Filtre de Kalman étendu (EKF)

(15)

Initialisation du filtre:

P

0|0

= E [( x

⁰

− x

0|0

)( x

⁰

− x

0|0

)

^T

]

Phase de prédiction:

Pour :

k ∈ { 1 , 2 , . . .}

x

⁰_|⁰

= E [ x

⁰

]

où est l’état initial du système

x

⁰

x

_k|k−1

= f [ x

_k−1|k−1

, u

_k−¹

]

P

_k|k−1

= F

_k−¹

P

_k−1|k−1

F

^T_k−₁

+ Q

_k−¹

où

Fk−1 = ∂ f[x, u_k−¹]

∂ x

x=x_k−1|k−1

• Matrice jacobienne de la fonction

• On a considéré ici le

développement en série de

Taylor à l’ordre 1 de en x_k−1|k−1

f

Filtre de Kalman étendu (EKF)

(16)

Phase de correction:

où

K

k

= P

_k|k−¹

H

^T_k

( H

k

P

_k|k−¹

H

^T_k

+ R

k

)

⁻¹ Gain de Kalman

et

H

_k

= ∂ h [ x ]

∂ x

x =x_k|k−1

• Matrice jacobienne de la fonction

• On a considéré ici le développement

en série de Taylor à l’ordre 1 de en x_k|k−1

h h

P

_k|k

= ( P

⁻_k|k−¹ ₁

+ H

^T_k

R

⁻_k ¹

H

k

)

⁻¹

x

_k|k

= x

_k|k−¹

+ K

k

( z

k

− h [ x

_k|k−¹

])

Filtre de Kalman étendu (EKF)

(17)

• Un chariot à roues encodeuses avance pas-à-pas

• Un laser mesure la distance du chariot par rapport à un mur

Exemple I : une variable d’état

σ

^laser

= 0 . 1 m

x

mur

laser

Odométrie Laser

TD2, ex. 1

Exemple:

Une variable d’état:

la position du chariot

x

σ^odom = 0.01 m

z

(18)

Dans cet exemple, nous avons que (rappel la forme générale d’un système dynamique à temps discret vue précédemment):

où est le pas de discrétisation (fixe) et est la vitesse moyenne du chariot dans cet intervalle de temps

Exemple I : une variable d’état

Équation d’état:

Équation de mesure:

Δk V

x

_k

= x

_k−¹

+ V Δ k + w

_k−¹

z

_k

= x

_k

+ r

_k

Donc, en conclusion:

xk = xk, uk−1 = uk−1 = V Δk, wk−1 = wk−1, zk = zk, rk = rk

Ak−1 = 1, Bk−1 = 1, Qk = σ_odom² , Hk = 1, Rk = σ_laser²

(19)

: estimé de la position du robot avant le déplacement : estimé de la position du robot après le déplacement

et avant la mesure

: estimé de la position du robot après la mesure

Correction Prédiction

Exemple I : une variable d’état

x

_k−¹_|k−¹

x

_k−1|k−1

x

_k|k−¹

x

_k|k−1

x

_k|k

x

_k|k

u

_k−¹

u

k−1

z

_k

z

_k

z

k

Mesure par capteur laser Commandes

Déplacements

(20)

Prédiction:

Correction:

x

_k|k

= x

_k|k−¹

+ K

k

( z

k

− x

_k|k−¹

) P

_k|k

= (1 − K

k

) P

_k|k−¹

K

k

= P

_k|k−¹

( P

_k|k−¹

+ R )

⁻¹

= P

_k|k−¹

P

_k|k−¹

+ σ

_laser²

où

Exemple I : une variable d’état

Les équations du filtre de Kalman sont alors:

x

_k|k−1

= x

_k−1|k−1

+ V Δ k P

_k|k−1

= P

_k−1|k−1

+ σ

odom²

Fichier Matlab (TD2):

FiltreKalmanChariot.m

(21)

Exemple II : deux variables d’état

Un gyroscope et un compas magnétique sont placés sur une plate-forme mobile

• Les mesures de vitesse fournies par le gyroscope sont entachées par un bruit blanc gaussien:

(avec biais)

• Les mesures de position fournies par le compas sont aussi entachées par un bruit blanc gaussien:

(sans biais) La plate-forme tourne librement autour de son axe vertical

N ( μ

^gyro

, σ

gyro²

) = N (0 . 1 , 0 . 2

²

) deg/s

N ( μ

^comp

, σ

comp²

) = N (0 , 10

²

) deg

Les deux mesures sont supposées non corrélées

g c

θ

(22)

Objectif: fusionner ensemble les deux mesures

• Le mesures du gyroscope (capteur proprioceptif) sont

précises, mais elles dérivent (cf. l’odométrie dans l’Exemple I)

• Le mesures du compas (capteur extéroceptif) ne dérivent pas, mais elles sont fortement bruitées

donc, on peut conclure que:

• Les mesures du gyroscope sont bonnes à court terme

• Les mesures du compas sont bonnes à long terme

Exemple II : deux variables d’état

TD2, ex. 2

(23)

• L’état du système consiste en la position et en la vitesse angulaire de la plate-forme, à savoir:

• Étant un capteur proprioceptif, les mesures du gyroscope équivalent à une commande: . On peut donc définir le système dynamique suivant:

où est le pas de discrétisation (fixe) et

E [ w

k

] = [0 , μ

^gyro

]

^T

, Q = E [ w

k

w

_k^T

] = diag(0 , 16 σ

gyro²

)

Sur-estime, afin de prendre en compte le biais

x = [ θ, θ ˙ ]

^T

A B

(1)

Exemple II : deux variables d’état

Δ k

x

_k

=

1 Δ k

0 0 x

_k−¹

+

0 1 u

_k−¹

+ w

_k−¹

u

k

= ˙ θ

^gyro

( k )

(24)

• Les “vraies mesures” sont effectuées uniquement par le compas (capteur extéroceptif)

• En d’autres termes, nous ne voulons pas que le gyroscope intervienne dans la phase de correction du filtre de Kalman.

Donc on peut définir simplement:

où

E [ r

_k

] = μ

^comp

= 0 , R = E [ r

_k²

] = σ

comp²

(2)

Grâce à l’équation d’état (1) et de mesure (2) ainsi trouvées, on peut écrire les équations du filtre de Kalman pour la fusion des mesures du gyroscope et du compas

z

k

= [1 0] x

k

+ r

k

H

Exemple II : deux variables d’état

Fichier Matlab (TD2):

FiltreKalmanGyro Compas.m

(25)

Résultats de la simulation (20 mesures/s)

Angle [deg]Angle [deg]Angle [deg]

Temps [s]

197.5 198 198.5 199 199.5 200

0 20 40 60 80 100 120 140 160 180 200

0 0.5 1 1.5 2 2.5

0 20 40 60 80 0 10 20 3040

-10 -20 0 50 100

-50 -100

Position angul. réelle plate-forme Compas

Intégrale des mesures du gyroscope Estim. Kalman

Derive du gyro

Exemple II : simulation

^θ

* * O

(26)

Système de navigation inertielle (INS) pour un véhicule

• Avantages:

• Autonome

• Grande précision pour une navigation à court terme

• Inconvénient majeur:

L’erreur de positionnement augmente sans cesse au fils du temps, à cause de l’accumulation des erreurs du gyroscope et de

l’accéléromètre avec le temps, entre autres

Pour cette raison, pour une navigation à long terme un INS est souvent couplé avec un système d’aide à la navigation,

par exemple:

• Système de radionavigation (ex. LORAN, OMEGA et TACAN)

• Système de navigation par satellites (ex. GPS, GLONASS)

Exemple III: navigation intégrée, fusion INS/GPS

(27)

• L’intégration INS/GPS est souvent réalisée grâce à un filtre complémentaire

• Le filtre de Kalman (étendu) peut être utilisé dans certains schémas de filtrage complémentaire

Pour plus de détails, voir la Sect. 12.10 du livre de Bar-Shalom et al.

Architecture en boucle ouvert pour la fusion INS/GPS basée sur le filtre de Kalman

État + erreurs GPS

État + erreurs INS

erreurs GPS − erreurs INS

Estimé de l’état

≈ − erreurs INS

Exemple III: navigation intégrée, fusion INS/GPS

Filtre

+ +

(28)

Exemple IV : Projet ANR CityVIP (2009-2011)

“Déplacement sûr de véhicules individuels adaptés à l’environnement urbain” (source: D. Bétaille, IFSTTAR)

• Le milieu urbain cumule:

– l’intérêt d’un positionnement précis et intègre – mais plusieurs difficultés ...

• pour le positionnement GPS:

– les masquages de satellites – les trajets multiples

• pour le positionnement par traitement de vidéos:

– les problèmes d’ombres et d’éblouissements – les problèmes de réflexions

Mais le milieu urbain est fortement structuré et la connaissance d’une carte est d’un précieux secours Problématique de la géolocalisation en ville

(29)

Objectifs du projet CityVIP

•

Développer un système de localisation adapté aux véhicules urbains:

– précis (erreur inférieure à 1 m à 95%)

– intègre (estimation sûre d’un majorant de l’erreur) – bas coût (utilisation de capteurs bas de gamme)

• Le système de localisation exploite:

– un récepteur GPS bas de gamme (u-blox)

– les codeurs CAN des roues arrière du véhicule – une caméra vidéo N&B standard (Marlin)

– une carte numérique 3D précise d’un centre urbain ... et on tire le meilleur parti de toutes ces sources d’information (fusion)

Exemple IV : Projet ANR CityVIP (2009-2011)

(30)

Coopération des modules actualisant la pose

• Intégration de mesures proprioceptives (odométrie)

• Mise en correspondance avec la carte numérique 3D («map matching»)

• Utilisation de données satellitaires en couplage lâche ou serré:

⇒ Test préalable de validité Force du signal

Visibilité directe

• Fusion avec la localisation par vision

Exemple IV : Projet ANR CityVIP (2009-2011)

(31)

Localisation par vision : construction de la base d'amers visuels

Séquence vidéo de référence

Base d'amers visuels géoréférencés (points 3D +

descripteurs associés) Trajectoire de référence

Exemple IV : Projet ANR CityVIP (2009-2011)

(32)

Base d'amers visuels

Localisation par vision: exécution en temps réel

Points d'intérêt sur l'image courante (jaunes)

Estimation de la dose du véhicule et incertitude associée (ellipse jaune)

Appariement 3D/2D

Exemple IV : Projet ANR CityVIP (2009-2011)

(33)

Intégration dans le véhicule VERT

Mesure de référence des trajectoires

Codeurs de roues Baie d’acquisition

(RT-Maps, Aroccam)

Récepteur GPS bas coût

Exemple IV : Projet ANR CityVIP (2009-2011)

(34)

Résultats: localisation globale par fusion multi-capteurs et carte 3D

Véhicule

Exemple IV : Projet ANR CityVIP (2009-2011)

carte 3D

Vidéo: points d'intérêt

(35)

Localisation après fusion

lot 1 (carte) / lot 2 (vision)

Résultats: localisation globale par fusion multi-capteurs et carte 3D

Exemple IV : Projet ANR CityVIP (2009-2011)