A Radio-Inertial Localization and Tracking System with BLE Beacons Prior Maps

TL;DR

This paper presents a low-cost indoor localization system combining radio signals, inertial sensors, and BLE beacon maps, using probabilistic modeling and optimization for accurate robot tracking and map correction.

Contribution

It introduces a novel probabilistic IMU motion model and an integrated optimization framework for simultaneous localization and map refinement.

Findings

Improved localization accuracy with the proposed system.

Robustness to beacon map errors demonstrated.

Closed-loop feedback enhances system performance.

Abstract

In this paper, we develop a system for the low-cost indoor localization and tracking problem using radio signal strength indicator, Inertial Measurement Unit (IMU), and magnetometer sensors. We develop a novel and simplified probabilistic IMU motion model as the proposal distribution of the sequential Monte-Carlo technique to track the robot trajectory. Our algorithm can globally localize and track a robot with a priori unknown location, given an informative prior map of the Bluetooth Low Energy (BLE) beacons. Also, we formulate the problem as an optimization problem that serves as the Back-end of the algorithm mentioned above (Front-end). Thus, by simultaneously solving for the robot trajectory and the map of BLE beacons, we recover a continuous and smooth trajectory of the robot, corrected locations of the BLE beacons, and the time-varying IMU bias. The evaluations achieved using hardware show that through the proposed closed-loop system the localization performance can be improved; furthermore, the system becomes robust to the error in the map of beacons by feeding back the optimized map to the Front-end.

Tables

Table 1. TABLE I: Parameters used in the experiments.

Parameter	Symbol	Value
$-$ Path-loss model parameters [9]:
Attenuated transmission power	$a_X$	-64.53
The path-loss exponent	$\gamma$	1.72
Reference distance	$d_0$	1.78 $m$
$-$ Range-only measurement model (Gaussian noise):
standard deviation	${\sigma }_n$	5 $m$
$-$ Random walk motion model:
Position standard deviation	${\sigma }_p$	0.1 $m$
Velocity standard deviation	${\sigma }_v$	0.05 $m{\sec }^{-1}$
$-$ Probabilistic IMU motion model:
Acceleration standard deviation	${\sigma }_a$	1.5 $m^2{\sec }^{-1}$
Velocity standard deviation	${\sigma }_v$	0.005 $m{\sec }^{-1}$
$-$ Dead reckoning motion model:
Motion increment size	$-$	0.075 $m$
Velocity standard deviation	${\sigma }_v$	0.03 $m{\sec }^{-1}$
$-$ Particle filter:
Number of particles	$n_p$	300
Resampling threshold	$n_{thr}$	60
$-$ BLE Beacon Parameters:
Transmission Power	$-$	+4 $dBm$
Broadcasting Frequency	$-$	10 $Hz$

Equations

$$p({\mathcal{X}}_{0:T},{\mathcal{D}}_{1:T})=p({\mathcal{X}}_0)\prod _{t=1}^{T}p({\mathcal{D}}_t|{\mathcal{X}}_t)$$

$$\log p({\mathcal{X}}_{0:T},{\mathcal{D}}_{1:T})=\log p({\mathcal{X}}_0)+\sum _{t=1}^T\log p({\mathcal{D}}_t|{\mathcal{X}}_t)$$

$${\mathcal{X}}_{0:T}^{\star }=$$

$$=\underset{{\mathcal{X}}_{0:T}}{arg\; min}\Vert {\mathbf{r}}_0{\Vert }_{{\mathbf{\Sigma }}_0}^2+\Vert {\mathbf{r}}_{0:T}{\Vert }_{{\mathbf{\Sigma }}_{\mathbf{r}}}^2$$

$${\bar{\mathbf{x}}}_{t+1}={\mathbf{F}}_t{\bar{\mathbf{x}}}_t+{\mathbf{G}}_t{\mathbf{u}}_t$$

$${\mathbf{F}}_t=\left[\begin{array}{cc}{\mathbf{I}}_3& t_s{\mathbf{I}}_3\\ {\mathbf{O}}_3& {\mathbf{I}}_3\end{array}\right],{\mathbf{G}}_t=\left[\begin{array}{cc}0.5t_s^2{\mathbf{I}}_3& t_s{\mathbf{I}}_3\\ t_s{\mathbf{I}}_3& {\mathbf{I}}_3\end{array}\right]$$

