Modélisation de l’incertitude et fusion des mesures

UPJV, Département EEA Master 2 3EA, EC53

UE optionnelle

Fabio MORBIDI, Mohammed CHADLI

Laboratoire MIS Équipes PR et COVE

{fabio.morbidi,mohammed.chadli}@u-picardie.fr

AU 2018-2019

Vendredi 8h30-12h30, Salle CURI 305 ou TP204

2

Introduction

Systèmes dynamiques multi-agents en réseau:

• 

Modélisation de l’incertitude et fusion des mesures

• 

Estimation distribuée

• 

Diagnostic et surveillance distribués

IoT

CSAIL-MIT

3

Plan du cours

Chapitre 1: Modélisation de l’incertitude

1. Introduction

3. Incertitude d’un capteur 2. Représentation de l’erreur

4. Propagation d’incertitude

Chapitre 2: Traitement des mesures

1. Réseaux multi-capteurs 2. Fusion des mesures

Partie I : F. Morbidi

Chapitre 3: Estimation distribuée basée sur le consensus

1. Dynamic average consensus estimators

3. Filtre de Kalman distribué

2. Méthode des moindres carrés distribuée

4

•  Introduction to Autonomous Mobile Robots, R. Siegwart, I.R. Nourbakhsh, D. Scaramuzza, MIT press, 2e éd., 2011 [Ch. 4]

Bibliographie

•  Graph Theoretic Methods in Multiagent Networks, M. Mesbahi, M. Egerstedt, Princeton University Press, 2010 [Ch. 8]

•  Estimation with Applications to Tracking and Navigation, Y. Bar-Shalom, X.R. Li, T. Kirubarajan, Wiley & Sons, 1re éd., 2001 [Ch. 5, 12]

•  Optimal Control and Estimation, R.F. Stengel, Dover Publications, 1994 [Ch. 4], 24 €

5

•  Diapos du cours

http://home.mis.u-picardie.fr/~fabio/Teaching_SDSM18-19.html

Bibliographie

6

Connaissances préalables …

On part de l'hypothèse que vous avez des bonnes connaissances de:

• 

Algèbre linéaire (manipulation de vecteurs et de matrices)

• 

Calcul différentiel (gradient, matrice jacobienne, etc.)

• 

Calcul des probabilités et théorie de l’estimation (variables aléatoires, densités de probabilités, etc.)

• 

Théorie des systèmes (équations différentielles ordinaires,

représentation d’état d’un système LTI à temps discret et à temps continu, protocole de consensus)

Note finale = 1 2

DS +

TP1 + TP2 + TP3 3

7

Ch. 1: Modélisation de l’incertitude

Introduction

Représentation de l’erreur

Incertitude d’un capteur

Propagation d’incertitude

Partie 2 Partie 3 Partie 1

Partie 4

8

• 

Les mesures en provenance d’un capteur sont toujours entachées d’une certaine erreur

• 

Il est nécessaire de représenter cette incertitude, de la propager et de la prendre en compte dans le processus en aval (par ex.

la localisation et planification de chemin d’un robot)

Introduction : modéliser l’incertain

9

Modéliser l’incertain : les objectifs

Objectifs

◦  Quantifier un degré de confiance dans une mesure

◦  Propager l’incertitude de chaque mesure sur l’état

◦  Évaluer l’adéquation modèle/mesure tenant compte de l’incertitude et quantifier un degré de confiance dans l’état estimé

◦  Modéliser l’incertain entachant les mesures et les combiner (fusionner)

Exemple: un robot se déplace dans un environnement connu muni d’un capteur de distance. Passe-t-il par une porte ?

Problème: Comment estimer la position du robot à chaque instant ? Avec quelle précision ?

robot porte

10

Exemple : radar de contrôle routier

Vitesse retenue = vitesse mesurée - marge technique Marge technique (écart type sur l’erreur de mesure de vitesse) :

• 

radar fixe: 5 km/h pour des vitesses < 100 km/h et 5% pour des vitesses ≥ 100 km/h

• 

radar utilisé en mouvement: 10 km/h pour des vitesses

< 100 km/h et 10% pour des vitesses ≥ 100 km/h

Les différents types d’erreur

•  Erreurs systématiques (déterministes)

Une erreur systématique est soit constante (biais), soit à variation lente en fonction du temps (dérive)

