Date post: | 14-Jun-2015 |
Category: |
Education |
Upload: | andrea-alessandretti |
View: | 74 times |
Download: | 1 times |
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 ?