Hardware and software design of BMW system for multi-floor localization

Abstract

Although the Micro Electro Mechanical System (MEMS) sensors are capable of providing short-term high positioning accuracy, every positioning result significantly depends on the historical ones, which inevitably leads to the long-term error accumulation. The Bluetooth Low Energy (BLE) is independent of the accumulative error, but the positioning accuracy is suffered by the irregular jump error resulted from the Received Signal Strength Indicator (RSSI) jitter. Considering the requirement of accurate, seamless, and consecutive positioning by the existing commercial systems, we propose a new integrated BLE and MEMS Wireless (BMW) system for multi-floor positioning. In concrete terms, first of all, the way of fingerprint database construction with the reduced workload is introduced. Second, the fingerprint database is denoised by the process of affinity propagation clustering, outlier detection, and RSSI filtering. Third, the robust M estimation-based extended Kalman filter is applied to estimate the two-dimensional coordinates of the target on each floor. Finally, the barometer data are used to calculate the height of the target. The extensive experimental results show that the proposed system can not only restrain the accumulative error caused by the MEMS sensors but also eliminate the irregular jump error from the BLE RSSI jitter. In an actual multi-floor environment, the proposed system is verified to be able to achieve the Root Mean Square (RMS) positioning error within 1 m.

1 Introduction

At present, the indoor positioning has broad application such as searching the cars and elevators in underground parking lot and pushing advertisements or discounts to the customers in large shopping malls. The Global Navigation Satellite System (GNSS) [1–3] well meets the precision requirement of outdoor positioning, but in indoor environment, its performance may drastically deteriorate due to the serious signal blocking and multipath effect. In response to this compelling problem, a batch of researchers put forward a variety of indoor positioning systems based on the Bluetooth Low Energy (BLE) [4], Ultra Wideband (UWB) [5], Radio Frequency Identification (RFID) [6], Micro Electro Mechanical System (MEMS) sensors [7], and Wireless Local Area Network (WLAN) [8, 9].

The hardware cost, range limitation, and long-term error accumulation limit the development of the conventional positioning systems. Among them, the MEMS sensors can be used to perform the Pedestrian Dead Reckoning (PDR) [7, 10] by using the inertia and heading information though it contains the accumulative error. The BLE Received Signal Strength Indicator (RSSI)-based positioning system can meet the requirement of low power, low cost, and no accumulative error though the RSSI jitter caused by multipath effect may seriously decrease the positioning accuracy.

In recent decade, various integrated systems have been significantly concerned to improve positioning accuracy [11, 12]. The systems [13–15] using Kalman and particle filter to achieve the WLAN/MEMS fusion positioning reduce the accumulative error of MEMS sensors, but they fail to constrain the WLAN RSSI jitter. To solve the RSSI jitter problem, the authors in [16] build the observation equation of the RSSI according to the signal propagation model, which is difficult to be constructed in the complex indoor environment. In [17], the authors design an indoor positioning system by fusing the WLAN and Magnetic Angular Rate and Gravity (MARG) data to solve the problems of RSSI jitter and accumulative error. However, this system cannot achieve the three-dimensional positioning, which is significantly required by the current commercial applications.

This paper presents a new integrated BMW system for multi-floor positioning. First of all, the fingerprint database is optimized by using the affine propagation clustering, outlier detection, and RSSI filtering algorithms. Specifically, the fingerprint denoising is performed to reduce the probability of large errors of BLE positioning, and then, the gait detection approach is used to estimate the walking speed and heading angle of the target based on the extended Kalman filter (EKF). Second, the robust EKF is designed to restrain the accumulative error caused by the MEMS sensors, as well as eliminate the irregular jump error from the BLE RSSI jitter. Finally, according to the barometer data and geographical location information, the height of the target is calculated to achieve the multi-floor positioning.

2 Related work

In this section, we will briefly introduce some related work in three aspects of fast fingerprint database construction, fusion positioning, and multi-floor positioning and also address the corresponding limitations.

Fast fingerprint database construction Although the fingerprint positioning has been studied for a long time, it still cannot be applied widely since its offline phase generally spends a huge amount of time on fingerprint database construction. By using the conventional approaches, the target area is calibrated with a batch of equally spaced grids, and then, the fingerprints are collected at the grids point-by-point [18, 19]. The involved time cost rises significantly with the increase of environmental size, which hinders the development of fingerprint positioning. In [20], the authors propose to construct an incomplete fingerprint database with realistic coverage gaps, and meanwhile study the performance of several interpolation and extrapolation approaches used for recovering the missing fingerprints. In [21], according to the distribution of Reference Points (RPs) with respect to each Wi-Fi Access Point (AP), the signal propagation model is constructed as a function of spatial structure, which can be used to construct the fingerprint database quickly.

