TP 1 – Propagation d'incertitude et fusion de mesures

Université de Picardie Jules Verne A.U. 2019-2020 Master 2 3EA Surveillance Distribuée de Systèmes Multi-agents

F. Morbidi Page 1/4

TP 1 – Propagation d'incertitude et fusion de mesures

Consignes pour le TP:

• Durée: 3h30 à partir de 9h00.

• Envoyez le compte rendu et les programmes Matlab réalisés (nom du fichier:

noms_du_binôme.zip) avant 12h30 à l'adresse e-mail : fabio.morbidi@u-picardie.fr Le but de ce TP est de développer des fonctions Matlab qui permettent d'effecteur la propagation d'incertitude et de fusionner de façon optimale les mesures d'un ou plusieurs capteurs.

Exercice 1 : Propagation d'incertitude

Écrire une fonction Matlab, dénotée PropInc(.), qui prend en entrée le vecteur espérance E [X] et la matrice de covariance Var[X] du vecteur de variable aléatoires X et qui renvoie le vecteur espérance et la matrice de covariance (approchés) de la fonction f(X): ! ⁿ → ! ^m . Considérer séparément les deux cas suivants:

1. La fonction f(X) est linéaire, c'est-à-dire f(X) = AX où A est une matrice constante.

2. La fonction f(X) est non linéaire.

Tester la fonction PropInc(.)développée, dans les trois cas suivants:

a. f(X) = 3X, E [X] = 0 et Var[X] = 1,

b. f(X) = X, E [X] = [1, 1] ^T et Var[X] = ,

c. f(X) = f(X,Y,Z) = [XY, YZ, XZ] ^T , E [X] = [0, 0, 1] ^T et Var[X] = 5I

où I

dénote la matrice identité 3 × 3.

Exercice 2 : Fusion de mesures

1. Un capteur laser monté sur un trépied a réalisé n mesures z 1 , z 2 ,…, z n de distance par rapport à un mur. Les mesures sont non corrélées et suivent le modèle linéaire suivant:

z i = x + r avec r ~ N (0, σ _i ² ), i ∈ { 1, 2, …, n}.

Écrire une fonction Matlab, dénotée FusMes(.), qui prend en entrée les n mesures et

leurs variances, et qui renvoie leur combinaison linéaire optimale (c'est-à-dire, la combinaison à variance minimale).

2. Un radar embarqué sur une voiture a réalisé n mesures de distance z(t 1 ), z(t 2 ), …, z(t n )

par rapport à un obstacle au fil du temps (0 = t 1 < t 2 < … < t n ). Les mesures sont non

corrélées et ils ont une variance,

Université de Picardie Jules Verne A.U. 2019-2020 Master 2 3EA Surveillance Distribuée de Systèmes Multi-agents

F. Morbidi Page 2/4

Var[ z (t _i )] = σ ² z( t i) , i ∈ { 1, 2, …,

n }.

Les mesures suivent le modèle linéaire décrit au point précédent.

a. Écrire une fonction Matlab, dénotée MoyRec(.), qui prend en entrée les n mesures et leurs variances, et qui calcule l'estimé optimal de la distance à l'instant t n , ainsi que la variance associée.

b. Tester la fonction MoyRec(.)avec les mesures fournies dans le tableau ci-après:

Temps Mesure de distance [m] Variance de la mesure [m]

t 1 27 0.5

t 2 28 0.6

t 3 26.5 0.45

t 4 27 0.5

t 5 28.5 0.65

Exercice 3 : Fusion de mesures par filtre de Kalman

On considère un robot à deux roues encodeuses (voir la Figure 1, ci-après). Le robot se déplace pas-à-pas dans le sens positif de l'axe x. Le robot est aussi muni d’un sonar qui fournit des mesures de distance en mètres par rapport à un mur. On souhaite combiner les mesures des roues encodeuses (odométrie) et du sonar pour connaître la position précise du robot. Ces mesures sont entachées d'un bruit blanc gaussien à moyenne zéro, avec variances σ

odom et σ

sonar, respectivement. Écrire un programme Matlab qui met en œuvre les équations du filtre de Kalman pour estimer la position du robot au fil du temps.

Considérer les deux cas suivants et comparer les résultats obtenus à l'aide de tracés Matlab:

a) σ ^odom = 0.01 m, σ ^sonar = 0.1 m, b) σ ^odom = 0.01 m, σ ^sonar = 0.01 m.

L'abscisse du robot à l'instant t = 0 s est zéro.

Figure 1 : Robot à deux roues encodeuses avec sonar embarqué.

Université de Picardie Jules Verne A.U. 2019-2020 Master 2 3EA Surveillance Distribuée de Systèmes Multi-agents

F. Morbidi Page 3/4

Exercice 4 : Conception d'un filtre Alpha-Beta-Gamma

Dans de nombreuses applications (robotique mobile, voitures autonomes, etc.), le mouvement d'un véhicule doit être décrit à l'aide d'un simple modèle cinématique.

Un choix possible est le modèle DWPA (Discrete Wiener Process Acceleration ou "Processus de Wiener à accélération constante par morceaux"). Ce modèle à temps discret du troisième ordre prend la forme suivante:

où x(k) = [p(k), u(k), a(k)] ^T est le vecteur d'état à l'instant k (c'est-à-dire, la position, la vitesse et l'accélération du véhicule le long d'un axe générique, par exemple l'axe x).

En outre, la matrice de transition et le gain du bruit ont la forme suivante,

Dans ce modèle, le bruit blanc gaussien w(k) représente la variation de l'accélération du véhicule pendant le k-ième intervalle d'échantillonnage (de durée T). On fera l'hypothèse qu'il s'agit d'une séquence blanche à moyenne zéro (en d'autres termes, l'accélération est un processus de Wiener à temps discret), et que:

La covariance du bruit de modèle multipliée par le gain Γ a la forme suivante:

Il est à noter que la dimension physique de w et σ _w est m/s

, c'est-à-dire un "jerk" ou un

"à-coup" (la dérivée de l'accélération par rapport au temps, soit la dérivée troisième de la position par rapport au temps). L'ordre de grandeur de σ

doit être celui de la variation d'accélération maximale dans un intervalle d'échantillonage, Δ a

. Un éventail pratique de valeurs est Δ a

/2 ≤ σ

≤ Δ a

. Pour conclure, on fera l'hypothèse que uniquement des mesures de position sont disponibles à l'instant k, c'est-à-dire,

où H = [1, 0, 0] et le bruit de mesure à moyenne zéro v(k), est tel que:

Université de Picardie Jules Verne A.U. 2019-2020 Master 2 3EA Surveillance Distribuée de Systèmes Multi-agents

F. Morbidi Page 4/4

Écrire un programme Matlab qui met en œuvre les équations du filtre de Kalman pour estimer l'état courant x(k) du véhicule à partir des mesures de position. En sachant que Δa

= 10 ^{– 2} m/s

et que σ v = 0.01 m, tester le filtre de Kalman pour un durée T f = 100 s avec un temps d'échantillonnage T = 0.1 s.

Il est intéressant de noter qu'il est possible de calculer une expression explicite pour la matrice de covariance de l'erreur d'estimation et pour le gain du filtre à régime permanent.

Le filtre à régime permanent pour le modèle DWPA est connu en littérature sous le nom de filtre α - β - γ (Alpha-Beta-Gamma). Pour plus de détails, voir la Section 6.5 du livre:

"Estimation with Applications to Tracking and Navigation", Y. Bar-Shalom, X.-R. Li,

T. Kirubarajan, John Wiley & Sons, 2001.