$${\tilde{\mathbf{a}}}_t={\hat{\mathbf{R}}}_t{\mathbf{a}}_t+\mathbf{g}$$

$${\hat{\mathbf{a}}}_t=\frac{{\tilde{\mathbf{a}}}_t}{\Vert {\tilde{\mathbf{a}}}_t\Vert }\Vert {\tilde{\mathbf{a}}}_t\Vert \ne 0$$

Full text

I INTRODUCTION

Indoor positioning systems are crucial for applications such as asset tracking and inventory management. Such systems can also increase the performance of first responders. The Visual-Inertial Navigation Systems (VINS) provide reliable solutions in both indoors and outdoors [1, 2]. However, VINS often rely on proper lighting and rich visual information streams. Furthermore, using cameras can raise privacy concerns [3] which can limit the application of such systems. As an alternative or (as we prefer) a complementary solution, indoor positioning systems based on standard wireless communication technologies have also been studied extensively [4, 5]. In indoors, radio signals are severely impacted due to shadowing and multipathing effects [6, 7] which make the available wireless-based positioning systems less accurate ($1-10\mathop{\mathrm{m}}$) [4, 8].

Wireless Local Area Network (WLAN) and Bluetooth Low Energy (BLE) technologies are widespread and ubiquitous. Thus, we focus our attention on Radio Signal Strength Indicator (RSSI) available through WLAN and BLE broadcasts. In our previous work [9], we developed an RSSI-based indoor localization framework embedded with an online observation classifier to localize a smartphone user or a robot in a given environment. Similar to [9], we use Sample Importance Resampling (SIR) filter (or particle filter) embedded with the systematic resampling algorithm as it is suitable to deal with global uncertainty, nonlinear observation space, and multi-modal posterior densities [10, 11, 12].

In this paper, we propose a radio-inertial localization and tracking system that exploits BLE, Inertial Measurement Unit (IMU), and magnetometer sensors with the quality available in standard smartphones; an illustrative example of the achieved results is shown in Figure 1. This paper offers the following contributions. We propose a novel and simplified probabilistic IMU motion model that serves as the proposal distribution of the Particle Filter (PF) algorithm. The probabilistic IMU motion model enables the localization algorithm to exhibit a probabilistically sound predictive behavior and track the robot trajectory (Front-end). We then simultaneously solve for the robot trajectory, the map of BLE beacons, and the time-varying IMU bias using incremental Smoothing And Mapping (iSAM) [14, 15, 16] and IMU-preintegration technique [2] (Back-end); and develop the entire radio-inertial localization and tracking framework and show that by a closed-loop architecture, Figure 2, the overall system performance can be improved according to the cumulative distribution function of the localization error [8]. In addition, the system becomes robust to the error in the map of beacons by feeding back the optimized map to the Front-end. Finally, we provide experimental evaluations along reasonably long trajectories for indoor environments.

I-A Outline

A review of related works is given in the following section. Section III describes the problem statement and formulation. The proposed system overview is explained in Section IV; followed by presenting the probabilistic IMU motion model in Section V. Experimental results as well as discussions on limitations of the proposed framework are presented in Section VI. Finally, Section VII concludes the paper and provides ideas as future work.

I-B Notation

Matrices are capitalized in bold, such as in $\mathbf{X}$, and vectors are in lower case bold type, such as in $\mathbf{x}$. Vectors are column-wise and $1\colon n$ means integers from $1$ to $n$. The Euclidean and Frobenius norms are shown by $\lVert\cdot\rVert$ and $\lVert\cdot\rVert_{\mathrm{F}}$, respectively. $\lVert\mathbf{e}\rVert_{\boldsymbol{\Sigma}}^{2}\triangleq\mathbf{e}^{\mathrm{T}}\boldsymbol{\Sigma}^{-1}\mathbf{e}$. Random variables, such as $X$, and their realizations, $x$, are sometimes denoted interchangeably. $x^{[i]}$ denotes a reference to the $i$-th element of the variable. An alphabet such as $\mathcal{X}$ denotes a set. The $n$-by-$n$ identity and zero matrices are denoted by $\mathbf{I}_{n}$ and $\mathbf{O}_{n}$, respectively. $\mathbf{0}_{n}$ denotes a vector of zeros of size $n$. $\mathrm{vec}(x^{[1]},\dots,x^{[n]})$ denotes a vector such as $\mathbf{x}$ constructed by stacking $x^{[i]}$, $\forall i\in\{1\colon n\}$.