Fusion positioning At present, the common single positioning systems are difficult to adapt to the complex indoor environment. In response to this compelling problem, a variety of fusion positioning systems are designed to compensate for the shortcomings of each single one. In [22], the data fusion from both the proprioceptive and exteroceptive sensors, like the odometer, Global Positioning System (GPS), Light Detection and Ranging (LIDAR), and vision, as well as the knowledge of road map is considered to perform the fusion positioning. In [23], the authors conduct data fusion by integrating the RSSI and Time difference of Arrival (TDOA) measurements to estimate the superior locations of the target. Specifically, by employing the nonparametric estimation approach, which is robust to the variations of measurement noise and quantization, it is addressed that the fusion positioning is more robust and higher accurate and has lower implementation cost.

Multi-floor positioning The mainstream of indoor positioning systems mainly focuses on the horizontal coordinate estimation, whereas little research has been done on the vertical coordinate estimation. In [24], the authors propose a WiFi-based indoor positioning system that takes both the characteristics of trilateration and scene analysis into account. The authors in [25] rely on the path loss model to construct a light fingerprint radio map to find the target floor. However, these systems are applied to only determine the floor on which the target is most probably located, but they cannot estimate the accurate locations of the target.

3 System description

3.1 System framework

As shown in Fig. 1, our system contains four modules, BLE fingerprint positioning module, speed and heading calculation module, EKF module, and height calculation module. According to the output of accelerometer, gyroscope, and magnetometer, the gait detection and quaternion calculation are carried out to estimate the walking speed and heading angle of the target, which will be sent to the PDR to perform localization. Then, the localization results by the PDR and Weighted K Nearest Neighbor (WKNN) are selected as the input of the EKF to obtain the two-dimensional coordinates of the target. After that, based on the output of barometer, two-dimensional positioning result, and geographical location information, the height of the target is inferred.

Fig. 1

Framework of BLE/MEMS fusion positioning system

3.2 BLE fingerprint positioning

BLE fingerprint positioning includes two phases, offline phase and online phase. The main tasks of offline phase are the coordinate calibration and fingerprint database construction with respect to the target environment. In addition, the affinity propagation clustering, outlier detection, and RSSI filtering are carried out to eliminate the large errors as well as optimize each sub-database. The main task of online phase is to match the newly collected RSSI data against the fingerprint database to obtain the positioning result. This process is shown in Fig. 2.

Fig. 2

Process of BLE fingerprint positioning

3.2.1 Fast fingerprint database construction

The data in the conventional fingerprint database are collected point-by-point, which limits the application of fingerprint positioning due to the requirement of huge time cost. In response to this compelling problem, we propose a new fast fingerprint database construction approach. First of all, the floor plan of the target environment is imported into the terminal as a map. Second, the map is represented by a batch of straight lines (labeled with dash blue lines) with the starting and ending points (labeled with red triangles), as shown in Fig. 3.

Fig. 3

Floor plan imported into the terminal

Supposing that the pixel size of the map is m _mapx by m _mapy and the actual size of the target environment is M _x by M _y , we can obtain the relations of the pixels and actual locations in (1).