Décalage entre la valeur vraie et la valeur mesurée

Exemples: erreur sur la valeur d’une grandeur de référence, erreur due à la surchauffe d’un appareil (par ex. une caméra)

•  Erreurs aléatoires (accidentelles)

Exemples: bruit induit par une carte de numérisation, par un microphone, etc.

•  Erreurs aberrantes

Écart énorme entre la vraie valeur et la valeur mesurée Exemple: erreur de mise en correspondance en vision

11

12

Rappel : la méthode des moindres carrés

Modèle théorique: Famille de fonctions d’une ou plusieurs variables muettes , indexées par un ou plusieurs paramètres inconnus

•  La méthode des moindres carrés permet de sélectionner parmi ces fonctions celle qui reproduit (explique) le mieux les données expérimentales

•  La méthode consiste en une prescription, qui est que la fonction qui décrit « le mieux » les données est celle qui minimise la somme quadratique des déviations des mesures aux prédictions de

x

(« least squares method » en anglais)

h ( x ; β )

h ( x ; β )

h ( x ; β ) β

y

13

Rappel : la méthode des moindres carrés

: résidu du modèle ( est l’écart entre la mesure et la prédiction donnée par le modèle)

: mesure de la “distance” entre les données expérimentales et le modèle théorique qui prédit ces données

Si, par exemple, nous disposons de mesures les paramètres « optimaux » au sens de la méthode des

moindres carrés sont ceux qui minimisent la fonction de coût:

n

où

La prescription des moindres carrés commande que cette distance soit minimale, à savoir:

β

S ( β ) d

( β )

h

( x

; β ) d

( β )

β = arg min

β

S ( β ) S ( β ) =

d

( β ) =

( y

− h

( x

; β ))

y

y

, . . . , y

14

Rappel : la méthode des moindres carrés

Si est linéaire, on parle de méthode des moindres carrés ordinaire (ou des moindres carrés linéaires). Le calcul du vecteur de paramètres optimal est direct dans ce cas

Mesures

h ( x ; β )

Exemple (1 variable muette):

β = [ a, b ]

S ( a, b ) =

( y

− ax

− b )

x y

y = ax + b β

h

( x

; a, b ) = ax

+ b

15

Ch. 1: Modélisation de l’incertitude

Introduction

Représentation de l’erreur

Incertitude d’un capteur

Propagation d’incertitude

Partie 2 Partie 3 Partie 1

Partie 4

Représentation de l’erreur

1) Représentation par une distribution de probabilité

• 

Une mesure = réalisation d’une variable aléatoire (v.a.)

• 

En répétant plusieurs fois une mesure, on obtient des résultats plus ou moins différents, dont la répartition est aléatoire

Intérêt: facilité de manipulation (composition, comparaison, transformation de deux densités de probabilité)

Remarque: représentation par zone d’incertitude

Utilisation d’une distribution uniforme

16

U ( a, b )

U ( a, b )

0 a b

La représentation doit permettre de composer et de fusionner facilement les erreurs

1 b − a

x

Incertitude d’un capteur : l’exemple du sonar

Numéro de mesure Valeur

Objet Emetteur/

récepteur

Onde émise

Onde réfléchie

μ μ + 3 σ

μ − 3 σ

17

Représentation de l’erreur

18

Soit une v.a. d’espérance et de variance finie L'inégalité de Bienaymé-Chebyshev s'énonce de la façon suivante. Pour tout réel strictement positif :

X

Cette inégalité montre qu'une v.a. prend avec une grande probabilité une valeur relativement proche de son espérance

σ

> 0 .

μ

(en réalité, seulement le cas de est utile) k > 1 k

On déduit de cette inégalité que:

Pr( |X − μ| ≥ 3 σ ) ≤ 1 9

À savoir, l’intervalle contient au moins 8/9 (88.8%) c’est-à-dire, l’essentiel de la distribution

[ μ − 3 σ, μ + 3 σ ]

Pr( |X − μ| ≥ kσ ) ≤ 1

k

_μ

μ − kσ μ + kσ

Représentation de l’erreur

19

• 

Cette majoration est souvent excessive (trop conservative)

• 

En fait, pour la loi normale de moyenne nulle et d'écart type

unitaire (la gaussienne centrée réduite ou standard: ), l’intervalle contient de la distribution (au lieu

de )