II Related Work

WiFi or radio signal fingerprint-based indoor positioning has become the standard approach for commercial applications [4, 5, 17, 18, 19]. Such systems provide accuracies from $1-10\mathop{\mathrm{m}}$ while they require an offline survey of the radio signal strength map. Furthermore, these systems enforce strong assumptions such as static environment and limited or slow user movements. For a recent survey see [18] and references therein. Another common technique is the angle of arrival estimation using a phased array. In [20], multiple WiFi access points compute angle of arrival information and aggregate them to estimate the client’s location. Angle of arrival estimation is also used for localizing RFID transponders [21], but only in areas measuring a few meters. Utilizing a phased array to determine the angle of arrival is challenging for smartphone-based hardware devices as the orientation of the antennas is unknown and not accessible.

On the other end of the spectrum, Pedestrian Dead Reckoning (PDR) is a popular technique which senses user motion to perform navigation. In PDR systems, either IMU sensors are handheld [22, 23] or mounted on different body parts, e.g. foot mounted [24], or torso-waist mounted [25]. Unfortunately, these systems cannot offer a generalized solution and, for example, a person in a wheelchair cannot benefit from them.

In [26, 27, 28], to overcome the drawbacks encountered when using Radio Frequency (RF) or IMU sensors individually for localization, the combination of both RF and IMU sensors has been used. In [28], PDR and WiFi fingerprinting is fused. Beacon scans occasionally correct the PDR drift. It appears that the PDR results dominate this approach with WiFi fingerprinting and Beacon scans to correct the drift. In [26], a step-detection strategy is used as the motion model of an Extended Kalman Filter (EKF) while WiFi signals provide measurements. EKF is a single hypothesis filter and cannot solve the global localization or the so-called kidnapped robot problem [12, page 274]. Furthermore, the radio signal propagation is more likely to follow a log-normal distribution than a Gaussian [6, 7, 9].

In this work, we bring the advances in Simultaneous Localization And Mapping (SLAM) [29] to efficiently solve the indoor localization and tracking problem using sensors available in smart handheld devices. The main features that distinguish this work from the available radio signal-based indoor positioning literature are as follows. We develop an adaptive (online) system that does not require the tedious process of fingerprinting (site survey); hence, our approach is more scalable. We use the underlying dynamical system of the IMU sensor as the motion model, and our system is robust to outliers and occasional lack of informative observations, due to the multi-hypothesis nature of the proposed Front-end. Moreover, the system is robust to the error in the map of beacons due the SLAM back-end.

III Problem Formulation

Let $\mathbf{x}_{t}\in\mathop{\mathrm{SE}(3)}$ be the device/robot pose at time $t$ which consists of a rotation matrix $\mathbf{R}_{t}\in\mathop{\mathrm{SO}(3)}$ and a position vector $\mathbf{p}_{t}\in\mathop{\mathbb{R}^{3}}$; and $\dot{\mathbf{p}}_{t}\in\mathop{\mathbb{R}^{3}}$ denotes the corresponding velocity. The device is initially located at $\mathbf{x}_{0}$ which is unknown. Let $\mathcal{L}\triangleq\{\mathbf{l}^{[j]}\in\mathop{\mathbb{R}^{3}}|j=1:n_{l}\}$ be a set of BLE beacons locations where an informative prior over any $\mathbf{l}^{[j]}$ is given. Let $\mathcal{Z}_{t}\subset\mathop{\mathbb{R}_{\geq 0}}$ be the set of possible range measurements (converted RSSI) at time $t$. The probabilistic measurement model $p(z_{t}|\mathbf{x}_{t},\mathbf{l})$ is a Gaussian conditional probability distribution that represents the likelihood of range measurements. The IMU and magnetometer measurements at time $t$ include a vector of angular velocity $\boldsymbol{\omega}_{t}\in\mathop{\mathbb{R}^{3}}$ and an acceleration vector $\mathbf{a}_{t}\in\mathop{\mathbb{R}^{3}}$, and a vector of local magnetic field $\mathbf{m}_{t}\in\mathop{\mathbb{R}^{3}}$, respectively.

