“Human SLAM” – Simultaneous Localisation and Configuration – Particle representation
“Human SLAM” – Simultaneous Localisation and Configuration – Particle representation
0 0
slacjs-presentation
On Github wouterbulten / slacjs-presentation
“Human SLAM”
Simultaneous Localisation and Configuration
for indoor wireless sensor networks
Wouter Bulten
Supervisors: Anne van Rossum (DoBots & Almende) & Pim Haselager (Donders Institute)
Part of a Master of Science Thesis in Artificial IntelligenceRadboud University, Nijmegen
Crownstone
Intelligent wall socket for smart spaces - DoBots
In a smart home / hospital / office, how do we act when..
We do not know where users are;
or locations of devices are unknown?
→ Location is key ←
Problem statement
Given an unknown building with (smart) devices, can we autonomoumsly locate these devices and the users that walk around inside the building?
General idea
Outline
Sources of Input: What can we use?
How? FastSLAM & Particle Filters
The result: Room level accuracy
In the ‘Internet of Things’ devices are connected wirelessly
In our case: Bluetooth Low Energy (BLE)
→ Received Signal Strength Indicator (RSSI)
Measured RSSI
Beacon at fixed distances to the receiver with clear line of sightLog-distance path loss model:
$$RSSI = -10n \log_{10}(\frac{d}{d_{0}}) + A$$- $d$ the distance to the device
- $A$ signal strength at $d_0$
- $n$ an evironment constant
Estimated distance
Beacon at fixed distances to the receiver with clear line of sightFiltered RSSI signal
Using a Kalman filter with static motion modelRSSI is not enough..
RSSI gives us distance, but distance needs a where
Lets talk about robots..
Simulteneous Localisation and Mapping (SLAM)
Given robot's controls and sensor readings what is the current estimated location and map of the environment?
Replacing robots with humans
Robot → Human
Control → Heading & Number of steps
Landmark → Device / Crownstone
2D Sensor readings → (1D) RSSI + initalisation phase
Focus on robot's path → Focus on device locations
FastSLAM
Online algorithm for solving the SLAM problem
Uses Rao-Blackwellized particle filter for estimation
Each device is represented by anextended Kalman filter (EKF)
Independent landmarks
$$ p(x_{0:t}, m_{1:M} | z_{1:t}, u_{1:t}) = p(x_{0:t} | z_{1:t}, u_{1:t})\; \prod_{i=1}^M p(m_i | x_{0:t}, z_{1:t}) $$Particle filters
Probabilistic implementation of Darwin's Survival of the Fittest
Approximate the true state by a set of particles Mutate the samples on each step Resample set (with replacement)$\bar\chi_t = \chi_t = \emptyset$
$\bar\chi_t = \chi_t = \emptyset$
Initialisation, for $m \in M$:
$x_t^{[m]} \sim p(x_t | z_t, x_t^u$)
$\bar\chi_t = \chi_t = \emptyset$
Initialisation, for $m \in M$:
$x_t^{[m]} \sim p(x_t | z_t, x_t^u$)
Sample step, for $m \in M$:
$x_t^{[m]} \sim p(x_t | u_t, x_{t-1}^{[m]}$)
$w_t^{[m]} = p(z_t | x_t^{[m]}) = \frac{1}{\sigma\sqrt{2\pi}} e^{-\frac{(z_t-\mu)^{2}}{2\sigma^{2}}}$
$\bar\chi_t = \chi_t = \emptyset$
Initialisation, for $m \in M$:
$x_t^{[m]} \sim p(x_t | z_t, x_t^u$)
Sample step, for $m \in M$:
$x_t^{[m]} \sim p(x_t | u_t, x_{t-1}^{[m]}$)
$w_t^{[m]} = p(z_t | x_t^{[m]}) = \frac{1}{\sigma\sqrt{2\pi}} e^{-\frac{(z_t-\mu)^{2}}{2\sigma^{2}}}$
Resample step, for $m \in M$:
draw $i$ with probability $\propto w_t^{[m]}$
$\chi_t = \chi_t + x_t^{[m]}$
$\bar\chi_t = \chi_t = \emptyset$
Initialisation, for $m \in M$:
$x_t^{[m]} \sim p(x_t | z_t, x_t^u$)
Sample step, for $m \in M$:
$x_t^{[m]} \sim p(x_t | u_t, x_{t-1}^{[m]}$)
$w_t^{[m]} = p(z_t | x_t^{[m]}) = \frac{1}{\sigma\sqrt{2\pi}} e^{-\frac{(z_t-\mu)^{2}}{2\sigma^{2}}}$
Resample step, for $m \in M$:
draw $i$ with probability $\propto w_t^{[m]}$
$\chi_t = \chi_t + x_t^{[m]}$
$\bar\chi_t = \chi_t = \emptyset$
Initialisation, for $m \in M$:
$x_t^{[m]} \sim p(x_t | z_t, x_t^u$)
Sample step, for $m \in M$:
$x_t^{[m]} \sim p(x_t | u_t, x_{t-1}^{[m]}$)
$w_t^{[m]} = p(z_t | x_t^{[m]}) = \frac{1}{\sigma\sqrt{2\pi}} e^{-\frac{(z_t-\mu)^{2}}{2\sigma^{2}}}$
Resample step, for $m \in M$:
draw $i$ with probability $\propto w_t^{[m]}$
$\chi_t = \chi_t + x_t^{[m]}$
$\bar\chi_t = \chi_t = \emptyset$
Initialisation, for $m \in M$:
$x_t^{[m]} \sim p(x_t | z_t, x_t^u$)
Sample step, for $m \in M$:
$x_t^{[m]} \sim p(x_t | u_t, x_{t-1}^{[m]}$)
$w_t^{[m]} = p(z_t | x_t^{[m]}) = \frac{1}{\sigma\sqrt{2\pi}} e^{-\frac{(z_t-\mu)^{2}}{2\sigma^{2}}}$
Resample step, for $m \in M$:
draw $i$ with probability $\propto w_t^{[m]}$
$\chi_t = \chi_t + x_t^{[m]}$
$\bar\chi_t = \chi_t = \emptyset$
Initialisation, for $m \in M$:
$x_t^{[m]} \sim p(x_t | z_t, x_t^u$)
Sample step, for $m \in M$:
$x_t^{[m]} \sim p(x_t | u_t, x_{t-1}^{[m]}$)
$w_t^{[m]} = p(z_t | x_t^{[m]}) = \frac{1}{\sigma\sqrt{2\pi}} e^{-\frac{(z_t-\mu)^{2}}{2\sigma^{2}}}$
Resample step, for $m \in M$:
draw $i$ with probability $\propto w_t^{[m]}$
$\chi_t = \chi_t + x_t^{[m]}$
Ingredients
Signal strength + Motion
FastSLAM + Particle Filters
Particle representation
- Current user position:
$$(x^u_t,y^u_t)$$
-
Extended Kalman Filter for each landmark:
$$[(x_t^{[m]}, y_t^{[m]}), \Sigma_{t}^{[m]}]$$
- Importance weight:
$$w_{t}^{[m]} = w_{t-1}^{[m]} f(z | h_{m}, \sigma_z)$$
- Trace of the user's path:
$$[(x^u_0,y^u_0), \ldots, (x^u_t,y^u_t)]$$
(Only required for visualisation)
EKF Update
$$[x_{t+1}^{[m]}, y_{t+1}^{[m]}]^T = [x_{t}^{[m]}, y_{t}^{[m]}]^T + Kv$$
$$\Sigma_{t+1}^{[m]} = \Sigma_{t}^{[m]} - K \sigma_v K^T$$
Implementation details
Fully written in Javascript (including pedometer, SLAM algorithm, particle filters and visualisations)
ECMAScript 6/2015 (with BabelJS as transpiler)
Built for Android & iOS(Using Cordova)
DoBeacons from DoBots(iBeacon compatible, BLE)
Code on GitHub (See: github.com/wouterbulten/slacjs)
Demo
Average error: ∞ m
Evaluating the algorithm
Simulations
Perfect world performance
Effect of number of RSSI measurements
Effect of device location
Live tests
Effect of environment noise
Effect of RSSI measurements
Curve estimation model. Significant regression equation. $$(F (1, 3498) = 33868.750, p < .000), R^2 = .906$$Effect of device location
Repeated-measure-ANOVA for each condition. Significant effect for each condition (p = .000). $$eta^2 = [.503, .698, .284, .063, .052, .065, .065]$$Simulations
Error in meters per landmark, 100 runs per data setSimulation experiments
Noisy movement: 0.56 m (sd 0.20 m)
Perfect world: 0.26 m (sd 0.07 m)
Live tests
Evaluation method
Online recordings (RSSI & motion)
Offline evaluation, 500 per run
Measured average localisation error for DoBeacons
Localisation error @ Almende
Accuraccy of 2.3m, 60 steps ~ 2min walking
Real world experiments
Error in meters per landmark, 100 runs per data setReal world experiments
Run 1: 2.41 m (sd 0.38 m)
Run 1: 2.30 m (sd 0.18 m)
Run 1: 3.01 m (sd 0.51 m)
Difference in performance between simulation and real world
Walls, obstacles, users themselves
Mirroring errors
→ Invest in better motion modelling
→ Use more of the environment
Beacon performance fluctuates
Dependent on movement and location
→ Longer traces
→ Map fusing
Future work: Map fusing
“Human SLAM”
Using an adapted range-only version of SLAM, we replaced the robot with a human and used that to locate both humans and devices inside buildings.