μ − 3σ μ − 2σ μ − σ

μ

μ = 0 , σ = 1

99.7% des données se situent à 3 écarts types de la moyenne 95% des données à 2 écarts

types de la moyenne 68% des

données

[ − 1 . 65 , 1 . 65]

100(1 − 1 / 1 . 65

) = 63 . 2%

90%

2) Représentation par des régions

20

• 

En statistique, une v.a. vectorielle X est représentée par une distribution de probabilité. Au contraire, dans une approche

ensembliste, X est représentée par un ensemble S dans lequel

de densité de probabilité (pdf) de X est inclus à l’intérieur de S )

• 

Définition d’une zone où la vraie grandeur doit se trouver

S

Illustration 1D:

pdf

Rappr. probabiliste Rappr. ensembliste

X

X

Modélisation et représentation de l’erreur

Propagation de contraintes sur les régions: uniquement des opérations (intersection, union, etc.) sur des

ensembles, polytopes en général, mais aussi disques

ou ellipses

21

"Applied interval analysis: with examples in parameter and state estimation, robust control and robotics", L. Jaulin, M. Kieffer, O. Didrit, E. Walter, vol. 1, Springer, 2001

•  Si on représente une v.a. vectorielle par un ensemble, on a besoin de moins d’assumptions sur la v.a. (par ex. l’hypothèse d’indépendance des variables) et on peut traiter les non linéarités plus facilement

•  Cependant:

1)  Une pdf fournit des informations plus riches que l’ensemble qui enferme son support

2)  Par composition, la région peut devenir très complexe et/ou ne plus appartenir à la famille de représentation choisie

(calculs onéreux)

3)  La représentation par des régions peut être assez conservatrice

Avantages et inconvénients:

?!

Modélisation et représentation de l’erreur

2) Représentation par des régions

22

Ch. 1: Modélisation de l’incertitude

Introduction

Représentation de l’erreur

Incertitude d’un capteur

Propagation d’incertitude

Partie 2 Partie 3 Partie 1

Partie 4

23

Combiner des mesures incertaines

Par ex. en robotique mobile:

◦  Plusieurs mesures

  Différents capteurs

  Différents instants de temps

23

Quelle incertitude sur connaissant

Propagation d’incertitude : introduction

Système

Y

= f

( X

, . . . , X

)

f

X

.. . X

.. . X

Y

.. . Y

.. . Y

Y

X

24

• 

Soit une v.a. continue et la valeur spécifique que admette

24

Propagation d’incertitude

• 

Soit une fonctionne déterministe (connue)

X

Problème: Comment calculer (ou approcher), l’espérance et la variance de , à savoir

E [ f ( X )] , ?

Si admet une fonction de densité de probabilité (pdf) X , c’est direct:

p ( x )

E [ f ( X )] = x =

−∞

f ( x ) p ( x ) dx var[ f ( X )] =

−∞

( f ( x ) − x )

p ( x ) dx f

Var[ f ( X )]

Var[ f ( X )]

f ( X ) : R → R

X x

25 25

De la même façon, soient:

Si chaque v.a. de admet une pdf , alors: p ( x )

: vecteur de variables aléatoires (vecteur aléatoire) n

: fonction déterministe de plusieurs variables

X

E [ f ( X )] =

⎡

⎢ ⎢

⎢ ⎣

E [ f

( X )]

E [ f

( X )]

.. .

E [ f

( X )]

⎤

⎥ ⎥

⎥ ⎦

(matrice de covariance)

Var[ f ( X )] = E [( f ( X ) − E [ f ( X )])( f ( X ) − E [ f ( X )])

] f ( X ) : R

→ R

Propagation d’incertitude

X

26 26

Var[ f ( X )] = E [( f ( X ) − E [ f ( X )])( f ( X ) − E [ f ( X )])

]

• 

La matrice de covariance est symétrique: ses éléments

diagonaux sont les variances et ses éléments extra-diagonaux sont les covariances des couples de variables

• 

La matrice de covariance est semi-définie positive (ses valeurs propres sont positives ou nulles)

=

⎡

⎢ ⎢

⎢ ⎢

⎢ ⎣

Var[ f

] Cov[ f

, f

] · · · Cov[ f

, f

] Cov[ f

, f

] Var[ f

] · · · .. .