$$ \left\{ {\begin{array}{*{20}{c}} {{X_{i}} = {x_{\text{map}{_{i}}}}*\frac{{{M_{x}}}}{{{m_{\text{map}}}}} + {X_{0}}}\\ {{Y_{i}} = {y_{\text{map}{_{i}}}}*\frac{{{M_{y}}}}{{{m_{\text{map}}}}} + {Y_{0}}} \end{array}} \right. $$

(1)

where $({X_{0}},{Y_{0}})\phantom {\dot {i}\!}$ is the actual origin location, which is represented by (0,0) in the map. $\phantom {\dot {i}\!}({x_{\text {map}{_{i}}}},{y_{\text {map}{_{i}}}})$ and $({X_{i}},{Y_{i}})\phantom {\dot {i}\!}$ are the coordinates of the ith point in the map (with the length m _mapx and width m _mapy ) and actual environment (with the length M _x and width M _y ) respectively.

We construct the standard fingerprint database by

$$ \left\{ {\begin{array}{*{20}{c}} {{X_{i}} = {X_{0}} + {L_{\text{step}\_x}}*ii \in 1,{\mathrm{}} \cdots,{\mathrm{}}\frac{{\left| {{X_{\text{end}}} - {X_{0}}} \right|}}{{{L_{\text{step}\_x}}}}}\\ {{Y_{j}} = {Y_{0}} + {L_{\text{step}\_y}}*jj \in 1,{\mathrm{}} \cdots,{\mathrm{}}\frac{{\left| {{Y_{\text{end}}} - {Y_{0}}} \right|}}{{{L_{\text{step}\_y}}}}} \end{array}} \right.{\mathrm{}} $$

(2)

where L _{step_x} and L _{step_y} are the physical intervals in the X and Y directions, respectively, in the actual environment, which are normally set as L _{step_x} =L _{step_y} .

By taking the walking path from (x ₀,y ₀) to (x _end,y _end) (in Fig. 3) as an example, a volunteer starts walking after labeling (x ₀,y ₀) on the screen of the terminal and then labels (x _end,y _end) on the screen of the terminal when he/she stops walking. During the walk, (x _i ,y _i ) is notated as the ith location, which is calculated by

$$ \left\{ {\begin{array}{*{20}{c}} {{x_{i}} = {x_{0}} + \sum \limits_{n = 1}^{i} {v_{n}}*\sin ({\theta_{n}}) + {\varepsilon_{xi}}}\\ {{y_{i}} = {y_{0}} + \sum \limits_{n = 1}^{i} {v_{n}}*\cos ({\theta_{n}}) + {\varepsilon_{yi}}} \end{array}} \right. $$

(3)

where v _n and θ _n are the walking speed and heading. ${\varepsilon _{xi}} = \frac {{{v_{i}}\sin ({\theta _{i}})}}{{\sum {{v_{i}}\sin ({\theta _{i}})} }}(L\sin ({\Psi _{1}}) - {L_{pdr}}\sin ({\Psi _{2}}))$ and ${\varepsilon _{yi}} = \frac {{{v_{i}}\cos ({\theta _{i}})}}{{\sum {{v_{i}}\cos ({\theta _{i}})} }}(L\cos ({\Psi _{1}}) - {L_{pdr}}\cos ({\Psi _{2}}))$ are the error compensation in the X and Y directions respectively. L and ${L_{pdr}} = \sum \limits _{i = 1}^{N} {{v_{i}}}$ are the actual distance and the estimated one by the PDR from the starting to ending locations. Ψ ₁ is the heading of (x _end,y _end) relative to (x ₀,y ₀). Ψ ₂ is the heading of (x _N ,y _N ) relative to (x ₀,y ₀).

Since the RSSI in standard fingerprint database may be lost when the walking speed is too fast, we propose to rely on the interpolation approach to estimate the lost RSSI by propagation modeling, as shown in Fig. 4.

Fig. 4

RSSI interpolation

We use the propagation model below to describe the relations of the RSSI and distance from each anchor to the terminal.

$$ {P_{d}} = - 10{N_{t}}\log d + {P_{{d_{0}}}} $$

(4)

where ${P_{{d_{0}}}}$ is the RSSI with 1-m distance from the anchor. d is the distance from the anchor to terminal. N _t is the path loss exponent.

Figure 5 shows two groups of test points indicating the variation of RSSI with respect to the distance from an anchor to the terminal. As can be seen from Fig. 6, the constructed propagation model matches the variation of RSSI well when the distance is within 10 m.

Fig. 5

Variation of RSSI with respect to the distance

Fig. 6

Result of propagation modeling

We randomly select N _m terminal locations to train the path loss exponent for each anchor by

$$ {\small{\left\{ {\begin{array}{*{20}{c}} {{P_{{d_{1}}}} = - 10{N_{1}}\log {{\left\| {{D_{1}} - {D_{0}}} \right\|}_{2}} + {P_{{d_{0}}}}}\\ {{P_{{d_{2}}}} = - 10{N_{2}}\log {{\left\| {{D_{2}} - {D_{0}}} \right\|}_{2}} + {P_{{d_{0}}}}}\\ \vdots \\ {{P_{{d_{i}}}} = - 10{N_{i}}\log {{\left\| {{D_{i}} - {D_{0}}} \right\|}_{2}} + {P_{{d_{0}}}}}\\ \vdots \\ {{P_{{d_{m}}}} = - 10{N_{m}}\log {{\left\| {{D_{m}} - {D_{0}}} \right\|}_{2}} + {P_{{d_{0}}}}} \end{array}} \right.}} $$

(5)

where the notation “ ∥∙∥₂” represents the 2-norm operation. D ₀ is the anchor location. D _i is the ith location of the terminal. ${P_{{d_{i}}}}$ is the RSSI at D _i . We calculate $\overline N = {{\sum {{N_{i}}}} / m}$ as the path loss exponent. Thus, the lost RSSI at D _i+1 is estimated by

$$ {P_{{d_{i + 1}}}} = - 10\overline N \log {\left\| {{D_{i + 1}} - {D_{0}}} \right\|_{2}} + {P_{{d_{0}}}} $$

(6)

3.2.2 Outlier detection

We rely on the affinity propagation clustering [26] to classify the off-line RSSI data and then construct each cluster as a sub-database, as shown in Table 1. In online phase, we match the newly collected RSSI data to the cluster center of each sub-database and then select the sub-database with the best matching to perform positioning. To achieve this goal, we define density(x,k) and relative_density(x,k) as the density and relative density [27] of the newly collected RSSI x with respect to its k nearest neighbors respectively.

$$ {\text{density}}(x,k) = {\left({\frac{{\sum\limits_{y \in N(x,k)} {{\text{distance}}(x,y)} }}{{\left| {N(x,k)} \right|}}} \right)^{- 1}} $$

(7)

Table 1 Pseudo-code of sub-database construction

$$ {\text{relative\_density}}(x,k) = \frac{{{\text{density}}(x,k)}}{{\sum\limits_{y \in N(x,k)} {\frac{{{\text{density}}({\mathrm{y}},k)}}{{\left| {N(x,k)} \right|}}} }} $$

(8)

where distance(x,y) is the distance between x and y. N(x,k) is the set of k nearest neighbors with respect to x. |N(x,y)| is the number of RSSI data in N(x,k).The pseudo-code of sub-database construction is shown in Table 1.

c l u s t e r _i is the ith cluster. point _j is the jth point. Threshold is the threshold of relative density.

3.3 Speed and heading estimation

We integrate the data from the 3-axis accelerometer, 3-axis gyroscope, and 3-axis magnetometer to estimate the walking speed and heading of the target (Fig. 7). First of all, the median and mean filters are fused to eliminate the device noise. Second, the low-pass filtering is performed on the output of 3-axis accelerometer to detect the gait and consequently estimate the walking speed. Finally, the data from the 3-axis accelerometer, 3-axis gyroscope, and 3-axis magnetometer are combined with the posture matrix [28] to estimate the heading.

Fig. 7

Process of speed and heading estimation

3.3.1 Speed estimation

We calculate the step size of the kth step by ${P_{k}} = {C_{Sk}}\sqrt [4]{{{A_{\text {norm}{_{\max }}}} - {A_{\text {norm}{_{\min }}}}}}$, where the maximum and minimum acceleration respectively are ${A_{\text {norm}{_{\max }}}}\phantom {\dot {i}\!}$ and $\phantom {\dot {i}\!}{A_{\text {norm}{_{\min }}}}$. The parameter C _Sk is trained by using the Back Propagation (BP) neural network [29, 30]. Specifically, the step frequency F _Sk and height ${H_{{S_{k}}}}$ are selected as the input and C _Sk is selected as the output to train the coefficients V _ij and W _j . The number of neurons on hidden layer is 11, as shown in Fig. 8.

Fig. 8

Structure of BP neural network

With the pedestrian walking, the output of accelerometer ${A_{\text {norm}}}\; = \sqrt {a_{{x_{b}}}^{2} + a_{{y_{b}}}^{2} + a_{{z_{b}}}^{2}}$ generally changes in sinusoid mode [28], where ${a_{{x_{b}}}}$, ${a_{{y_{b}}}}$, and ${a_{{z_{b}}}}$ are the output of accelerometer in X, Y, and Z directions respectively. A step is detected when A _norm reaches a local maximum higher than a given threshold A ₀. By setting the sampling frequency as f _s and number of samples between two adjacent maximum as Δ N, the time duration of the kth step is ${t_{k}} = \frac {\Delta N}{f_{s}}$ and the corresponding estimated speed is

$$ {v_{k}} = \frac{P_{k}}{t_{k}} = \frac{{P_{k}}{f_{s}}}{\Delta N} $$

(9)

3.3.2 Heading estimation

We define the carrier and geographic reference coordinate systems as x-y-z and ENU, where E, N, and U axes point to the geographic east, north, and sky directions, respectively, as shown in Fig. 9.

Fig. 9

Geographic reference coordinate system

The coordinate transformation from the carrier to geographic reference coordinate systems is described as

$$ \left[ \begin{array}{l} {x_{b}}\\ {y_{b}}\\ {z_{b}} \end{array} \right] = T_{n}^{b}(q)\left[ \begin{array}{l} {x_{n}}\\ {y_{n}}\\ {z_{n}} \end{array} \right] $$

(10)

where $T_{n}^{b}(q)$ is the posture matrix. q=q ₀+q ₁ i+q ₂ j+q ₃ k is the quaternion. ${\left [ {\begin {array}{*{20}{c}} {{x_{b}}}&{{y_{b}}}&{{z_{b}}} \end {array}} \right ]^{T}}$ and ${\left [ {\begin {array}{*{20}{c}} {{x_{n}}}&{{y_{n}}}&{{z_{n}}} \end {array}} \right ]^{T}}$ are the coordinates of the target in the carrier and geographic reference coordinate systems respectively. According to the relations of the posture matrix and quaternion [31], we have

$$ \begin{array}{l} {T_{n}^{b}(q)} \\ = \left[\begin{array}{cc} {{q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2}}}&{2({q_{1}}{q_{2}} + {q_{0}}{q_{3}})}\\ {2({q_{1}}{q_{2}} - {q_{0}}{q_{3}})}&{q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2}}\\ {2({q_{1}}{q_{3}} + {q_{0}}{q_{2}})}&{2({q_{2}}{q_{3}} - {q_{0}}{q_{1}})}\\ \end{array}\right.\\ \left.\begin{array}{c} {2({q_{1}}{q_{3}} - {q_{0}}{q_{2}})}\\ {2({q_{2}}{q_{3}} + {q_{0}}{q_{1}})}\\ {q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}}\\ \end{array}\right] \end{array} $$

(11)

Using the rigid body angular differential equation, we obtain

$$ \frac{{dq}}{{dt}} = \frac{1}{2}q \otimes w $$

(12)

where w=0+w _x i+w _y j+w _z k is the angular velocity rotation quaternion. w _x , w _y , and w _z are the angular velocities in X, Y, and Zdirections in the carrier coordinate system. The notation “ ⊗” represents the quaternion multiplication, which is also used in [31]. We rewrite the formula above in matrix form as

$$ \begin{array}{l} \frac{{dq}}{{dt}}\\ = \frac{1}{2}\left[ {\begin{array}{*{20}{c}} \begin{array}{l} 0\\ {w_{x}}\\ {w_{y}}\\ {w_{z}} \end{array}&\begin{array}{l} - {w_{x}}\\ 0\\ - {w_{z}}\\ {w_{y}} \end{array}&\begin{array}{l} - {w_{y}}\\ {w_{z}}\\ 0\\ - {w_{x}} \end{array}&\begin{array}{l} - {w_{z}}\\ - {w_{y}}\\ {w_{x}}\\ 0 \end{array} \end{array}} \right]\\ \quad \times \left[ \begin{array}{l} {q_{0}}\\ {q_{1}}\\ {q_{2}}\\ {q_{3}} \end{array} \right]{\mathrm{= }}\frac{1}{2}\Omega (w)\vec q \end{array} $$

(13)

We discretize the formula above as

$$ \left\{ \begin{array}{l} {{\vec q}_{m + 1}} = \left(I + \frac{1}{2}\Omega \left(w{T_{s}}\right)\right){{\vec q}_{m}},\;m = 0,{\mathrm{}}1,{\mathrm{}} \cdots \\ {{\vec q}_{0}} = \vec q(0) \end{array} \right. $$

(14)

where T _s is the sampling interval. I is the unit matrix. $\vec q(0)$ is the initial quaternion. m is the index of samples. We normalize the quaternion as

$$ {\vec q_{m}} = \frac{{\vec q}_{m}}{\left\| {{{\vec q}_{m}}} \right\|} = \frac{{\vec q}_{m}}{\sqrt {q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2}}} $$

(15)

The posture matrix can be updated based on the quaternion obtained in (12). However, the quaternion may not be accurate due to the error of the gyroscope. To solve this problem, we construct the quaternion equations of status and observation as follows.

$$ \left\{ \begin{array}{l} {{\vec q}_{m + 1}} = {A_{m}}{{\vec q}_{m}} + {W_{m}}\\ {Y_{m + 1}} = h({{\vec q}_{m + 1}},{V_{m + 1}}) \end{array} \right. $$

(16)

where ${A_{m}} = \left (I + \frac {1}{2}\Omega \left (w{T_{s}}\right)\right)$ is the transition matrix of status. W _m is the process noise. V _m+1 is the observation noise. The observation variable Y _m+1 is calculated by

$$ \begin{array}{l} {Y_{m + 1}} = \left[ \begin{array}{l} {a_{m + 1}}\\ {c_{m + 1}} \end{array} \right]\\ \qquad\;\;= \left[ \begin{array}{l} \begin{array}{*{20}{c}} {T_{n}^{b}({{\vec q}_{m + 1}})}&0 \end{array}\\ \begin{array}{*{20}{c}} 0&{T_{n}^{b}({{\vec q}_{m + 1}})} \end{array} \end{array} \right]\left[ \begin{array}{l} g\\ L \end{array} \right] + {V_{m + 1}} \end{array} $$

(17)

where ${a_{m + 1}} = {\left [ {\begin {array}{*{20}{c}} {{a_{{x_{b}}}}}&{{a_{{y_{b}}}}}&{{a_{{z_{b}}}}} \end {array}} \right ]^{T}}$ and ${c_{m + 1}} = {\left [ {\begin {array}{*{20}{c}} {{c_{{x_{b}}}}}&{{c_{{y_{b}}}}}&{{c_{{z_{b}}}}} \end {array}} \right ]^{T}}$ are the output of accelerometer and magnetometer, respectively, in the carrier coordinate system. $g = {\left [ {\begin {array}{*{20}{c}} 0&0&1 \end {array}} \right ]^{T}}$is the normalized output of accelerometer when the target is static in the geographic reference coordinate system. $L = {\left [ {\begin {array}{*{20}{c}} 0&{{b_{y}}}&{{b_{z}}} \end {array}} \right ]^{T}}$ is the output of magnetometer in the geographic reference coordinate system, in which ${b_{y}} = \sqrt {c_{{x_{\mathrm {b}}}}^{2} + c_{{y_{b}}}^{2}}\phantom {\dot {i}\!}$ and $\phantom {\dot {i}\!}{b_{z}} = {c_{{z_{\mathrm {b}}}}}$. Based on (14), we can calculate that

$$ \begin{array}{l} {\Phi_{m + 1}} = \frac{{\partial h({{\vec q}_{m + 1}},{V_{m + 1}})}}{{\partial {{\vec q}_{m + 1}}}}\\ \qquad\; = \left[\begin{array}{cc} {{\mathrm{- }}2{q_{2}}}&{2{q_{3}}}\\ {2{q_{1}}}&{2{q_{0}}}\\ {2{q_{0}}}&{{\mathrm{- }}2{q_{1}}}\\ {2({q_{3}}{b_{y}} - {q_{2}}{b_{z}})}&{2({q_{2}}{b_{y}} + {q_{3}}{b_{z}})}\\ {2({q_{0}}{b_{y}} + {q_{1}}{b_{z}})}&{ - 2({q_{1}}{b_{y}} - {q_{0}}{b_{z}})}\\ { - 2({q_{1}}{b_{y}} - {q_{0}}{b_{z}})}&{ - 2({q_{0}}{b_{y}} + {q_{1}}{b_{z}})}\\ \end{array}\right.\\ \qquad\; \left.\begin{array}{cc} {{\mathrm{- }}2{q_{0}}}&{2{q_{1}}}\\ {2{q_{3}}}&{2{q_{2}}}\\ {{\mathrm{- }}2{q_{2}}}&{2{q_{3}}}\\ {2({q_{1}}{b_{y}} - {q_{0}}{b_{z}})}&{2({q_{0}}{b_{y}} + {q_{1}}{b_{z}})}\\ {2({q_{2}}{b_{y}} + {q_{3}}{b_{z}})}&{ - 2({q_{3}}{b_{y}} - {q_{2}}{b_{z}})}\\ {2({q_{3}}{b_{y}} - {q_{2}}{b_{z}})}&{2({q_{2}}{b_{y}} + {q_{3}}{b_{z}})}\\ \end{array}\right] \end{array} $$

(18)

Finally, the heading of the target is estimated by

$$ \varphi = \arctan \left(- \frac{{2({q_{1}}{q_{2}} + {q_{0}}{q_{3}})}}{{q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2}}}\right) $$

(19)

3.4 Robust EKF

3.4.1 EKF

We choose the geographic east and north directions, walking speed, and heading to construct the equation of status below.

$$ \begin{array}{l} {X_{t}} = f({X_{t - 1}},{W_{t - 1}})\\ \quad\;= \left[ \begin{array}{l} 1\;\;0\;\;\sin ({\varphi_{t - 1}})\;\;0\\ 0\;\;1\;\;\cos ({\varphi_{t - 1}})\;0\\ 0\;\;0\;\;\;\;\;1\;\;\;\;\;\;\;\;0\\ 0\;\;0\;\;\;\;\;0\;\;\;\;\;\;\;1 \end{array} \right] \times \left[ \begin{array}{l} {E_{t - 1}}\\ {N_{t - 1}}\\ {v_{t - 1}}\\ {\varphi_{t - 1}} \end{array} \right] \\ \quad\;+ {W_{t - 1}} \end{array} $$

(20)

where X _t =[E _t N _t v _t φ _t ]^T. E _t−1 and N _t−1 are the geographic east and north directions, respectively, at moment t−1. v _t−1 and φ _t−1 are the walking speed and heading, respectively, in the geographic reference coordinate system at moment t−1. W _t−1 is the Gaussian white noise with zero mean at moment t−1. $E\left [ {{W_{i}}W_{j}^{\mathrm {T}}} \right ] = Q(i,j){\delta _{ij}},\;\;i,j = 1, \cdots,o$, in which o is the number of estimation variables, δ _ij is the Kronecker function, and Q is the covariance matrix of process noise.

The output of BLE fingerprint positioning and the estimated speed and heading are selected as the observation variable to construct the equation of observation below.

$$ \begin{array}{l} {Z_{t}} = h({X_{t}},{V_{t}})\\ \quad\;= \left[ \begin{array}{l} 1\;\;0\;\;0\;\;0\\ 0\;\;1\;\;0\;\;0\\ 0\;\;0\;\;1\;\;0\\ 0\;\;0\;\;0\;\;1 \end{array} \right] \times \left[ \begin{array}{l} {E_{t}}\\ {N_{t}}\\ {v_{t}}\\ {\varphi_{t}} \end{array} \right] + {V_{t}} \end{array} $$

(21)

where ${Z_{t}} = {\left [ {E_{t}^{\text {ble}}\;N_{t}^{\text {ble}}\;v_{t}^{\text {mems}}\;\varphi _{t}^{\text {mems}}} \right ]^{\mathrm {T}}}$. $E_{t}^{{\text {ble}}}$ and $N_{t}^{{\text {ble}}}$ are the geographic east and north directions by the BLE fingerprint positioning at moment t. $v_{t}^{\text {mems}}$ and $\varphi _{t}^{\text {mems}}$ are the estimated speed and heading, respectively, by using the MEMS sensors at moment t. V _t is the Gaussian white noise with zero mean at moment t. E[V(i)V ^T(j)]=R(i,j)δ _ij , i,j=1,⋯,n, in which n is the number of observation variables and R(R>0) is the covariance matrix of observation noise.

3.4.2 Robustness enhancement

Considering that the change of the weighting factors of the IGG3 [32] is slight, we first define the IGG3 weight function below to enhance the robustness of the EKF. We first define the IGG3 weight function below.

$$ \begin{array}{l} {{\bar p}_{i}} = \left\{ \begin{array}{l} {p_{i}}\\ {p_{i}}\frac{{{k_{0}}}}{{\left| {{u_{i}}} \right|}}{\left(\frac{{{k_{1}} - \left| {{u_{i}}} \right|}}{{{k_{1}} - {k_{0}}}}\right)^{2}}\\ 0 \end{array} \right.\\ \begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {{k_{0}}\begin{array}{*{20}{c}} < \end{array}} \end{array}} \end{array}\begin{array}{*{20}{c}} {\left| {{u_{i}}} \right| \le {k_{0}}}\\ {\left| {{u_{i}}} \right| \le {k_{1}}}\\ {\left| {{u_{i}}} \right| > {k_{1}}} \end{array} \end{array} $$

(22)

where p _i and ${\bar p_{i}}$ are the ith diagonal elements in the weight matrix P=R ⁻¹ and equivalence weight matrix $\bar P$ respectively. k ₀∈[1,1.5] and k ₁∈[2.5,3] are the weights. The normalized residual vector u _i is calculated by

$$ {u_{i}} = \frac{v_{i}}{\sigma_{v_{i}}} $$

(23)

where v _i is the ith element in the n×1-dimensional residual vector of observation V. $\sigma _{v_{i}} = \frac {\sigma _{0}}{\sqrt {q_{v_{i}}}}$, in which $q_{v_{i}}$ is the reciprocal of v _i and σ ₀ is the variance factor [32].

Then, we apply the M estimation approach to enhance the robustness of the EKF. The main difference between the conventional EKF and robust one is the updating of filter gain, as shown below.

$$ \begin{array}{l} \bar K\left(t \right) = P\left({t,t - 1} \right){H^{\mathrm{T}}}\left(t \right) \times \\ \qquad\quad {\left[ {H\left(t \right)P\left({t,t - 1} \right){H^{\mathrm{T}}}\left(t \right) + \bar R\left(t \right)} \right]^{- 1}} \end{array} $$

(24)

where $\bar K\left (t \right)$ is the filter gain at moment t. P(t,t−1) is the one-step prediction of the matrix of error covariance at moment t. H(t) is the matrix of observation at moment t. $\bar R\left (t \right) = \bar B_{t}^{- 1}$ is the covariance matrix of observation noise, in which ${\bar B_{t}}$ is the equivalent weight matrix.

3.5 Height estimation

3.6 Pressure measure

The principle of barometric pressure measure is that the atmospheric pressure in the gravitational field decreases as the height increases [33]. Thus, the altitude of the target can be estimated by using the barometric pressure measured by the barometer. When the height of gravity, H, is less than 11 km, the standard pressure formula can be described as

$$ H = 44330.76\left[1 - {\left(\frac{{{P_{s}}}}{{101.325}}\right)^{0.190255}}\right] $$

(25)

where P _s is the pressure value.

Finally, the height of the target is estimated by

$$ h = \frac{{R \times H}}{{R - H}} $$

(26)

where R=6356766 m is the radius of earth. Since R≫H, we can obtain $\frac {R}{{R - H}} \approx 1$ and h≈H.

3.7 Algorithm design

The walking pattern in indoor environment can be simply divided into walking flat, upstairs, and downstairs. In our system, we rely on the output of barometer to determine the walking pattern of the target and then count the number of steps to estimate the height of the target, as shown in Table 2. In this table, h _t−1 and h _stair stand for the height of the target at moment t and step height respectively.

Table 2 Pseudo-code of height estimation

3.8 System implementation

Figure 10 shows the schematic diagram of the proposed system. The BLE anchor broadcasts the packets in a real-time manner. A smartphone equipped with the BLE and MEMS modules is selected as the target. Both the BLE and MEMS data are packaged and transmitted to the positioning server through the 4G network, and then, the estimated locations of the target are displayed by the server, as shown in Fig. 11.

Fig. 10

Schematic diagram of the proposed system

Fig. 11

Interface of positioning server

3.8.1 Hardware platform

The CC2540 chip of Texas Instruments (TI) is selected as the BLE anchor with the broadcast frequency 10 Hz and power 0 dBm, as shown in Fig. 12.

Fig. 12

BLE anchors

The target is installed with the Android 6.0 operating system and designed to support the BLE, accelerometer, magnetometer, and barometer modules. The positioning and web servers are based on the Windows 7 operating system with the processor of Intel (R) Core (TM) i3-416 CPU @ 3.60 GHz, as shown in Fig. 13.

Fig. 13

Hardware configuration

3.8.2 Software platform

The APP used to collect the data from the BLE and MEMS sensors as well as access the positioning server is developed by Android Studio software, and the corresponding algorithms are written on the Eclipse software platform in JAVA (Figs. 14 and 15). The MySQL database and Apache Web server are selected to save the positioning result and show them through the PHP and JavaScript (Fig. 16).

Fig. 14

Interface of Android Studio software

Fig. 15

Interface of Eclipse software platform

Fig. 16

Interface of database management

4 Experimental results

Two adjacent floors in a building are selected as the experimental environment, as shown in Figs. 17 and 18. There are 11 BLE anchors, namely Anchor 1,⋯,11, fixed in target environment and the 514 RPs (marked with bullet) are uniformly calibrated with the interval of 0.6 m. Figure 19 shows some photos of the actual environment. The positioning result can be displayed on both the terminal and server sides, as shown in Figs. 20 and 21.

Fig. 17

Floor plan of the lower floor

Fig. 18

Floor plan of the upper floor

Fig. 19

Photos of the actual environment

Fig. 20

Display on terminal side

Fig. 21

Display on server side

Figures 22 and 23 show the result of fingerprints clustering without and with denoising. The process of denoising increases the probability of merging the physically adjacent RPs into the same cluster.

Fig. 22

Fingerprints clustering without denoising

Fig. 23

Fingerprints clustering with denoising

Although the irregular jump error of BLE positioning may exceed 10 m, the ultimate error by fusion positioning will converge into a small value as the time going on. By taking the result in Fig. 24 as an example, the error of initial positioning is over 13 m, but the one almost decreases to less than 2 m when the timestamp is over 100 s.

Fig. 24

Error variation of fusion positioning

We continue to compare the performance of the proposed and conventional positioning algorithms by using the MEMS or BLE solely. Figure 25 shows the result of locations tracking by different algorithms on a floor. The real trajectory starts from point A, along Anchor 3 and Anchor 6, along Anchor 4 and Anchor 3, and back to point A (see Fig. 25).

Fig. 25

Locations tracking on a floor

The result of locations tracking on two different floors is also shown in Fig. 26. In this test, the real trajectory is selected from point A, along Anchor 3 and point B, going upstairs to point C, along Anchor 8 and Anchor 7, and ending at point D (see Figs. 26 and 27).

Fig. 26

Results in 3D view

Fig. 27

Projection result in Y-Z plane view

As can be seen from Fig. 26, the large accumulative error of MEMS positioning exists, while the BLE positioning is suffered by the irregular jump error and cannot be accurate enough when the target is located in the staircase. In contrast, the proposed fusion positioning algorithm can restrain the accumulative error of MEMS positioning as well as eliminate the irregular jump error of BLE positioning. In addition, it is also verified that the proposed algorithm is featured with good height resolution, which makes the system more robust to the actual indoor multi-floor positioning.

Figures 27 and 28 show the Cumulative Density Function (CDF) of errors and the error at each test point respectively. From these figures, we can find that the positioning errors by using the BLE or MEMS solely are much larger than the one of the proposed fusion positioning. In addition, different percentile errors of fusion, BLE, and MEMS positioning are also illustrated in Table 3.

Fig. 28

CDF of positioning errors

Table 3 Different percentile values with respect to the positioning error

Figure 29 shows the height error of each test point when the target is located in the staircase. From this figure, we can find that our system is capable of achieving better height resolution compared to the conventional ones using the MEMS or BLE solely.

Fig. 29

Positioning error at each test point

Finally, we investigate the stability of our system under the long-term testing. To achieve this goal, three shapes (square, linear, and irregular shapes) of trajectories are considered in Figs. 30, 31, 32, 33, 34, 35, and 36 respectively. Obviously, our system exhibits the best performance in terms of accumulative error constraint, irregular jump error elimination, and height resolution under different shapes of trajectories.

Fig. 30

Height error at each test point

Fig. 31

Locations tracking under square shape of trajectory

Fig. 32

CDF of positioning errors under square shape of trajectory

Fig. 33

Locations tracking under linear shape of trajectory

Fig. 34

CDF of positioning errors under linear shape of trajectory

Fig. 35

Locations tracking under irregular shape of trajectory

Fig. 36

CDF of positioning errors under irregular shape of trajectory

5 Conclusions

In this paper, both the hardware and software of BMW system are designed and implemented for indoor multi-floor positioning. Based on the extensive experimental results, it is demonstrated that the proposed system is capable of solving the problems of accumulative error constraint and irregular jump error elimination in MEMS and BLE positioning respectively. In general, our system is featured with high positioning accuracy, good height resolution, and strong long-term stability.