Furthermore, the control action $\mathbf{u}_{t}\in\mathcal{U}_{t}$ and the action set $\mathcal{U}_{t}$ have to be defined possibly based on the IMU and magnetometer measurements. Note that the purpose of control actions here is the prediction of the device motion rather than actively controlling its behavior. Therefore, here, control actions are proprioceptive measurements. The ultimate goal is to estimate the device initial pose (global localization) and trajectory including its position and orientation, given noisy range, IMU, and magnetometer measurements. To this end, we break the original problem into the following sub-problems.

Problem 1** (Probabilistic motion model)**

Given the action set $\mathcal{U}_{t}$ and $\mathbf{u}_{t}\in\mathcal{U}_{t}$, find the discrete stochastic dynamics that describes transition equation $p(\mathbf{x}_{t}|\mathbf{x}_{t-1},\mathbf{u}_{t-1})$.

Problem 2** (Global localization and tracking)**

Let $z_{1:t}\triangleq\{z_{1},...,z_{t}\}$ be a sequence of range measurements up to time $t$. Let $\mathbf{x}_{t}$ be a Markov process of initial distribution $p(\mathbf{x}_{0})$ and transition equation $p(\mathbf{x}_{t}|\mathbf{x}_{t-1},\mathbf{u}_{t-1})$. Given $p(z_{t}|\mathbf{x}_{t},\mathbf{l})$, estimate recursively in time the posterior distribution $p(\mathbf{x}_{0:t}|z_{1:t},\mathbf{l})$.

Problem 3** (Radio-inertial SLAM)**

Given all measurements up to time $t$, estimate the joint smoothing distribution $p(\mathbf{x}_{0:t},\mathbf{l},\boldsymbol{\theta}_{1:t}|z_{1:t},\mathbf{u}_{1:t})$; where $\boldsymbol{\theta}_{1:t}$ are the, possibly time-varying, system calibration parameters.

We solve Problem 1 by developing the probabilistic IMU motion model and embed it into the SIR filter as the proposal distribution to solve Problem 2. In Problem 3, we solve the SLAM problem to estimate the smoothing distribution of the device trajectory, the optimized map, as well as time-varying IMU bias. We note that solving the SLAM problem has the following advantages:

We relax the need for an exact prior map of beacons and, given sufficient range measurements, any misalignment in the beacon placement can be recovered. 2. 2.

Through joint parameter and state estimation, we can recover system calibration parameters in a computationally affordable manner.

Remark 1

The magnetometer sensor is almost available on all smart handheld devices alongside the IMU. However, it is possible to remove magnetometer measurements from the problem formulation, while the problem remains solvable. In this case, the attitude estimation accuracy might degrade.

IV System Overview and Architecture

The proposed localization system is suitable for GPS-denied indoor environments and consists of two main layers which we call them Front-end and Back-end. The entire system architecture is shown in Figure 2. The necessary modules can be split up into four parts; in the following, we describe each module.

IV-A Sensors and Raw Signals

Standard and commercially available smartphones typically are equipped with Bluetooth transceiver, IMU, and magnetometer sensors. This module receives raw signals. The BLE operates in the $2.4\mathop{\mathrm{GHz}}$ license-free band and uses 40 channels each with a width of $2\mathop{\mathrm{MHz}}$; the ideal sampling rate of RSSI is about $10\mathop{\mathrm{Hz}}$ [17]. The IMU sensor includes a 3-axis accelerometer, which measures the linear acceleration, and a 3-axis gyroscope, that measures the angular velocity, and typically has a sampling rate higher than $100\mathop{\mathrm{Hz}}$. The magnetometer measures the strength and direction of the local magnetic field and has a sampling rate of about $50\mathop{\mathrm{Hz}}$.

IV-B Signal Processing and Attitude Estimation

In this module, we apply median filter to IMU and magnetometer signals to remove noise. However, the BLE signals depend on the availability of a link between the beacon and the receiver; as such, the filtering is only possible when there is a sufficient sequence of the RSSI. After this pre-processing step, we use the simplified free space path-loss model to convert RSSI to range measurements. For details of this step see [9]. In this work, we clamp range measurements by discarding any range value greater than $10\mathop{\mathrm{m}}$. This is the known effective range of the BLE technology [30, 9].