.. . .. . . .. .. .

Cov[ f

, f

] · · · · · · Var[ f

]

⎤

⎥ ⎥

⎥ ⎥

⎥ ⎦

Propagation d’incertitude

27 27

•  Ces valeurs peuvent toujours être calculées par intégration numérique ...

... mais cela pose un problème si on veut faire des développements formels !

Par ex. si la v.a. dépend d’un paramètre , trouver tel que soit minimale ou maximale X a a

Il y a donc un intérêt à disposer de formules, même approchées, pour la propagation de l’incertitude

Var[ X ( a )]

Propagation d’incertitude

28 28

Dans le cas linéaire:

Propagation d’incertitude : cas linéaire

Y = f ( X ) = AX

: vecteur colonne de variables aléatoires n X

, . . . , X

A ∈ R

: matrice de scalaires m × n

D’après la linéarité de l’espérance:

E [ Y ] = E [ AX ] = AE [ X ] ∈ R

Var[ Y ] = Var[ AX ] = E [ A ( X − E [ X ])( X − E [ X ])

A

]

= A Var[ X ] A

∈ R

X

29 29

Dans le cas non linéaire:

En l’absence d’autres informations, on approche la fonctionne avec l’application linéaire tangente (en d’autres termes,

on utilise une approximation de Taylor de au 1

ordre)

Y = f ( X )

X

= E [ X ]

Soit

f

: vecteur colonne de variables aléatoires n ^X

, . . . , X

X

Propagation d’incertitude : cas non linéaire

f ( X )

30 30

Au premier ordre:

avec matrice jacobienne:

Y = f ( X ) f ( X

) + J

( X

)( X − X

)

Alors:

E [ Y ] E [ f ( X

) + J

( X

)( X − X

)]

= f ( X

) + J

( X

)( E [ X ] − X

)]

= f ( X

)

Propagation d’incertitude : cas non linéaire

J

( X

) = ∂ f

∂ X ( X

) =

∂ f

∂X

( X

) , . . . , ∂ f

∂X

( X

)

∈ R

31 31

En outre:

cov[ Y ] = E [( f ( X ) − E [ f ( X )])( f ( X ) − E [ f ( X )])

] E [( f ( X ) − f ( X

))( f ( X ) − f ( X

))

]

E [( J

( X

)( X − X

))( J

( X

)( X − X

))

] E [ J

( X

)( X − X

)( X − X

)

J

( X

)]

= J

( X

) cov[ X ] J

( X

)

Remarque:

Cas linéaire:

Cas non linéaire:

Var[ Y ]

J

( X

)Var[ X ] J

( X

)

Var[ AX ] = A Var[ X ] A

Var[ f ( X )] J

( X

)Var[ X ] J

( X

) Propagation d’incertitude : cas non linéaire

=

32 32

Sous l’hypothèse de v.a. indépendantes, la matrice de covariance « d’entrée » devient:

Rappel: Si deux v.a. and sont indépendantes:

Attention: la réciproque ne est pas toujours vraie

X Y

Cov[ X, Y ] = E [ X Y ] − E [ X ] E [ Y ]

= E [ X ] E [ Y ] − E [ X ] E [ Y ] = 0 C

= diag( σ

, σ

, . . . , σ

) =

⎡

⎢ ⎢

⎢ ⎢

⎣

σ

0 · · · 0 0 σ

· · · .. .

.. . .. . . .. .. . 0 · · · · · · σ

⎤

⎥ ⎥

⎥ ⎥

⎦

Var[ X ]

Propagation d’incertitude : en résumé

33 33

•  Approximation de Taylor au 1

ordre de

Nous avons la matrice jacobienne suivante:

•  Loi de propagation d’incertitude:

F _X =

⎡

⎢ ⎢

⎣

∂ f

∂X

∂ f

∂X

. . . _∂X ^{∂ f}

.. . .. . . .. .. .

∂ f

∂X

∂ f

∂X

· · · ^{∂ f} _∂X

⎤

⎥ ⎥

⎦

C Y = F X C X F ^T _X

∈ R ^m×n

( )

Propagation d’incertitude : en résumé

f ( X )

34 34

Exemple (1/3)

Soit un vecteur gaussien de moyenne et de covariance

Soit la v.a. scalaire

