A Minimum Energy Solution to Monocular Simultaneous Localization And Mapping

Andrea Alessandretti 1 (speaker)António Pedro Aguiar 2 João Pedro Hespanha 3Paolo Valigi 4

Conference on Decision and Control 2011, Orlando, USA

1 IST-EPFL joint doctoral program, Portugal/Switzerland2 Instituto Superior Técnico (IST), Portugal

3 University of California Santa Barbara (UCSB), USA4 University of Perugia, Italy

Motivation

• Autonomous robotics

• Navigation without a priori knowledge of the map

• Localization in environment without global positioning ( e.g. underwater vehicle )

Motivation

• Autonomous robotics

• Navigation without a priori knowledge of the map

• Localization in environment without global positioning ( e.g. underwater vehicle )

SLAM problem : Estimate the robot position and environment map using only information of the control input and relative observation of the environment.

SLAM module

u(t), t � [0, T ] y(t), t � [0, T ]

li(T ), i = 1, . . . , Np(T )

Motivation

• Autonomous robotics

• Navigation without a priori knowledge of the map

• Localization in environment without global positioning ( e.g. underwater vehicle )

Monocular SLAM problem : Perform SLAM using a single camera sensor for the relative observation.

SLAM problem : Estimate the robot position and environment map using only information of the control input and relative observation of the environment.

SLAM module

u(t), t � [0, T ] y(t), t � [0, T ]

li(T ), i = 1, . . . , Np(T )

Outline

•From SLAM to System with perspective outputs

•Minimum Energy Observer

•SLAM algorithm

•Simulation results

Kinematic model

lWi = pW + RWC lCi

lCi = RTWC l

Wi � RT

WCpW

Camera configuration : (pW , RWC) � SE(3)

• Model description {C}

pW

{W}

Kinematic model

lWi = pW + RWC lCi

lCi = RTWC l

Wi � RT

WCpW

Camera configuration : (pW , RWC) � SE(3)

�C =

�

�0 ��3 �2

�3 0 ��1

��2 �1 0

�

�

Camera twist : (vC ,�C) � se(3)

Control input : u =�vC �C

�T

• Model description {C}

pW

{W}

Kinematic model

lWi = pW + RWC lCi

lCi = RTWC l

Wi � RT

WCpW

� �� �li p� �� �

Camera configuration : (pW , RWC) � SE(3)

�C =

�

�0 ��3 �2

�3 0 ��1

��2 �1 0

�

�

Camera twist : (vC ,�C) � se(3)

Control input : u =�vC �C

�T

• Model description {C}

pW

{W}

Kinematic model

lWi = pW + RWC lCi

lCi = RTWC l

Wi � RT

WCpW

� �� �li p� �� �

Camera configuration : (pW , RWC) � SE(3)

�C =

�

�0 ��3 �2

�3 0 ��1

��2 �1 0

�

�

Camera twist : (vC ,�C) � se(3)

Control input : u =�vC �C

�T

• Model description

�

����

pl1���˙lN

�

���� =

�

�����

��C 0 . . . 0

0 ��C� � � 0

������

� � ����

0 0 . . . ��C

�

�����

�

����

pl1���lN

�

����+

�

����

vC

0���0

�

����

• State equation

{C}

pW

{W}

Perspective outputs

�iyi = lCi = li � p

Observation

Unknown scalar

Omnidirectional camera :�i = lCi,3Normalized retina :

Optic center

yi Principal Point(uc , vc)

(u, v) Image point

Camera Image Plane

{W}

{C}

yi =

�

�lCi,1/l

Ci,3

lCi,2/lCi,3

1

�

� Normalized Retina (i.e. f=1)

lCi =

�

�lCi,1lCi,2lCi,3

�

�

�i = ||lCi ||

Perspective outputs

�iyi = lCi = li � p

Observation

Unknown scalar

Omnidirectional camera :�i = lCi,3Normalized retina :

Optic center

yi Principal Point(uc , vc)

(u, v) Image point

Camera Image Plane

{W}

{C}

yi =

�

�lCi,1/l

Ci,3

lCi,2/lCi,3

1

�

� Normalized Retina (i.e. f=1)

�

����

pl1���

pN

�

���� =

�

�����

��C 0 . . . 0

0 ��C � � � 0���

���� � �

���0 0 . . . ��C

�

�����

�

����

pl1���

pN

�

����+

�

����

vC

0���0

�

����

�iyi = li � p , i = 1, . . . , N

• System with perspective outputs:

lCi =

�

�lCi,1lCi,2lCi,3

�

�

�i = ||lCi ||

Minimum Energy Observer

• System with perspective outputs : x = A(u)x + b(u) + G(u)d

�iyi = Ci(u)x + di(u) + ni , i = 1, . . . , N

J(z ; t) := mind :[0,t),nj (ti ),�ji

i=0,1,...,k

{(x(0)� x0)TP�10 (x(0)� x0)

+

� t

0�d(�)�2 d�+

k�

i=0

�

j�J�nj(ti)�2 : x(t) = z}

