Un algorithme EM récursif pour le SLAM

Un algorithme EM r´ ecursif pour le SLAM
Sylvain LECORFF,Gersende FORT ,Eric MOULINES
LTCI, TELECOM ParisTech & CNRS
46, rue Barrault, 75634 Paris Cedex 13, France
pr´ [email protected]
R´ esum´ e – Nous proposons un algorithme de type Expectation Maximisation (EM) r´ ecursif pour l’inf´ erence dans les mod` eles de Chaˆ ınes de
Markov cach´ ees (HMM) ` a espace d’´ etat g´ en´ eral. Cet algorithme permet de traiter les situations o` u les observations arrivent s´ equentiellement et
o` u le mod` ele complet n’est pas de type exponentiel. Nous appliquons cet algorithme au probl` eme de Localisation et Cartographie Simultan´ ees
(SLAM) lorsque les mod` eles d’´ etat et d’observations sont non lin´ eaires et non gaussiens. Pour ce faire, nous mod´ elisons la carte comme un pa-
ram` etre du HMM. L’originalit´ e de cette contribution ` a la r´ esolution du SLAM r´ eside ` a la fois (i) dans la formulation du probl` eme de cartographie
comme un probl` eme d’estimation de param` etres dans un HMM; et (ii) dans l’approche algorithmique retenue, bas´ ee sur l’algorithme EM.
Abstract – We propose a new online Expectation Maximization(EM)-based algorithm for inference in general state space Hidden Markov
models (HMM). This algorithm applies when observations are sequential and when the complete likelihood is not in the exponential distribution
class. This algorithm is applied to the Simultaneous Localisation and Mapping (SLAM) problem when the state and observation models are non
linear and non gaussian. To that goal, the map is seen as a parameter of the HMM. The novelty of our approach for the SLAM problem is (i) in
the expression of the mapping problem as an inference problem in HMM and (ii) the algorithm used for solving inference, based on EM.
1 Introduction
L’algorithme EM [1] est un algorithme tr` es r´ epandu pour la
r´ esolution de probl` emes inverses dans des mod` eles ` a donn´ ees
cach´ ees. Cet algorithme a tout d’abord ´ et´ e propos´ e pour l’es-
timation “hors ligne” i.e. dans le cas o` u toutes les observa-
tions sont disponibles avant de lancer la proc´ edure. N´ eanmoins,
l’int´ erˆ et pour des proc´ edures de r´ esolution de probl` emes in-
verses “en ligne” est croissant du fait par exemple de la taille
importante des donn´ ees qui empˆ eche leur stockage et/ou de la
n´ ecessit´ e d’une r´ eponse en temps r´ eel. Ainsi, des proc´ edures
de type EM r´ ecursif i.e. des algorithmes de type EM capables
d’int´ egrer les nouvelles observations au fur et ` a mesure de leur
disponibilit´ e, ont ´ et´ e r´ ecemment propos´ ees (par [2] dans le cas
d’observations i.i.d. ; par [3, 4] pour les HMM ` a espace d’´ etat
fini ; par [5] pour les mod` eles de Markov cach´ es g´ en´ eraux,
lorsque la vraisemblance compl` ete est une distribution expo-
nentielle).
Nous proposons une autre extension de ces m´ ethodes au cas
des HMM ` a espace d’´ etat g´ en´ eral qui permet aussi de traiter les
HMM dont la vraisemblance compl` ete n’est pas exponentielle.
Dans ce contexte de HMM non lin´ eaire, non n´ ecessairement
gaussien et ` a espace d’´ etat g´ en´ eral, la principale difficult´ e est
de d´ efinir l’´ etape E de l’algorithme EM r´ ecursif. Pour ce faire,
nous combinons (i) une approximation (locale) de la loi jointe
par une distribution dans la famille exponentielle ; et (ii) un
algorithme de calcul r´ ecursif de l’esp´ erance de la statistique
suffisante sous la loi a posteriori, m´ ethode adapt´ ee de [6].
Ces extensions sont motiv´ ees par la probl´ ematique du SLAM :
un robot se d´ eplace et observe des amers ; la carte (i.e. la po-
sition des amers et leurs nombres) ainsi que la trajectoire du
robot sont inconnues et doivent ˆ etre reconstruites ` a l’aide des
observations. Dans les approches classiques, le probl` eme du
SLAM est mod´ elis´ e via un HMM tel que la carte et la position
du robot sont int´ egr´ ees ` a l’´ etat cach´ e. Les premi` eres r´ eponses
ont ´ et´ e apport´ ees pour des HMM lin´ eaires gaussiens (via le
filtre de Kalman ; puis l’algorithme de Kalman ´ etendu pour
l’approximation de HMM par un mod` ele lin´ eaire gaussien).
N´ eanmoins ces algorithmes souffrent d’inconsistance notam-
ment du fait de l’existence d’une composante fixe (la carte)
dans l’´ etat cach´ e [7, 8]. R´ ecemment, [9] a propos´ e de voir la
carte comme un param` etre du HMM, l’´ etat cach´ e se r´ eduisant
`
a la pose du robot. La carte est alors estim´ ee au sens du maxi-
mum de vraisemblance r´ ecursif r´ esolu en pratique par un al-
gorithme de gradient stochastique, et la distribution de la pose
est approch´ ee par une m´ ethode particulaire. Nous nous plac¸ons
dans le mˆ eme mod` ele que [9] et r´ esolvons le probl` eme inverse
en appliquant l’algorithme EM r´ ecursif pour les HMM ` a es-
pace d’´ etat g´ en´ eral. Nous obtenons une estimation ponctuelle
de la carte - via un algorithme it´ eratif -, et une approximation
particulaire de la distribution a posteriori de la pose.
En section 2, nous pr´ esentons les algorithmes EM en ligne
pour les HMM puis exposons notre nouvel algorithme (sec-
tion 2.3). Nous consid´ erons l’application au SLAM en sec-
tion 3.

2 Algorithme de type EM r´ ecursif pour
l’inf´ erence dans les HMM
´
Etant donn´ ees des observations Y1:T= (Y1,· · · ,YT), on
ajuste un mod` ele de Chaˆ ınes de Markov cach´ ees d´ ependant
d’un param` etre θ∈Θ⊆R`, en d´ eterminant le meilleur mod` ele
dans cette famille au sens du maximum de vraisemblance. Les
mod` eles HMM sont d´ efinis par la donn´ ee (a) d’une loi initiale
de l’´ etat cach´ e, que l’on va consid´ erer ind´ ependante de θpour
simplifier l’expos´ e, (b) d’un noyau de transition mθ(x, x0)dλ(x0)
de l’´ etat cach´ e et, (c) d’une densit´ e conditionnelle de l’observa-
tion ´ etant donn´ e l’´ etat cach´ egθ(x, y)dν(y). Notons {(Xt, Yt)}t≥1
un HMM d´ efini par ces noyaux de transition, ` a valeurs dans des
espaces g´ en´ eraux X×Y.
2.1 EM hors ligne - Cas des HMM exponentiels
Les HMM sont des mod` eles ` a donn´ ees latentes et pour ces
mod` eles, l’algorithme EM permet le calcul du maximum de
vraisemblance ? Il consiste ` a d´ efinir de fac¸on it´ erative une suite
{θn}n≥0, qui converge vers un point stationnaire de la vraisem-
blance. Chaque it´ eration comprend deux ´ etapes :
´
Etape E Calcul de
QT(θ, θn)def =Eθn"T
X
t=1
log{mθ(Xt−1, Xt)gθ(Xt,Yt)}Y1:T#
o` uEθ[·|Y1:T]d´ esigne l’esp´ erance sous la loi conditionnelle des
donn´ ees manquantes X1:Tsachant Y1:Tpour la valeur θdu
param` etre.
´
Etape M Mise ` a jour de l’estim´ ee courante :
θn+1 ∈argmaxθ∈ΘQT(θ, θn).
Except´ e dans des mod` eles HMM simples, la quantit´ eQT(·, θn)
n’est pas calculable explicitement car la loi conditionnelle φθ
1:T|T
de X1:Tsachant Y1:Tpour la valeur θdu param` etre (i.e. la loi
de lissage) n’est pas connue explicitement. N´ eanmoins, du fait
du caract` ere markovien et donc r´ ecursif de la dynamique de
construction du HMM, il existe une relation de r´ ecurrence liant
la distribution φθ
1:T−1|T−1`
aφθ
1:T|T, relation que l’on peut ex-
ploiter pour construire une approximation particulaire de la loi
d’int´ erˆ et φθ
1:T|T(voir e.g. [10]) ; dans ce cas, l’´
etape E est ap-
proch´ ee par une m´ ethode de Monte Carlo S´ equentielle.
L’´ etape M n´ ecessite de calculer la quantit´ e interm´ ediaire θ7→
QT(θ, θn)pour toutes les valeurs de θ, c’est-` a-dire calculer une
infinit´ e d’esp´ erances sous la loi conditionnelle ´ etant donn´ ee
la valeur courante du param` etre. Dans les applications r´ eelles,
cela est impossible ` a mettre en oeuvre sauf dans le cas o` u la
vraisemblance compl` ete est une distribution exponentielle : il
existe des fonctions ψ: Θ →Rdet S:X×X×Y→Rd
telles que
log{mθ(x, x0)gθ(x0, y)}=hS(x, x0, y), ψ(θ)i,
o` uh·,·i d´ esigne le produit scalaire. Dans ce cas, l’´ etape E consiste
`
a calculer la quantit´ e
¯
S1:T(θn,Y1:T)def =Eθn"T
X
t=1
S(Xt−1, Xt,Yt)Y1:T#,
et l’´ etape M devient θn+1 ∈argmaxθQT(θ, θn)o` u
QT(θ, θn) = h¯ S1:T(θn,Y1:T), ψ(θ)i.
Nous qualifierons ces mod` eles de HMM exponentiels.
2.2 Algorithme EM en ligne pour des HMM ex-
ponentiels
Pour r´ esoudre un probl` eme d’estimation par maximum de
vraisemblance r´ ecursif, il faut introduire des proc´ edures “en li-
gne” qui ne n´ ecessitent ni le stockage des observations, ni l’uti-
lisation des observations sur plusieurs it´ erations successives.
Ces m´ ethodes doivent ˆ etre capables d’int´ egrer la nouvelle in-
formation disponible YT+1 aux quantit´ es disponibles ` a l’ins-
tant T. Ainsi, l’algorithme EM en ligne consiste ` a d´ efinir une
suite {θT}T≥0d´ efinie par
θT+1 ∈argmaxθ∈ΘQT+1(θ, θT).
Cette m´ ethode n’est r´ ecursive que si le calcul de QT+1(θ, θT)
est r´ ecursif. Observons que dans les HMM exponentiels,
¯
S1:T+1(θT,Y1:T+1 )
=EθT"EθT"T
X
t=1
S(Xt−1, Xt,Yt)Y1:T, XT+1#Y1:T+1 #
+EθThS(XT, XT+1,YT+1)Y1:T+1i(1)
de sorte que le calcul de QT+1(θ, θT)n´ ecessite de connaˆ ıtre
une approximation (i) de l’esp´ erance conditionnelle
EθT"T
X
t=1
S(Xt−1, Xt,Yt)Y1:T, XT+1#
et (ii) de la loi de filtrage. L’esp´ erance conditionnelle se cal-
cule r´ ecursivement (voir e.g. [5, 4]). La loi de filtrage ` a l’ins-
tant (T+ 1) peut aussi ˆ etre approch´ ee r´ ecursivement par des
m´ ethodes de Monte Carlo s´ equentielles (SMC, voir e.g. [10]).
N´ eanmoins, le calcul de ces lois relatives ` a l’instant (T+ 1)
pour la valeur θTdu param` etre n´ ecessite la connaissance de
ces lois relatives ` a l’instant Tpour la mˆ eme valeur θTdu pa-
ram` etre. Dans l’it´ eration de l’EM, la propagation de ces lois de
Tvers T+ 1 ne peut donc pas ˆ etre men´ ee ` a l’aide des quantit´ es
disponibles ` a l’instant Tqui reposent toutes sur la valeur cou-
rante du param` etre θT−1. Il faut donc introduire des approxi-
mations de propagation : substituer les lois relatives ` a l’instant
Tpour la valeur θTdu param` etre par les lois relatives ` a l’ins-
tant Tpour la valeur θT−1.
Ces approximations de propagation de loi et la d´ ecomposition
(1) sont ` a l’origine des algorithmes EM en ligne pour les HMM
exponentiels propos´ es par [4] pour les HMM ` a espace d’´ etat
fini, et par [5, 6] pour les HMM ` a espace d’´ etat g´ en´ eral.

2.3 Nouvel algorithme : EM en ligne pour les
HMM
L’algorithme que nous proposons ´ etend les proc´ edures pro-
pos´ ees par [5, 6] ; cette extension indique comment exploiter
les proc´ edures pour des HMM exponentiels lorsque le mod` ele
HMM consid´ er´ e n’est pas exponentiel. Cette extension, mo-
tiv´ ee par la probl´ ematique du SLAM (voir section 3) mime ce
qui est propos´ e dans la litt´ erature SLAM probabiliste pour pas-
ser d’un mod` ele HMM non lin´ eaire (et gaussien) ` a un mod` ele
lin´ eaire (gaussien) afin de r´ esoudre le SLAM par des tech-
niques de Kalman.
La solution que nous proposons repose sur une approxima-
tion de la quantit´ eθ7→ log{mθ(x, x0)gθ(x0, y)}par un mod` ele
exponentiel. Cette approximation est “locale” au sens o` u elle
est faite ` a chaque it´ eration de l’algorithme autour de la valeur
courante du param` etre. Par suite, notre algorithme consiste ` a
d´ efinir une suite {θT}T≥0de fac¸on it´ erative, en r´ ep´ etant les
trois ´ etapes suivantes. ´ Etant donn´ ee la valeur courante du pa-
ram` etre θT,
´
Etape d’approximation par mod` ele exponentiel : approcher
log{mθ(xt−1, xt)gθ(xt,Yt)}par hS(xt−1, xt,Yt;θT), ψ(θ)iet
en d´ eduire une approximation de la log-vraisemblance compl` ete
PT+1
t=1 log{mθ(xt−1, xt)gθ(xt,Yt)}de la forme :
h
T+1
X
t=1
S(xt−1, xt,Yt;θT), ψ(θ)i.
´
Etape E de type SMC : approcher l’esp´ erance conditionnelle
EθT"T+1
X
t=1
S(Xt−1, Xt,Yt;θT)Y1:T+1#
en utilisant la proc´ edure it´ erative d´ ecrite dans la section 2.2.
Notons ˜ ST+1 cette approximation.
´
Etape M : mettre ` a jour le param` etre
θT+1 ∈argmaxθ∈Θh˜ ST+1, ψ(θ)i.
Nous donnons dans la section suivante des exemples d’approxi-
mation par mod` ele exponentiel de type quadratique. Puisque la
premi` ere ´ etape est une approximation locale, on peut substi-
tuer l’´ etape M par une ´ etape de maximisation locale, l’intuition
´
etant que l’approximation du mod` ele est d’autant meilleure que
l’on est dans un voisinage du param` etre courant.
3 Application au SLAM
Un robot ´ evolue dans un environnement (plan) inconnu. A
chaque instant t, il observe qtamers ; Yt= (Yt,i, i ∈ At)
collecte les qtmesures de distances et d’angles d’observation
de ces amers. Il se d´ eplace selon une dynamique markovienne
guid´ ee par des contrˆ oles ut(connus), et ind´ ependante de la po-
sition des amers. L’´ etat cach´ eXt∈R2×[0,2π]collecte la
pose du robot ` a l’instant t(coordonn´ ees cart´ esiennes) et son
orientation. L’objectif est d’estimer la position des amers i.e.
pour chaque amer i, le vecteur θi∈R2de ses coordonn´ ees
cart´ esiennes ; et la pose du robot. On mod´ elise ces observations
par un HMM de la forme
Xt=f(Xt−1, Vt, ut)
∀i∈ At,Yt,i =h(Xt, θi) + Wt,i ,
o` u{(Vt, Wt,i), t ≥0, i ∈ At}sont des bruits centr´ es i.i.d.
de loi connue. Avec ce choix de mod` ele, la transition de l’´ etat
cach´ emθ(x, x0)est ind´ ependante de θ; par suite, la quantit´ e
interm´ ediaire de l’EM s’exprime uniquement en fonction de
log gθ(Xt,Yt)- seuls les termes d´ ependant de θcomptant dans
l’´ etape M - et la structure exponentielle du mod` ele ne repose
que sur la forme de la loi conditionnelle log gθ. Dans les appli-
cations num´ eriques, on prend pour mod` ele (x, v ∈R3, τ, u ∈
R2)
f(x, v, u) = x+
(u1+v1)δcos(x3+u2+v2)
(u1+v1)δsin(x3+u2+v2)
(u1+v1)δ B−1sin(u2+v2) + v3
,
h(x, τ) = p(τ1−x1)2+ (τ2−x2)2
arctan(τ2−x2
τ1−x1−x3),
o` uδest le temps entre deux mesures successives et Best l’em-
pattement. On envisage successivement deux lois pour le bruit
d’observation Wt,i : tout d’abord un mod` ele gaussien puis un
mod` ele de m´ elange de deux lois gaussiennes pour d´ efinir un
mod` ele plus robuste aux observations aberrantes. Dans les deux
cas, les lois des bruits sont connues. Avec ces choix, le mod` ele
n’est pas de type “HMM exponentiel” ; on applique donc l’al-
gorithme d´ ecrit en section 2.3. Pour ce faire, on utilise l’ap-
proximation quadratique de θ7→ log gθ(x, y)issue de l’ap-
proximation ` a l’ordre 1de θ7→ h(x, θ)autour de l’estim´ ee
courante θT:h(x, θ)≈h(x, θT) + h(θ−θT),∇θh(x, θT)i.
Nous suivons en cela la d´ emarche utilis´ ee dans les algorithmes
bas´ es sur des extensions du filtre de Kalman (e.g. l’EKF).
L’application de l’algorithme pr´ esent´ e en section 2.3 permet
de r´ epondre ` a la probl´ ematique du SLAM : on dispose ` a chaque
instant T(i) d’une estim´ ee ponctuelle de la carte θT; et (ii)
d’une approximation particulaire de la loi de filtrage ` a l’instant
Ti.e. une approximation particulaire de la pose du robot ` aT.
Dans les simulations ci-dessous, on r´ esume cette loi a posteriori
par la valeur moyenne (pond´ er´ ee) des particules pour proposer
une estim´ ee ponctuelle de la pose du robot ` a chaque instant T.
Les figures 1 ` a 4 pr´ esentent l’application de cet algorithme
sur donn´ ees simul´ ees et sur donn´ ees r´ eelles1(Figure 4). Pour
les donn´ ees simul´ ees, les bruits Vtet Wt,i sont gaussiens resp.
de variance Q=diag(σ2
v, σ2
φ)o`
uσv= 0.5m.s−1et σψ=
π
60 rad ; et R=σ2
rρ
ρ σ2
b,o`
uσr= 0.5m, σb=π
60 rad et ρ=
0.01. Pour mettre en oeuvre l’algorithme, le nombre d’amers et
les matrices R, Q sont suppos´ es connus. La loi de proposition
du filtre particulaire est une approximation gaussienne de la loi
a priori m(x, x0). L’exp´ erience a ´ et´ e r´ ealis´ ee avec N= 100
particules. La figure 1 repr´ esente la vraie trajectoire du robot
(tirets), la vraie position des amers (´ etoiles) et l’estimation de
1http ://www.cas.kth.se/SLAM

la position courante du robot (points) avec le nuage de par-
ticules. La figure 2 compare notre m´ ethode (trait plein) aux
algorithmes it´ eratifs fastSLAM [11] (pointill´ es) et marginalS-
LAM [9] (points) pour l’estimation de la position d’un amer :
on trace l’erreur d’estimation moyenne (sur 100 r´ eplications
de Monte Carlo) en fonction du nombre d’it´ erations. La fi-
gure 3 donne le r´ esultat de l’exp´ erience sur 100 r´ eplications
de Monte Carlo. La vraie position des amers (points) est com-
par´ ees aux estimations moyennes fournies ` a la fin de la tra-
jectoire (ici, T= 1492) par le marginalSLAM (´ etoiles) et par
notre algorithme (croix). De mˆ eme, la vraie trajectoire du robot
(trait plein) est compar´ ee aux trajectoires moyennes sur les 100
r´ eplications pour le marginalSLAM (pointill´ es) et pour notre
algorithme (tirets). Ces figures illustrent l’int´ erˆ et de notre al-
gorithme, tant en qualit´ e d’estimation qu’en vitesse de conver-
gence.
Enfin, la figure 4 montre le r´ esultat obtenu sur donn´ ees r´ eelles :
on trace les vraies donn´ ees (amers en points ; trajectoire en trait
plein) et les estim´ ees (amers en ´ etoiles ; trajectoire en poin-
till´ es). Ces r´ esultats sont obtenus en consid´ erant un m´ elange de
gaussiennes comme bruit d’observation, de poids ω0= 0.8et
ω1= 0.2et de variance respectives Ret 5R,R´ etant donn´ e plus
haut. A la diff´ erence du cas “donn´ ees simul´ ees”, le nombre to-
tal d’amers est inconnu : l’estimation est commenc´ ee avec une
carte vide et, pour chaque nouvelle observation rec¸ue, un pro-
cessus d’association bas´ e sur la vraisemblance est utilis´ e. Si
aucun amer n’est suffisamment vraisemblable, un nouvel amer
est cr´ e´ e dans notre carte.
R´ ef´ erences
[1] A. P. Dempster, N. M. Laird, and D. B. Rubin, “Maximum likelihood
from incomplete data via the EM algorithm,” J. Roy. Statist. Soc. B, vol.
39, no. 1, pp. 1–38 (with discussion), 1977.
[2] O. Capp´ e and E. Moulines, “On-line expectation-maximization algo-
rithm for latent data models,” J. Roy. Statist. Soc. B, vol. 71, no. 3, pp.
593–613, 2009.
[3] G. Mongillo and Den` eve S., “Online learning with Hidden Markov Mo-
dels,” Neural Computation, vol. 20, pp. 1706–1716, 2008.
[4] O. Capp´ e, “Online EM algorithm for hidden Markov models,” 2011, To
appear in J. Comput. Graph. Statist.
[5] O. Capp´ e, “Online sequential Monte Carlo EM algorithm,” in In IEEE
Workshop Statist. Signal Process. (SSP), 2009.
[6] P. Del Moral, A. Doucet, and S. S. Singh, “Forward smoothing using
sequential monte carlo,” arXiv :1012.5390v1, 2010.
[7] T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot, “Consistency
of the EKF-Slam algorithm,” in IEEE International Conference on Intel-
ligent Robots and Systems, 2006, pp. 3562–3568.
[8] S.J. Julier and J.K. Uhlmann, “A counter example to the theory of simul-
taneous localization and map building,” in IEEE international conference
on robotics and automation. Citeseer, 2001, vol. 4, pp. 4238–4243.
[9] R. Martinez-Cantin, “Active map learning for robots : insights into sta-
tistical consistency,” PhD Thesis, 2008.
[10] O. Capp´ e, E. Moulines, and T. Ryd´ en, Inference in Hidden Markov Mo-
dels, Springer, 2005.
[11] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM 2.0 :
An improved particle filtering algorithm for simultaneous localization
and mapping that provably converges,” in Proceedings of the Sixteenth
IJCAI, Mexico, 2003.
FIG. 1: Donn´ ees simul´ ees
200 400 600 800 1000 1200 1400 1600 1800 2000
0
0.2
0.4
0.6
0.8
1
1.2
1.4
Time
Error
FIG. 2: Estimation de la position d’un amer
FIG. 3: Estimation de la pose
FIG. 4: Donn´ ees r´ eelles