Définition: Soit un vecteur aléatoire. est un vector gaussien, si et seulement si, pour toute suite de nombres réels, la v.a.

est une v.a. gaussienne

X = [ X, Y ]

[0 , 0]

X = [ X

, . . . , X

]

X

a

, a

, . . . , a

Z = a

X

+ a

X

+ . . . + a

X

X

= f ( X, Y ) = X

+ 3 X − 2 Y + 5

Objectif: Comparer les valeurs exactes et les valeurs approchées de la variance de X

σ

1 0 0 4

35 35

Exemple (2/3)

Nous avons la pdf suivante ( ):

En intégrant numériquement avec Maple (ou Matlab), nous obtenons:

p ( X ) = 1

(2 π )

det( Σ ) exp( − 1

2 ( X − μ )

Σ

( X − μ ))

= 1

4 πσ

exp

− 1

2 σ

( X

+ 1

4 Y

)

E [ X

] = 5 + σ

Var[ X

] = 25 σ

+ 2 σ

p ( X ) = N ( μ, Σ )

μ = [0, 0]^T Σ = σ²

1 0

0 4

36 36

Exemple (3/3)

Par contre, en utilisant les formules d’approximation:

Donc, au point X

= [0 , 0]

:

J

( X

) = [3 , − 2]

D’où E [ X

] f ( X

) = 5

Conclusion: Si est petite, est négligeable devant et l’approximation fournie de la variance de est précise σ σ

σ

Var[ X

] J

( X

)Var[ X ] J

( X

)

= σ

[3 , − 2]

1 0

0 4

3

− 2

= 25 σ

J

( X ) =

∂ f ( X, Y )

∂ X , ∂ f ( X, Y )

∂ Y

= [2 X + 3 , − 2]

X

37 37

Calcul des ellipsoïdes d’incertitude

La matrice est une matrice carrée symétrique et semi- définie positive: elle peut être représentée par un ellipsoïde d’incertitude

C

C _Y = F _X C _X F ^T _X

Nous avons que (décomposition en valeurs/vecteurs propres):

C _Y U = UΛ

Λ = diag( λ

, λ

, . . . , λ

) , U = [ u

u

· · · u

]

où

u _i

: valeurs propres de la matrice triées par ordre décroissant C

: vecteur propre associé à la valeur propre λ

, i ∈ { 1 , 2 , . . . , m}

λ

≥ λ

≥ · · · ≥ λ

38 38

λ

C

•  Les deux premières coordonnées et du vecteur propre associé à la valeur propre la plus forte de nous permettent de calculer l’orientation de l’ellipse

d’incertitude:

u

u

u

θ

= arctan

u

u

•  La longeur des demi-axes de l’ellipse est définie par la racine carrée des deux valeurs propres de les plus fortes, c’est-à-dire et

On peut interprétéer et comme des variances.

Les demi-axes de l’ellipse auront donc pour longueurs:

C

λ

λ

λ

λ

k

λ

, k

λ

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

θ

Par exemple, si on veut représenter l’incertitude de

localisation d’un robot en deux dimensions (plan du sol)

mais à 3 degrés de liberté ( ), par une ellipse

d’incertitude autour de la position estimée: C

∈ R

39

Graphiquement:

Ellipse d’incertitude

Rappel que si :

θ

θ

= arctan

u

u

Pr( μ − σ ≤ X ≤ μ + σ ) 0 . 68269 Pr( μ − 2 σ ≤ X ≤ μ + 2 σ ) 0 . 95450 Pr( μ − 3 σ ≤ X ≤ μ + 3 σ ) 0 . 99730 Pr( μ − 4 σ ≤ X ≤ μ + 4 σ ) 0 . 99993 Pr( μ − 5 σ ≤ X ≤ μ + 5 σ ) 0 . 99999 k

λ

k λ

kσ

X ∼ N ( μ, σ

) x

y

Position estimée du robot

40

Exemple : localisation d’un robot

Point de départ du robot

x [mm]

y [mm] Ellipses

d’incertitude

Propagation d’incertitude:

exemple

Ajustement de droites

• 

Données en provenance de scanners lasers ou sonars

• 

Localisation et cartographie

◦  Connaissance d’un plan

  Comparer l’ensemble des points à la carte de

référence est très couteux   Comparer quelques droites

à la carte est très rapide