• Minimum Energy observer :

x(t) := arg minz�Rn

J(z, t)

• Deterministic unknown noise

• Solution (filter equation) : impulsive dynamic filter

Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. “IEEE Trans. on Automat. Contr.”, 51, 226--241.

(nonlinear dynamic)

• We present the fully discrete version

Minimum Energy Observer

• System with perspective outputs : x = A(u)x + b(u) + G(u)d

�iyi = Ci(u)x + di(u) + ni , i = 1, . . . , N

J(z ; t) := mind :[0,t),nj (ti ),�ji

i=0,1,...,k

{(x(0)� x0)TP�10 (x(0)� x0)

+

� t

0�d(�)�2 d�+

k�

i=0

�

j�J�nj(ti)�2 : x(t) = z}

• Minimum Energy observer :

x(t) := arg minz�Rn

J(z, t)

Example :• Deterministic unknown noise

• Solution (filter equation) : impulsive dynamic filter

Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. “IEEE Trans. on Automat. Contr.”, 51, 226--241.

(nonlinear dynamic)

• We present the fully discrete version

Minimum Energy Observer

• System with perspective outputs : x = A(u)x + b(u) + G(u)d

�iyi = Ci(u)x + di(u) + ni , i = 1, . . . , N

J(z ; t) := mind :[0,t),nj (ti ),�ji

i=0,1,...,k

{(x(0)� x0)TP�10 (x(0)� x0)

+

� t

0�d(�)�2 d�+

k�

i=0

�

j�J�nj(ti)�2 : x(t) = z}

• Minimum Energy observer :

x(t) := arg minz�Rn

J(z, t)

Example :• Deterministic unknown noise

• Solution (filter equation) : impulsive dynamic filter

Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. “IEEE Trans. on Automat. Contr.”, 51, 226--241.

(nonlinear dynamic)

• We present the fully discrete version

Minimum Energy Observer

• System with perspective outputs : x = A(u)x + b(u) + G(u)d

�iyi = Ci(u)x + di(u) + ni , i = 1, . . . , N

J(z ; t) := mind :[0,t),nj (ti ),�ji

i=0,1,...,k

{(x(0)� x0)TP�10 (x(0)� x0)

+

� t

0�d(�)�2 d�+

k�

i=0

�

j�J�nj(ti)�2 : x(t) = z}

• Minimum Energy observer :

x(t) := arg minz�Rn

J(z, t)

Example :• Deterministic unknown noise

• Solution (filter equation) : impulsive dynamic filter

Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. “IEEE Trans. on Automat. Contr.”, 51, 226--241.

(nonlinear dynamic)

• We present the fully discrete version

Minimum Energy Observer

• System with perspective outputs : x = A(u)x + b(u) + G(u)d

�iyi = Ci(u)x + di(u) + ni , i = 1, . . . , N

J(z ; t) := mind :[0,t),nj (ti ),�ji

i=0,1,...,k

{(x(0)� x0)TP�10 (x(0)� x0)

+

� t

0�d(�)�2 d�+

k�

i=0

�

j�J�nj(ti)�2 : x(t) = z}

• Minimum Energy observer :

x(t) := arg minz�Rn

J(z, t)

Example :• Deterministic unknown noise

• Solution (filter equation) : impulsive dynamic filter

Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. “IEEE Trans. on Automat. Contr.”, 51, 226--241.

(nonlinear dynamic)

• We present the fully discrete version

Minimum Energy Observer

• System with perspective outputs : x = A(u)x + b(u) + G(u)d

�iyi = Ci(u)x + di(u) + ni , i = 1, . . . , N

J(z ; t) := mind :[0,t),nj (ti ),�ji

i=0,1,...,k

{(x(0)� x0)TP�10 (x(0)� x0)

+

� t

0�d(�)�2 d�+

k�

i=0

�

j�J�nj(ti)�2 : x(t) = z}

• Minimum Energy observer :

x(t) := arg minz�Rn

J(z, t)

Example :• Deterministic unknown noise

• Solution (filter equation) : impulsive dynamic filter

Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. “IEEE Trans. on Automat. Contr.”, 51, 226--241.

(nonlinear dynamic)

• We present the fully discrete version

Minimum Energy Observer

• System with perspective outputs : x = A(u)x + b(u) + G(u)d

�iyi = Ci(u)x + di(u) + ni , i = 1, . . . , N

J(z ; t) := mind :[0,t),nj (ti ),�ji

i=0,1,...,k

{(x(0)� x0)TP�10 (x(0)� x0)

+

� t

0�d(�)�2 d�+

k�

i=0

�

j�J�nj(ti)�2 : x(t) = z}

• Minimum Energy observer :

x(t) := arg minz�Rn

J(z, t)

Example :• Deterministic unknown noise

• Solution (filter equation) : impulsive dynamic filter

Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. “IEEE Trans. on Automat. Contr.”, 51, 226--241.