Using IMU and magnetometer measurements, we solve the Attitude and Heading Reference System (AHRS) [31, 32] problem to estimate the device orientation. Ideally, the magnetometer senses the direction of the Earth gravitational field which provides the North-East-Down coordinate system. However, if the local gravitational field is affected by external disturbances such as the structure of the building, this reference system might not be valid anymore and needs to be verified. Upon availability of the Back-end module, as shown in Figure 2, AHRS can compensate IMU bias which improves the orientation estimation.

IV-C Front-end

In the Front-end we use the sequential Monte-Carlo or PF technique known as SIR filter to solve the problem of global localization and tracking the device trajectory. Given the current device orientation, we sample from the IMU dynamics to generate particles along the moving direction. Given a prior map of the BLE beacons and using range measurements converted from BLE RSSI, we compute importance weights of particles. In this work, we use a range-only measurement model with additive white Gaussian noise. Finally, in the Resampling step particles with higher weights are replicated and all weights are set uniformly. The Sample, Importance, and Resampling steps are repeated sequentially.

Since the IMU has a higher frequency than the BLE receiver, the AHRS solver runs with a higher frequency to use all available measurements which in turn improves the orientation estimation accuracy. We sample from the IMU motion model using the latest device orientation with a frequency that is adaptable to the available computational resources (usually the BLE RSSI sampling rate). In this way, without discarding any sensory information, we can track the device within the Particle Filter framework.

IV-D Back-end

The Back-end includes an incremental optimization (smoothing) algorithm to minimize the error in Front-end estimates as well as estimation of the system calibration parameters. Let $\mathcal{X}_{t}\triangleq(\mathbf{x}_{t},\dot{\mathbf{p}}_{t},\mathbf{l},\boldsymbol{\theta}_{t})$ be the state variables tuple at any time step $t$. Let $\mathcal{D}_{t}$ be the set of all range measurements at time step $t$ and all IMU measurements from time step $t-1$ to $t$. The joint probability distribution of the SLAM problem, Problem 3, by assuming the measurements are conditionally independent and are corrupted by additive zero mean white Gaussian noise can be written as follows [33]:

[TABLE]

[TABLE]

Given that $p(\mathcal{X}_{0:T}|\mathcal{D}_{1:T})\propto p(\mathcal{X}_{0})\prod_{t=1}^{T}p(\mathcal{D}_{t}|\mathcal{X}_{t})$, the maximum-a-posteriori estimate of $\mathcal{X}_{0:T}$ can be computed by solving the following nonlinear least-squares problem:

[TABLE]

where $\mathbf{r}_{t}$ denotes the residual term which is the error between measurements and their corresponding nonlinear models, i.e. range-only measurement model and preintegrated IMU measurement model as described in [2]; $\boldsymbol{\Sigma}_{0}$ and $\boldsymbol{\Sigma}_{r}$ denote the corresponding measurement noise covariances.

Therefore, the optimization algorithm simultaneously solves for time-varying IMU bias, the feature map of BLE beacons, and the entire device trajectory. The resultant trajectory is smooth and continuous, unlike PF output, due to the motion constraints enforced by the IMU measurements. Furthermore, the estimated IMU bias is fed to the AHRS algorithm to improve the attitude estimation; and the optimized BLE map is used in the measurement update step of the SIR filter in the Front-end. This closed-loop architecture improves the Front-end performance as we will see in Section VI. In this work, we use iSAM2 algorithm [15] and the GTSAM library [16] as the Back-end solver.

V Probabilistic IMU Motion Model

In this section, we describe the proposed simplified method to incorporate the IMU dynamics into the PF that does not require sampling from the full 6-dimensional state (pose). In general, sampling methods tend to become inefficient and computationally intensive as the dimension of the state grows. The key idea is to sample the tangential acceleration magnitude along the direction of movement and evolve the state estimate using a discrete-time stochastic dynamical system that corresponds to the IMU dynamics.

At any time $t$, using the AHRS solver the current orientation estimate, $\hat{\mathbf{R}}_{t}$, is given. Therefore, by knowing the orientation, the system dynamics can be expressed using the following linear system:

[TABLE]