◦  Construction de plan

  Points seuls: énormément de données à stocker

  Droites ajustées: beaucoup moins de données !

-0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.5

0.4 0.3 0.2 0.1

0

x [m]

y [m]

robot mur

mur

robot

droite

Extraction de droite

43

Cas réel: plusieurs droites !

Algorithmes existants

  Transformée de Hough

  Espérance-Maximisation (EM)   RANSAC

  Split-and-merge   Moindres carrés   Incrémental

x [m]

x [m]

y [m]

y [m]

Données brutes 6 droites extraites

robot robot laser

Extraction de droite

Objectif: extraire une droite d’un ensemble de mesures de points incertains en distance et en orientation

Les paramètres et suffisent à représenter une unique droite en coordonnées polaires : r α

( ρ, θ )

ρ (cos θ cos α + sin θ sin α ) − r = ρ cos( θ − α ) − r = 0

α

r

droite

44

mesure

robot

i

Extraction de droite

45

Problème: quelle est l’incertitude de la droite extraite en connaissant les incertitudes de mesure des points qui ont permis de l’obtenir ?

1.  Établir une transformation mesures droite 2.   Appliquer la loi de propagation d’incertitude

−→

α

r

droite mesure

robot

i

Mesures et incertitudes

46

Un scan laser donne points de mesure:

On les modélise comme deux variables aléatoires:

x

= ( ρ

, θ

) , i ∈ { 1 , 2 , . . . , n}

n

X

= ( P

, Q

)

α

r _{x i = ( ρ i , θ i )}

ρ _i

θ

Mesures et incertitudes

47

2) Chaque point est bruité selon une loi gaussienne sur les deux coordonnées: X

= ( P

, Q

)

P _i ∼ N ( ρ _i , σ _ρ ²

) Q i ∼ N ( θ i , σ _θ ²

)

1) Les v.a. et sont indépendantes, c’est-à-dire:

E [ P

P

] = E [ P

] E [ P

] , ∀ i, j ∈ { 1 , 2 , . . . , n}, i = j E [ Q

Q

] = E [ Q

] E [ Q

] , ∀ i, j ∈ { 1 , 2 , . . . , n}, i = j E [ P

Q

] = E [ P

] E [ Q

] , ∀ i, j ∈ { 1 , 2 , . . . , n}

P

Q

Dans ce qui suit, on fera les hypothèses suivantes:

Transformation des mesures en droites

48

•  On cherche une droite passant par les points de mesure et qui a pour équation:

ρ cos( θ − α ) − r = 0

•  Pour chaque point de mesure, l’erreur par rapport à la droite est donnée par:

α

r

x

= ( ρ

, θ

)

Interprétation géométrique

d

n

ρ

cos( θ

− α ) − r = d

, i ∈ { 1 , . . . , n}

Transformation des mesures en droites

49

•  Ajustement par la méthode des moindres carrés:

: résidu du modèle

: fonction de coût à minimiser

d

S ( r, α )

α

r

x

= ( ρ

, θ

) d

S ( r, α ) =

i=1

d

( r, α ) =

i=1

( ρ

cos( θ

− α ) − r )

Transformation des mesures en droites

50

Pour minimiser , on calcule les dérivées partielles par rapport à et et on les met à zéro: S ( r, α )

r α

Si on résout le système de deux équations par rapport à et

on obtient la solution suivante r α

∂ S ( r, α )

∂ r = 0 , ∂ S ( r, α )

∂ α = 0

S ( r, α ) =

i=1

d

( r, α ) =

i=1

( ρ

cos( θ

− α ) − r )

Transformation des mesures en droites

51

Solution au sens des moindres carrés:

r = 1 n

i=1

ρ

cos( θ

− α )

α = 1

2 arctan

⎛

⎜ ⎜

⎜ ⎜

⎝

ρ

sin(2 θ

) − 2 n

ρ

ρ

cos θ

sin θ

i=1

ρ

cos(2 θ

) − 1 n

ρ

ρ

cos( θ

+ θ

)

⎞

⎟ ⎟

⎟ ⎟

⎠

θ

ρ

Remarque: dans la

première formule on préfère normalement utiliser l’ atan2

r

α

x

Transformation des mesures en droites

52