(nonlinear dynamic)

• We present the fully discrete version

Outline

•From SLAM to System with perspective outputs

•Minimum Energy Observer

•SLAM algorithm

•Simulation results

SLAM algorithm

• Image acquisition

• Feature extraction

• Feature matching

• Filter Prediction step

• Initialization new feature

• Filter update step

li

p

SLAM algorithm

• Image acquisition

• Feature extraction

• Feature matching

• Filter Prediction step

• Initialization new feature

• Filter update step

li

p

SLAM algorithm

• Image acquisition

• Feature extraction

• Feature matching

• Filter Prediction step

• Initialization new feature

• Filter update step

li

p

Feature Initialization• Initialization function :�yn =

�y1 y2

�TR � ��

�lCn = g(y1, y2, �) =

��y1 �y2 �

�T

���50px 0px

�T�2 00 2

�15 2

�

���150px 0px

�T�1 00 1

�10 2

�

��120px 0px

�T�50 00 50

�8 1

�

��0px 0px

�T�1 00 1

�10 2

�

Feature Initialization• Initialization function :

LANDMARK ESTIMATE

lCn =��y1 �y2 �

�T

Pln = �g

�R 00 �2�

��T

g

�yn =

�y1 y2

�TR � ��

�lCn = g(y1, y2, �) =

��y1 �y2 �

�T

���50px 0px

�T�2 00 2

�15 2

�

���150px 0px

�T�1 00 1

�10 2

�

��120px 0px

�T�50 00 50

�8 1

�

��0px 0px

�T�1 00 1

�10 2

�

Feature Initialization• Initialization function :

LANDMARK ESTIMATE

lCn =��y1 �y2 �

�T

Pln = �g

�R 00 �2�

��T

g

�yn =

�y1 y2

�TR � ��

�

(x, P )

(x, P )(x, P )

STATE AUGMENTATION

x =�x lCn + p

�T

P =

�P 00 Pln + Pp

�

lCn = g(y1, y2, �) =��y1 �y2 �

�T

���50px 0px

�T�2 00 2

�15 2

�

���150px 0px

�T�1 00 1

�10 2

�

��120px 0px

�T�50 00 50

�8 1

�

��0px 0px

�T�1 00 1

�10 2

�

Observer equation

P�i+1 = AiPiATi + GiG

Ti

x�i+1 = Ai xi + bi

PREDICTION UPDATE

Pi+1 = ((P�i+1)�1 +Wi+1)

�1

xi+1 = x�i+1 � P�i+1Wi+1x

�i+1

• Recursive solution

W (ti) :=�

j�Jk�JCT

j (u)

�

I �yj(ti)yj(ti)T��yj(ti)

��2

�

Cj(u)

• Exact solution ( No linearization procedures ! vs EKF SLAM )

with

Convergence analysis

M(ti) =�

k�i ,k�Io

M(y(tk))with

�Pp(ti) Ppm(ti)Pmp(ti) Pm(ti)

�=

�Pp(0) Pp(0)Pp(0) Pp(0) + M�1(ti)

�At time :ti

UPDATE

Pi+1 = ((P�i+1)�1 +Wi+1)

�1

xi+1 = x�i+1 � P�i+1Wi+1x

�i+1

x(t) := x(t)� x(t)

||x(t)|| � ce��t ||x(0)||+ �d sup��(0,t)

||d(�)||+N�

j=1

�j sup��(0,t)

||nj(�)||

• ISS with respect to the disturbances

persistence of excitation-like condition ( e.g. parallax of observation )

• The landmark uncertainly is lower bounded by the camera pose uncertainty at the time of the first observation

Wi :=�

j�Jk�JCT

j (u)M(y(ti))Cj(u)

Analysis assumption : single landmark, static camera

Simulation results

!15 !10 !5 0 5 10 15

!5

0

5

10

15

20

Uncertainty Ellipses

True Landmark Position

Camera

Observed Landmarks

Simulation results

Settings

�ini = 0��ini = 2000

• Initialization parameters[m][m]

�t = 1/30v = 1 [m/s]

[s]

Q = (0.02)2I3�3R = (0.06)2I3�3

• Simulated noise

[m2][(m/s)2]

2D TOP VIEW

Simulation results

• Trace of the covariance matrix of the landmarks (m)0 10 20 30 40 50 60 70

-10-8-6-4-202468

10

Time (s)

0 10 20 30 40 50 60 700

200

400

600

800

1000

1200

Time (s)

69 69.2 69.4 69.6 69.8 70-0.08

-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0.08

0.1

Time (s)

69 69.1 69.2 69.3 69.4 69.5 69.6 69.7 69.8 69.9 70

-0.06-0.04-0.02

00.020.040.060.08

0.10.120.14

Time (s)

• State estimation error (m)

Conclusion

•Optimal solution to Monocular SLAM

•No linearization error introduced ( vs EKF SLAM )

•Convergency analysis

Thanks for your attention

Any questions ?