where $\bar{\mathbf{x}}_{t}\triangleq\mathrm{vec}(\mathbf{p}_{t},\dot{\mathbf{p}}_{t})\in\mathbb{R}^{6}$ is the state vector (of Front-end), and $\mathbf{F}_{t}\in\mathbb{R}^{6\times 6}$ and $\mathbf{G}_{t}\in\mathbb{R}^{6\times 6}$ are system and input matrices, respectively, and using a sampling time, $t_{s}$, can be derived as follows.

[TABLE]

We can compute the current tangential acceleration direction, $\hat{\mathbf{a}}_{t}$, as follows.

[TABLE]

[TABLE]

where $\tilde{\mathbf{a}}_{t}$ is the acceleration vector in the global coordinates after compensating the gravity $\mathbf{g}$. Let $\zeta_{t}\sim\mathcal{N}(\boldsymbol{0},\sigma_{a})$ be the sampled tangential acceleration magnitude where $\sigma_{a}$ is a sufficiently large linear acceleration standard deviation that covers a typical range of activities from slow walking to sudden movements. Furthermore, to maintain the diversity of the particles, let $\boldsymbol{\nu}_{t}\sim\mathcal{N}(\boldsymbol{0},\sigma_{v}\mathbf{I}_{3})$ be the sampled velocity vector with the isotropic covariance matrix, $\sigma_{v}\mathbf{I}_{3}$, that perturbs the position of the particles. We can then construct the input vector, $\mathbf{u}_{t}\in\mathbb{R}^{6}$, as $\mathbf{u}_{t}=\mathrm{vec}(\zeta_{t}\hat{\mathbf{a}}_{t},\boldsymbol{\nu}_{t})$.

Therefore, we addressed Problem 1 using the proposed stochastic dynamical system that can serve as the transition equation $p(\bar{\mathbf{x}}_{t}|\bar{\mathbf{x}}_{t-1},\mathbf{u}_{t-1})$. The proposed transition equation enables PF algorithm to track the device trajectory by predicting its motion. To address Problem 2 and 3, we use the described Front-end and Back-end, respectively. We then integrate all modules into a unified closed-loop system (Figure 2).

Remark 2

We note that upon availability of the Back-end for online system calibration task, the proposed IMU motion model, and therefore the Front-end can benefit from it. Consequently, the acceleration bias, $\boldsymbol{\theta}_{a,t}$, can be included in (6), i.e., $\tilde{\mathbf{a}}_{t}=\hat{\mathbf{R}}_{t}(\mathbf{a}_{t}-\boldsymbol{\theta}_{a,t})+\mathbf{g}$. However, as we will show later in the presented evaluations, the model at its basic form does not depend on this parameter to provide comparable results as it is used for the sampling purpose.

One might consider sampling $\zeta_{t}$ from a normal distribution centered at the current measured acceleration magnitude. However, we empirically observed that this method degrades the performance of the system as the measured magnitude, without calibration, is not reliable. Another reason to sample from a zero mean distribution is the fact that the variance can be sufficiently large to cover all possible values. Theoretically speaking in particle filtering, a sampling strategy is well-behaved as long as the proposal distribution’s support includes that of the true posterior distribution. Therefore, the claim for better tracking using the proposed simplified IMU model is connected to the fact that it provides a tighter support while still covers the support of the posterior distribution. Finally, we note that the proposed probabilistic IMU motion model follows the standard IMU dynamics as discussed in [2]. However, by providing the orientation, we propose a sampling strategy that is more computationally attractive for online applications.

VI Experimental Results and Discussion

In this section, we evaluate the proposed system using hardware experiments. In the first experiment, the focus is on the localization problem where an accurate map of the beacons is given. We compare localization techniques using PF with the random walk motion model (labeled as RW), PF embedded with the proposed probabilistic IMU motion model (labeled as IMU), PF with the robot dead reckoning motion model (labeled as DR), and our proposed closed-loop system including the Front-end (labeled as F.-end) and the Back-end (labeled as B.-end). We note that the Front-end is the same as PF with the probabilistic IMU motion model and the improvement in the results is due to the proposed closed-loop architecture and exploiting the Back-end for online system calibration. In the second experiment, using the same experimental dataset, we run a Monte Carlo simulation to study the effect of map error on the performance of each system.

VI-A Experimental Setup