•  Si on dispose d'une estimation de la variance du bruit qui entache chaque mesure, on peut l'utiliser pour « pondérer » la contribution de la mesure dans la fonction de coût

•  Une mesure aura d'autant plus de poids que son incertitude sera faible. Cela nous amène à l’ajustement par la méthode des moindres carrés pondérés:

Remarque:

Le poids est l’inverse de la variance du bruit affectant la mesure . Par exemple, un choix possible dans est:

S ( r, α )

w

i

S w ( r, α ) =

n

i=1

w i ( ρ i cos( θ i − α ) − r ) ²

w

= 1 /σ

, i ∈ { 1 , . . . , n}

Transformation des mesures en droites

53

Solution au sens des moindres carrés pondérés (cette fois-ci on calcule ):

α = 1

2 arctan

⎛

⎜ ⎜

⎜ ⎜

⎝

w

ρ

sin(2 θ

) − 2

i=1

w

w

w

ρ

ρ

cos θ

sin θ

i=1

w

ρ

cos(2 θ

) − 1

i=1

w

w

w

ρ

ρ

cos( θ

+ θ

)

⎞

⎟ ⎟

⎟ ⎟

⎠

min

S

( r, α )

r = 1

i=1

w

w

ρ

cos( θ

− α )

Remarque:

Si , nous retrouvons la solution aux moindres

carrés (non pondérés) w

= . . . = w

= 1

Propagation d’incertitude

54

•  Matrice de covariance « de sortie » à déterminer:

où et sont les v.a. associées à et

Objectif: comprendre comment les incertitudes sur les mesures de distance et d’orientation se propagent et affectent l’incertitude sur la droite à estimer

En d’autres termes, comment l’incertitude sur et se propage dans les deux équations précédentes et

affecte et ?

ρ

θ

r α

R A

C

=

σ

σ

σ

σ

∈ R

α

r

Incertitude/propagation de l’erreur

55

•  Matrice de covariance d’entrée (diagonale):

C

=

C

0

0

C

=

diag( σ

, . . . , σ

) 0

0

diag( σ

, . . . , σ

)

∈ R

•  Propagation d’incertitude:

C _RA = F _X C _X F ^T _X

Incertitude/propagation de l’erreur

56

où la matrice jacobienne :

F

=

⎡

⎢ ⎢

⎢ ⎣

∂ r

∂ P

∂ r

∂ P

· · · ∂ r

∂ P

∂ r

∂ Q

∂ r

∂ Q

· · · ∂ r

∂ Q

∂ α

∂ P

∂ α

∂ P

· · · ∂ α

∂ P

∂ α

∂ Q

∂ α

∂ Q

· · · ∂ α

∂ Q

⎤

⎥ ⎥

⎥ ⎦ F

= F

∈ R

• 

Propagation d’incertitude

C _RA = F _X C _X F ^T _X

et r = ρ cos( θ − α ) , α = θ − arccos( r/ρ )

Exemple [Livre de Siegwart, Sect. 4.7.1.1]

57

17 mesures prises par un télémètre laser 2D embarqué sur un robot mobile (voir le tableau dans la diapo suivante)

Nos hypothèses:

1.  Normalement, on considère que l’incertitude de mesure soit proportionnelle à la distance mesurée, mais pour plus de

simplicité on fera l’hypothèse que l’incertitude associée à chaque mesure soit la même

2.  Les mesures sont non corrélées

3.  Le robot n’est pas en mouvement lorsque il prend une mesure

58

Angle d'inclinaison du capteur [degrés] Distance [m]

0 0.5197 5 0.4404 10 0.4850 15 0.4222 20 0.4132 25 0.4371 30 0.3912 35 0.3949 40 0.3919 45 0.4276 50 0.4075 55 0.3956 60 0.4053 65 0.4752 70 0.5032 75 0.5273 80 0.4879

Mesures réalisées

i

θ

ρ

59

Si on calcule la solution aux moindres carrés, nous trouvons la droite optimale définie par (en rouge dans la figure):

Exemple

r = 0 . 4 m , α = 37 . 36

r

α

[m]

y

Mesure avec son ellipse d’incertitude

60

Représentation de l’incertitude par des ellipses dans

l’espace du robot et dans l’espace des droites ( r, α )

Espace des droites

α [rad]

r [m]

Espace du robot

Exemple

x y

( x, y )

[m]

[m]