King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 1 / 28 WiFi GraphSLAM in 1-D Jose Roberto Ayala Solares August 2, 2011
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 1 / 28
WiFi GraphSLAM in 1-D
Jose Roberto Ayala Solares
August 2, 2011
Overview
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 2 / 28
Introduction
The GraphSLAM Algorithm
WiFi Implementation
Simulation Results
Conclusions
Introduction
Overview
Introduction
SLAM ProblemGraph-basedSLAM
WiFi GraphSLAM
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 3 / 28
SLAM Problem
Overview
Introduction
SLAM ProblemGraph-basedSLAM
WiFi GraphSLAM
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 4 / 28
Nowadays there is active research in the area of mobilerobotics that deals with the capacity of a robot to build amap of the environment and to simultaneoulsy localize itselfwithin this map in absence of external referencing systemssuch as GPS.
This scenario is the so-called simultaneous localization andmapping (SLAM) problem.
Solving the SLAM problem consists of estimating the robottrajectory and the map of the environment as the robotmoves in it.
One common approach to solving the SLAM problem is touse an Extended Kalman filter.
Graph-based SLAM
Overview
Introduction
SLAM ProblemGraph-basedSLAM
WiFi GraphSLAM
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 5 / 28
A graph-based SLAM approach constructs a simplifiedestimation problem by abstracting the raw measuraments.
These raw measurements are replaced by the edges in agraphical model which can then be seen as “virtualmeasurements”.
Each edge between two nodes is labeled with a probabilitydistribution over the relative locations of the two poses,conditioned to their mutual measurements.
WiFi GraphSLAM
Overview
Introduction
SLAM ProblemGraph-basedSLAM
WiFi GraphSLAM
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 6 / 28
The widespread deployment of wireless sensor networksprovide the opportunity for localization and mapping usingonly signal-strength measurements.
Figure 1: WiFi GraphSLAM scenario
The GraphSLAM Algorithm
Overview
Introduction
The GraphSLAMAlgorithm
Description
The Algorithm:InitializationThe Algorithm:LinearizationThe Algorithm:ReductionThe Algorithm:Solution
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 7 / 28
Description
Overview
Introduction
The GraphSLAMAlgorithm
Description
The Algorithm:InitializationThe Algorithm:LinearizationThe Algorithm:ReductionThe Algorithm:Solution
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 8 / 28
Figure 2: Illustration of the acquisition of the information matrix Ωin GraphSLAM [From Probabilistic Robotics, MIT Press]
Description
Overview
Introduction
The GraphSLAMAlgorithm
Description
The Algorithm:InitializationThe Algorithm:LinearizationThe Algorithm:ReductionThe Algorithm:Solution
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 9 / 28
Figure 3: Illustration of the acquisition of the information matrix Ωin GraphSLAM [From Probabilistic Robotics, MIT Press]
Description
Overview
Introduction
The GraphSLAMAlgorithm
Description
The Algorithm:InitializationThe Algorithm:LinearizationThe Algorithm:ReductionThe Algorithm:Solution
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 10 / 28
Figure 4: Illustration of the acquisition of the information matrix Ωin GraphSLAM [From Probabilistic Robotics, MIT Press]
The Algorithm: Initialization
Overview
Introduction
The GraphSLAMAlgorithm
Description
The Algorithm:InitializationThe Algorithm:LinearizationThe Algorithm:ReductionThe Algorithm:Solution
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 11 / 28
To build our initial information matrix Ω and informationvector ξ, we need an initial estimate µ0:t for all poses x0:t.
The Algorithm: Linearization
Overview
Introduction
The GraphSLAMAlgorithm
Description
The Algorithm:InitializationThe Algorithm:LinearizationThe Algorithm:ReductionThe Algorithm:Solution
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 12 / 28
Prior
Ω← Ω0 (1)
ξ ← ~0 (2)
The Algorithm: Linearization
Overview
Introduction
The GraphSLAMAlgorithm
Description
The Algorithm:InitializationThe Algorithm:LinearizationThe Algorithm:ReductionThe Algorithm:Solution
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 13 / 28
Controls
Ω← Ω+
(
−GTt
1
)
R−1
t
(
−Gt 1)
(3)
ξ ← ξ +
(
−GTt
1
)
R−1
t [g (ut, µt−1)−Gtµt−1] (4)
Measurements
Ω← Ω+H iTt Q−1
t H it (5)
ξ ← ξ +H iTt Q−1
t
[
zit − h(
µt, cit
)
+H itµt
]
(6)
The Algorithm: Reduction
Overview
Introduction
The GraphSLAMAlgorithm
Description
The Algorithm:InitializationThe Algorithm:LinearizationThe Algorithm:ReductionThe Algorithm:Solution
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 14 / 28
Let us subdivide the matrix Ω and the vector ξ intosubmatrices:
Ω =
(
Ωx0:t, x0:tΩx0:t, m
Ωm, x0:tΩm,m
)
ξ =
(
ξx0:t
ξm
)
(7)
According to the marginalization lemma:
Ω = Ωx0:t, x0:t− Ωx0:t, mΩ−1
m, mΩm, x0:t(8)
ξ = ξx0:t− Ωx0:t, mΩ−1
m, mξm (9)
The Algorithm: Solution
Overview
Introduction
The GraphSLAMAlgorithm
Description
The Algorithm:InitializationThe Algorithm:LinearizationThe Algorithm:ReductionThe Algorithm:Solution
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 15 / 28
The mean and covariance of the robot poses are given by
Σ = Ω−1 (10)
µ = Σξ (11)
The mean location estimate for all features in the map isgiven by
Σm = Ω−1
m,m (12)
µm = Σm
(
ξm +Ωm, x0:tξ)
(13)
WiFi Implementation
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
DataState-to-measurementmapping
GaussianInterpolation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 16 / 28
Data
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
DataState-to-measurementmapping
GaussianInterpolation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 17 / 28
0 5 10 15 20 25 3030
35
40
45
50
55
60
Distance [blocks]
Sig
nal S
tren
gth
[dB
]
Data
Figure 5: WiFi signal-strength measurements
State-to-measurement mapping
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
DataState-to-measurementmapping
GaussianInterpolation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 18 / 28
Gaussian interpolation weights have been used with successto interpolate WiFi signal strengths.
The sensor model is given by
h (µ) =1√2πσ
∑
i
si · exp[
−(|µj − µt| − xi)2
2σ2
]
(14)
(xi, si) is the data collected
(µj , µt) is the estimated position of the landmark and robot at t
σ is a length scale that determines the strenght of correlationbetween points
Gaussian Interpolation
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
DataState-to-measurementmapping
GaussianInterpolation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 19 / 28
0 5 10 15 20 25 3030
35
40
45
50
55
60
Distance [blocks]
Sig
nal S
tren
gth
[dB
]
σ = 5
DataInterpolation
0 5 10 15 20 25 3030
35
40
45
50
55
60
Distance [blocks]
Sig
nal S
tren
gth
[dB
]
σ = 1
DataInterpolation
0 5 10 15 20 25 3030
35
40
45
50
55
60
Distance [blocks]
Sig
nal S
tren
gth
[dB
]
σ = 0.6
DataInterpolation
0 5 10 15 20 25 300
50
100
150
200
250
Distance [blocks]
Sig
nal S
tren
gth
[dB
]
σ = 0.1
DataInterpolation
Figure 6: Gaussian interpolation
Simulation Results
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Scenario
Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 20 / 28
Scenario
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Scenario
Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 21 / 28
System model: xt = xt−1 + 0.5
Process noise: R = 0.0625
Measurement noise: Q = 16
Number of routers: 2
Distance covered: 10 blocks
Results
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Scenario
Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 22 / 28
0 2 4 6 8 10 12 14 16 18 20−2
0
2
4
6
8
10
12
14
Iteration k
Pos
ition
of t
he r
obot
[blo
cks]
True valueEstimated valueConfidence interval
R1Location: 2.5584Variance: 0.7641
R2Location: 7.7175Variance: 7.2614
Figure 7: Simulation of the WiFi GraphSLAM algorithm
Results: Variation in the length scale σ
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Scenario
Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 23 / 28
0 2 4 6 8 10 12 14 16 18 20−2
0
2
4
6
8
10
12
Iteration k
Pos
ition
of t
he r
obot
[blo
cks]
σ = 0.1
True valueEstimated valueConfidence interval
R1Location: 2.3940Variance: 0.0313
R2Location: 7.3666Variance: 1.6037
0 2 4 6 8 10 12 14 16 18 20−2
0
2
4
6
8
10
12
14
Iteration k
Pos
ition
of t
he r
obot
[blo
cks]
σ = 5
True valueEstimated valueConfidence interval
R1Location: 2.1364Variance: 0.8487
R2Location: 8.0174Variance: 7.4375
Figure 8: Simulation of the WiFi GraphSLAM algorithm
Results: Variation in the process noise R
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Scenario
Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 24 / 28
0 2 4 6 8 10 12 14 16 18 20−2
0
2
4
6
8
10
12
Iteration k
Pos
ition
of t
he r
obot
[blo
cks]
R = 0.01
True valueEstimated valueConfidence interval
R1Location: 2.4217Variance: 0.2010
R2Location: 6.8494Variance: 1.7841
0 2 4 6 8 10 12 14 16 18 20−2
0
2
4
6
8
10
12
14
Iteration k
Pos
ition
of t
he r
obot
[blo
cks]
R = 1
True valueEstimated valueConfidence interval
R1Location: 2.2892Variance: 2.6820
R2Location: 7.4277Variance: 61.9038
Figure 9: Simulation of the WiFi GraphSLAM algorithm
Results: Variation in the measurement noise Q
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Scenario
Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 25 / 28
0 2 4 6 8 10 12 14 16 18 20−2
0
2
4
6
8
10
12
14
16
Iteration k
Pos
ition
of t
he r
obot
[blo
cks]
Q = 4
True valueEstimated valueConfidence interval
R1Location: 2.4285Variance: 1.2183
R2Location: 7.9306Variance: 43.6811
0 2 4 6 8 10 12 14 16 18 20−2
0
2
4
6
8
10
12
14
16
Iteration k
Pos
ition
of t
he r
obot
[blo
cks]
Q = 64
True valueEstimated valueConfidence interval
R1Location: 2.3210Variance: 6.0202
R2Location: 7.1906Variance: 84.0660
Figure 10: Simulation of the WiFi GraphSLAM algorithm
Conclusions
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 26 / 28
Conclusions
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 27 / 28
A WiFi GraphSLAM algorithm for localization and mappingin a one-dimensional environment was implemented.
The GraphSLAM algorithm is an intuitive and efficientalternative to solve the SLAM problem.
The obtention of real WiFi signal-strength measurementsprovided interesting and accurate results.
Variations on the parameters of the scenario providedinteresting results about their respective effect on the pathand the map estimates.
Further analysis is required to extend these results into amore realistic scenario (2D, fading, shadowing,...).
Questions?
Overview
Introduction
The GraphSLAMAlgorithm
WiFiImplementation
Simulation Results
Conclusions
King Abdullah University of Science and Technology - Thuwal - KSA - 2011 – 28 / 28