We employ a wheeled mobile robot equipped with a Google Tango device [13] which provides the robot trajectory estimate using visual-inertial odometry. We use Tango’s output as a proxy for ground truth. The mobile robot is also equipped with a Nexus6P smartphone111https://www.google.com/nexus/6p/ which collects BLE, IMU, and magnetometer sensors data. The data is published as Robot Operating System (ROS) [34] topics through a customized Android application developed in-house. The IMU and magnetometer sensors are sampled at $200\mathop{\mathrm{Hz}}$ and $50\mathop{\mathrm{Hz}}$, respectively. The BLE scans are sampled at $10\mathop{\mathrm{Hz}}$; however, in practice, we experienced an average sampling rate of $7\mathop{\mathrm{Hz}}$. The environment is populated with BLE beacons, as shown by the Bluetooth logo in Figure 1. Furthermore, the prior map accuracy and the density of BLE beacons are sufficiently high to make initialization and tracking possible. The experiments are conducted in a research office environment partitioned into separate offices cabins and consists of traditional office furniture. The data is collected by maneuvering the robot over $100\mathop{\mathrm{m}}$ in an office space of $40\times 50\mathop{\mathrm{m}}^{2}$. Note that data is collected in a natural setting on a normal working day and in presence of office staff members with no movement restrictions to staff members 222The dataset is available upon request. Please contact the authors..

VI-B System Configuration and Initialization

To detect the degeneracy of PF, we calculate the effective sample size, $n_{eff}=(\sum_{i=1}^{n_{p}}w_{t}^{[i]})^{-1}$, and perform (systematic) resampling when $n_{eff}<n_{thr}$; where $n_{p}$ is the number of particles, $w_{t}^{[i]}$ is the $i$-th particle’s weight, and $n_{thr}$ is a threshold $1<n_{thr}<n_{p}$. The explanation and details of the used parameters are summarized in Table I; and the random walk (constant velocity with random input) motion model is explained in [9]. The DR motion model assumes a constant incremental movement along the AHRS estimated direction at each time step. This is because the wheel encoder data is not available in smartphones; however, this model does not maintain the diversity of the particles. Therefore, we introduce small velocity noise to perturb the position of the particles. The velocity noise standard deviation is set to the smallest value that leads to achieving comparable results.

Our empirical observation showed that slight variations of the parameters, about $5-10\%$, do not alter the trend of the results; however, finding suitable parameters for all motion models and the path-loss model is an important part of the setup. In particular, the path-loss model parameters can be estimated as explained in [9]. We tuned all motion models parameters manually, except the acceleration standard deviation which can be justified by considering that the average walking speed is about $1.5\mathop{\mathrm{m}}\sec^{-1}$ and a person can reach that speed in one second. The velocity standard deviation is tuned as the secondary motion model parameter to ensure the diversity of the particles are maintained. We note that this is an important factor in all particle filtering frameworks. The first $1-2\sec$ is sufficient for all the compared techniques to initialize, i.e., global localization. The particles are drawn uniformly within the known map area. The robot starts from the bottom left corner and moves upwards.

We clamp range measurements by discarding any range value greater than $10\mathop{\mathrm{m}}$. This is the effective range of the BLE technology [30]. However, even using a smaller range does not prevent receiving non-line-of-sight RSSI measurements. In general, it is difficult to deal with non-line-of-sight observations in a computationally attractive manner, and this is the main challenge in radio signal-based indoor positioning. Therefore, we set the unusually large range standard deviation of $5\mathop{\mathrm{m}}$ for maximum range of $10\mathop{\mathrm{m}}$ to enforce the fact that range measurements are highly noisy and inaccurate.

VI-C Localization Comparison under Accurate Map

In this experiment, given an accurate map of the BLE beacons, we compare the performance of the localization techniques. We evaluate the results from $100$ independent runs on the collected dataset. Figure 3 shows the summary of results for all techniques. The evaluation consists of Root Mean Square Error (RMSE) to show the performance of each technique as well as the empirical cumulative distribution function (CDF) of the position and orientation error. The position error is computed using the Euclidean norm of the three-dimensional position error. The orientation error is computed as the misalignment angle between the ground truth rotation matrix, $\mathbf{R}_{\mathrm{gt}}$, and the estimated rotation matrix, $\hat{\mathbf{R}}$, using $\lVert\log(\mathbf{R}_{\mathrm{gt}}^{\mathsf{T}}\hat{\mathbf{R}})\rVert_{\mathrm{F}}$, where $\log(\mathbf{R})$ computes the matrix logarithm. The empirical CDF is an unbiased estimate of the population CDF and is a consistent estimator of the true CDF. Note that faster rise from zero to one along the vertical axis is a desirable outcome.

The proposed probabilistic IMU motion model improves the localization accuracy in all scenarios as it is a better proposal distribution to sample from. This model can predict the robot/device motion using IMU measurements, therefore, the drawn samples are more likely to be near the actual robot pose. Furthermore, the proposed radio-inertial localization and tracking system improves the overall system performance by decreasing the localization error.

However, the random walk motion model also provides comparable results and show that, given an accurate map, it can be a simple yet useful motion model. This model essentially explores the entire state space without any knowledge of actual actions. The DR motion model is based on counting motion increments; therefore, it cannot generalize the motion of the device. It is worth mentioning that by increasing the velocity noise variance in DR, the motion model’s behavior approaches that of the random walk.

VI-D Effect of Map Perturbation and Radio-inertial SLAM

In the second experiment, we study the effect of large errors in the prior map of BLE beacons. We use a Monte Carlo simulation over $100$ independent runs using the same collected dataset. In each run, the location of each beacon is randomly perturbed by drawing noise from $\mathcal{N}(\mathbf{0}_{3},3\mathbf{I}_{3})$. This means the beacon can be located from near its initial position to $3\times 3\mathop{\mathrm{m}}$ away from its true position along each axis. Figure 4 shows the position and orientation RMSE evolution over the experiment time as well as the statistical summary of position RMSE using a boxplot. Figure 5 shows the CDF plots of the position and orientation errors. The curves show the median of $100$ independent runs.

By definition, localization algorithms assume such a prior map is perfect; therefore, the expectation is to observe lower performance. However, in the closed-loop system, where the Back-end solves the SLAM problem, the Back-end simultaneously solves for the beacons’ map and the trajectory, while jointly estimating the system calibration parameters. The feedback to the Front-end results in better localization and tracking outcome from the PF (Front-end). Consequently, the more accurate position and velocity estimates from the Front-end provide better initial values for the Back-end optimization. Moreover, the proposed system does not discard any measurements and fuses all available data which reduces the estimation error.

Nevertheless, this test confirms that the proposed system, as expected, is robust to the error in the map. This property is highly desirable in practice since the exact measurements of the map can be challenging, or the map can be modified over time.

VI-E Discussion and Limitations

The qualitative results on a larger dataset is shown in Figure 6. The robot starts from the bottom left corner and moves upwards. In this experiment, there are no beacons on the top and right sides of the rectangular path. Therefore, the system relies on its tracking ability. The error increases when beacons are sparse, as seen in Figure 6. The algorithm recovers the robot location as soon as the robot reaches near BLE beacons at about $(25,10)$ horizontal and vertical coordinates, respectively. Each beacon has a unique MAC address which solves the data association problem. Therefore, assuming there are sufficient beacons along the path, the proposed localization and tracking system error on average is fixed. A video showing the results is available on here: https://www.youtube.com/watch?v=kEDGSnFvz8A

VII Conclusion and Future Work

We studied and developed a localization and tracking system that performs real-time and is robust to map errors. We developed a suitable motion model (proposal distribution) for the sequential Monte-Carlo algorithms that exploits the IMU dynamics to constrain the samples while improving the tracking ability. The proposed system has a closed-loop architecture and uses all available measurements for estimation.

Future work includes joint state and parameters estimation of radio signal measurements (RSSI factors) as done for IMU factors to extend the ranging to more than $10\mathop{\mathrm{m}}$. This approach enables the system to accept any type of RSSI signals, e.g. WiFi or BLE, regardless of the transmission power. Adopting the idea in [9] for robust observation selection can also be a step towards improving the system robustness as well as increasing the ranging to more than $10\mathop{\mathrm{m}}$. Finally, we think a visual-radio-inertial SLAM technique is an attractive research direction to follow. The radio signals nicely complement visual measurements, while using visual data increases the estimation accuracy significantly.