A Real-Time BLE/PDR Integrated System by Using an Improved Robust Filter for Indoor Position

Abstract

Indoor position technologies have attracted the attention of many researchers. To provide a real-time indoor position system with high precision and stability is necessary under many circumstances. In a real-time position scenario, gross errors of the Bluetooth low energy (BLE) fingerprint method are more easily occurring and the heading angle of the pedestrian will drift without acceleration and magnetic field compensation. A real-time BLE/pedestrian dead-reckoning (PDR) integrated system by using an improved robust filter has been proposed. In the PDR method, the improved Mahony complementary filter based on the pedestrian motion states is adopted to estimate the heading angle reducing the drift error. Then, an improved robust filter is utilized to detect and restrain the gross error of the BLE fingerprint method. The robust filter detected the gross error at different granularity by constructing a robust vector changing the observation covariance matrix of the extended Kalman filter (EKF) adaptively when the application is running. Several experiments are conducted in the true position scenario. The mean position accuracy obtained by the proposed method in the experiment is 0.844 m and RMSE is 0.74 m. Compared with the classic EKF, these two values are increased by 38% and 18%, respectively. The results show that the improved filter can avoid the gross error in the BLE method and provide high precision and scalability in indoor position service.

1. Introduction

With the rapid development of technology and people’s increasing demands for a better life, various applications based on location-based service (LBS) provide a great convenience for people’s life. Position information is the key element for LBS to provide services. Global Navigation Satellite System (GNSS) can provide high accuracy position outdoors. However, the accuracy deteriorates significantly because GNSS signals are unreliable or blocked in indoor environments. To provide a reliable, stable position service in indoor environments, many types of indoor positioning technologies such as wireless fidelity (Wi-Fi) [1,2,3], Bluetooth low energy (BLE) beacons [4,5], radio frequency identification (RFID) [6,7], ultrasonic [8], infrared [9], ultra-wideband (UWB) [10,11], pseudolite [12,13], computer vision [14,15] had been proposed by experts and scholars.

Among them, the positioning techniques based on Wi-Fi are the most popular indoor positioning method due to the wide deployment of Wi-Fi routers in shopping malls, hospitals, airports, and stations. However, the process of Wi-Fi scanning is time-consuming and power-consuming. Many devices have optimized their systems to limit the scanning frequency. According to the latest google android development document [16], it is specified that the scanning frequency of Wi-Fi is limited no more than four times in two minutes after the Android Oreo system. It is difficult for the Wi-Fi-based positioning method to be applied to indoor position service applications with high real-time performance. Fortunately, Bluetooth-based techniques are another good choice to position due to the increased popularity of smartphones along with the development of the Internet of Things. Low cost, power-saving, and ease of deployment without affecting the infrastructures of buildings can avoid the shortcoming of Wi-Fi scanning [17]. Most importantly, the principle of the BLE positioning method is the same as that of Wi-Fi. Some of the current Wi-Fi positioning algorithms can be directly used for BLE positioning [18]. The fingerprint-based method of BLE is one of the popular approaches for the indoor position which can offer many advantages for the realizable position accuracy and is infrastructure-free. However, the radio signals of BLE can be affected by the multipath effect and device heterogeneity [19] which cause the signal to change during offline acquisition and online position, resulting in the error in the positioning process.

In short, all of these indoor position techniques mentioned above have both advantages and disadvantages. It is the focus of researchers to propose a fusion of indoor position methods to keep a balance among accuracy, coverage, cost, and complexity. Common fusion position methods include multimodal fingerprinting, triangulation-based fusion, and pedestrian dead reckoning (PDR)-based fusion [20]. The PDR-based fusion method which combines PDR with wireless localization methods is widely used in the literature. PDR is a self-positioning method that provides a relatively high accuracy position estimation based on the smartphone’s built-in sensors, but it suffers from the drift problem, resulting in huge cumulative error for long-time positioning [21]. By contrast, wireless positioning method such as BLE fingerprint position can obtain absolute position without cumulative error but has poor accuracy position estimation. Fusing PDR with wireless localization methods which are often known as Bayes filter, Kalman Filter (KF), Extended Kalman filter (EKF), or particle filter (PF) can make up for both methods’ shortcomings to provide high accuracy and stable position service. The PF has good performance in solving nonlinear problems but suffers from a high computational load. In our research, the fusion EKF algorithm was chosen to combine PDR with the BLE fingerprint position method in the real-time integrated system. To inhibit the outliers from the BLE fingerprint method, a robust filter based on EKF was proposed to compensate for the gross error in the real-time positioning procedure. Our contributions are as follows:

• We found that the errors of the BLE fingerprint method are not only related to the signal fluctuation but are also affected by scanning numbers of BLE beacons after statistically analyzing the real-time signal data in a harsh environment. When the scanning BLE beacon numbers are few, coarse errors will more likely occur;
• We found that the accuracy of the heading is also affected by the motion states of the pedestrian. An improved Mahony complementary filter is introduced to keep the heading angle stable by adaptively changing the control parameters in the filter after considering the different people’s motion states;
• To meet the demand of real-time position and considering the computational load of the smartphone, we adopt the EKF method to solve the nonlinear fusion problem to combine PDR with the BLE fingerprint position method to provide the real-time position service. To cope with the gross error caused by the BLE fingerprint method in a harsh environment, a robust filter based on the EKF was proposed. The robust filter detected the gross error at different granularity by constructing a robust vector changing the observation covariance matrix of the extended Kalman filter (EKF) adaptively when the application is running. The experimental results demonstrate that the proposed method has better performance at position accuracy and stability.

The remainder of this paper is organized as follows: Section 2 is about the related works of the BLE-based position, self-contained position, and fusion position algorithms in detail. Section 3 introduces the BLE fingerprint method and analyzes the error of the position method. PDR method and the heading estimation based on the motion state are also introduced in this section. Next, the fusion method EKF and a robust filter are presented as well. Finally, a diagram about the BEL/PDR integrated system localization framework is demonstrated. Section 4 describes the experiments and analyzes the results. Then the discussion of the paper and the conclusions are presented in Section 5 and Section 6.

2. Related Works

The BLE-based position systems have been widely used by utilizing the received signal strength (RSS) measurements. Zuo et al. pointed out that the radio signals of BLE had two features. One is that the signal can change dramatically in a small spatial change. The other is that the signal can be reported multiple times or not reported at all during a single scan. Both features will cause huge noises in both RSS and BLE beacon availability [22]. In addition to the features of the signal, multipath effect, device heterogeneity, and deployment are the sources of errors in the BLE fingerprint position [19,23]. To analyze the effects of dense deployment, Ng et al. proposed a high-resolution proximity detection using an adaptive scanning mechanism fusion with spontaneous differential evolution [24]. Tian et al. defined a coverage degree criterion by leveraging the Cramer Rao Lower Bound (CRLB) and the differential evolution algorithm to optimize the placement of Wi-Fi and BLE access points (APS) to hybrid two types of signals to fuse position based on the position performance analysis [23]. Andrew et al. applied three Bayesian filtering techniques to fit the BLE signal distance equation based on considering various errors and conduct comparison experiments to verify the significant modification in two environments [25]. Subhan et al. presented an in-depth experimental analysis of RSS and its effect on distance and position [26]. The position methods based on BLE and WiFi are the same, which can be divided into two categories: proximity detection, multilateration, and fingerprinting approaches. The research by Zhao et al. showed that under the same environment and conditions, the BLE-based position is more accurate than Wi-Fi because of its lower transmission power and unique channel hopping mechanism [27]. The key point of much research on proximity detection and multilateralism is a distance-based estimation [25,28]. Moreover, it is difficult to obtain an accurate distance model under complex environmental circumstances. The fingerprint method has become the popular approach in real-time positioning with many advantages of being infrastructure-free and easily realizable. RADAR was the first RSS-based fingerprint system that used a K-nearest neighbor (KNN) algorithm to estimate the location indoors [29]. Zuo et al. introduced an efficient and graph optimization-based way for estimating the beacon positions and the reference fingerprint map to combine range-based and fingerprint-based methods of BLE [21]. A self-adaptive weighted KNN algorithm PhaseFi proposed a deep network instead of the fingerprint database and estimated the position by a radial basis function probabilistic method [30].

The self-contained position method mainly contains two types: the data-driven inertial navigation method and the PDR method. The data-driven inertial navigation technology has been increasingly used in recent works which use the deep neural network with great potential in model-free generalization to regress pedestrian motion characteristics. It used inertial measurement units (IMU) in a short time and ground-truth motion trajectories to regress motion parameters (velocity and heading). Robust IMU double integration (RIDI) had made a breakthrough in coordinate frame normalization and used support vectors to regress a more accurate velocity vector [31]. IONet and RoNIN utilized trained neural networks to regress the magnitude of speed and the rate of heading angle change, showing the capability to obtain plain displacement [32,33]. ILIO demonstrated a network that regresses 3D displacement estimation and its uncertainty to tightly fuse the relative state measurement into a stochastic cloning EKF to solve for the pose, velocity, and sensor biases [34]. IDOL presented a two-stage, data-driven pipeline using a commodity smartphone that first estimates device orientations and then estimates device position to solve the problem of inaccurate orientation estimates [35]. Although the data-driven inertial navigation has a good performance in long-time tracking based on the sophisticated deep learning technology, it needs a large amount of data to train and extra equipment to get the ground-truth trajectory in advance, and for real-time positioning, the device will be under greater computational load. PDR position is a self-positioning method that consists of three key components of step detection, step length, and heading estimation that is a good choice for the real-time position. Lachapelle et al. proposed three-step length error models: Gaussian model, constant random model, and Gauss Markov model. He modeled the error of the gyroscope as a random constant deviation when establishing the PDR error model, so the heading error was considered to be linear with time [36]. Jahn et al. established the error models for four methods of measuring step length and discussed the systematic and random errors with the Taylor expansion [37]. You et al. proposed the multipoint positioning algorithm based on the received RSSI for calibration to eliminate the cumulative error existing in the traditional PDR system by analyzing the characteristics of walking postures [38].

For fusion methods, Li et al. [39] proposed an adaptive system noise EKF algorithm to develop an integrated Wi-Fi/PDR system. The proposed filter could determine the dynamic noise of the transition matrix according to the movement (straight or turning) of the pedestrian and reduce the computational complexity of the matching fingerprint database by using an affinity clustering algorithm. The positioning error could be reduced to 2.32 m by the experiment. Deng et al. [40] also presented a novel data fusion framework by using an EKF to integrate Wi-Fi localization with PDR. They developed a measurement model based on kernel density estimation to enable accurate Wi-Fi localization and adaptive measurement noise statistics estimation. The experiments show that the proposed method obtains comparable accuracy and greatly reduces computation cost compared with a particle filter. Atia et al. [41] utilized a grid-based nonlinear Bayesian filter algorithm to fuse the Wi-Fi, BLE, and inertial navigation system (INS) sensor information to develop a calibration-free hybrid indoor positioning methodology. The experiments demonstrated that the performance of the fusion method is much better than the BLE fingerprint position method. However, the harsh indoor environment will lead to coarse errors in the wireless localization method while different motion states of pedestrians affect the performance of PDR, which both affect the accuracy of the dynamic and observation models. Adaptive and robust filters can be employed to mitigate the effects of large errors in the dynamic and observation models, respectively. Yang et al. [42] proposed an adaptively robust filter based on a robust maximum-likelihood estimation to kinematic geodetic positioning and measurement. The method could not only balance the contribution between the updated parameters and measurement but also mitigated the influence of measurement outliers. Yang et al. [43] also presented an adaptively robust filter with multi adaptive factors based on the principles of the adaptive KF and bifactor robust estimations for correlated observations. The proposed filter is more flexible in controlling the disturbing effects of the state components compared to the classified adaptive factors. Chang et al. [44] proposed a robust KF using the Chi-squared test to detect measurement outliers. Li et al. [45] presented an adaptive and robust filter to combine the Wi-Fi and PDR information to develop an integrated system. The adaptive filter is based on scenario and motion state recognition and the robust filter is based on the Mahalanobis distance. The experiment results indicate that the proposed filter is better than the common EKF.

3. Materials and Methods

3.1. BLE Position Technology

The fingerprint position is mainly based on the similarity of the received signal strength (RSS) and the fingerprint database to obtain the position result. The RADAR system was the first RSS-based fingerprint system developed by Bahl et al. [29] for indoor localization by utilizing a KNN algorithm. Like any other fingerprint method, the BLE fingerprint position method consists of two stages: the offline data training stage and the online positioning stage. During the offline data training stage: people stand at the reference points (RPs) whose coordinates are known in advance to collect the RSS from the access points (APs) for some time. Then, the fingerprint database as shown in Equation (1) is constructed after computing the distribution of collected data. Suppose that there are $n$ RPs and $m$ APs in the position scenario.

$$\mathrm{Fingerprint} \mathrm{database}=\left[\begin{array}{cc}(x_1,y_1)& \langle {rssi}_1^1,{rssi}_2^1,{rssi}_3^1,\dots ,{rssi}_m^1\rangle \\ (x_2,y_2)& \langle {rssi}_1^1,{rssi}_2^1,{rssi}_3^1,\dots ,{rssi}_m^1\rangle \\ ⋮& ⋮\\ (x_n,y_n)& \langle {rssi}_1^1,{rssi}_2^1,{rssi}_3^1,\dots ,{rssi}_m^1\rangle \end{array}\right]$$

(1)

During the online positioning stage, the position of the target is obtained by matching the real-time fingerprint to the database on certain algorithms. There are many matching algorithms. Considering the efficiency of real-time positioning and the simplicity of implementation, the classic matching method is K-nearest Neighbor (KNN). The basic idea of the KNN algorithm is to classify the target into the nearest sample class in the feature space. For the BLE fingerprint position method, the feature space is the fingerprint database. The process of BLE fingerprint KNN algorithms is shown as follows:

Step 1: When online positioning is carried out, a real-time fingerprint is collected by smartphone. The real-time fingerprint is expressed as Equation (2).

$$\mathrm{Real} \mathrm{time} \mathrm{fingerprint}=\left[\langle rss{i}_1,rss{i}_2,\dots ,rss{i}_m\rangle \right]$$

(2)

Step 2: Then the Euclidean distance between the real-time fingerprint and fingerprint database can be calculated by the following Equation (3).

$$d_i=\sqrt{{(rss{i}_1-rss{i}_1^i)}^2+{(rss{i}_2-rss{i}_2^i)}^2+\cdots +{(rss{i}_m-rss{i}_m^i)}^2}$$

(3)

Step 3: Then we sort the $n$ distance $d_i$ in ascending order and choose the first K items to calculate the target position by averaging the K corresponding coordinates as shown in Equation (4).

$$\left(x,y\right)=\frac{1}{K}{\displaystyle \sum }_i^K\left(x_i,y_i\right)$$

(4)

The process of the fingerprint position method is shown in Figure 1.

In addition to the general fingerprint position process described above, BLE real-time fingerprint also has its features. When we carry out a real-time BLE fingerprint position, the BLE module broadcasts data at a certain frequency. The broadcast data include the identity number, module name, mac address, received signal strength indicator (RSSI), and other information. The broadcast frequency is usually from 10 nanoseconds to 10 s, and the default broadcast frequency is 500 milliseconds. The common android devices such as smartphones provide a function to scanning the BLE signal for real-time fingerprint position. Generally, the continuous positioning with a time interval of 1 s is regarded as the real-time position. Due to the influence of the scanning mechanism, the number of the scanning BLE RSSI is often different in 1 s. In addition to the impact of signal fluctuations, the number of the scanning BLE RSSI will also cause errors in fingerprint position.

Here are the real-time RSSIs from 54 BLE APs in one position scenario at certain RP collected by smartphone HUAWEI P20 in 60 s. The collected data per second correspond to a real-time fingerprint data, totaling 60 fingerprint data. Then we count the scanning RSSI number of each fingerprint and Figure 2 shows the scanning RSSI number of each fingerprint in detail with a bar chart.

As can be seen from Figure 2, there are differences in the scanning RSSI number per second. The scanning RSSI number per second is at least 1 and at most 37, with an average of 19.78 RSSI. The different number of the scanning RSSI directly affects the positioning accuracy of BLE fingerprint position. If the scanned number of APs is too small, it is easy to cause a coarse error in the BLE fingerprint position [46]. A robust filter is needed to restrain the coarse error.

3.2. PDR Technology

Pedestrian dead reckoning (PDR) is a self-positioning method for indoor navigation. The key technologies of PDR are step detection (or counting), step length estimation, and heading angle of pedestrian estimation. After a pedestrian step was detected, the position can be obtained using step length and heading angle based on the previous position [20]. The general process of PDR is illustrated as Equation (5):

$$\{\begin{array}{c}{N}_{k+1}=N_k+s_k·cos{\psi }_k\\ E_{k+1}=E_k+s_k·sin{\psi }_k\end{array}$$

(5)

where $N_k$ and $E_k$ refer to the coordinate of the pedestrian at the north and east direction at time $k$, respectively. $s_k$ is the step length and ${\psi }_k$ is the heading angle at time $k$.

In the PDR method, step detection and step length are estimated according to the accelerometer readings. Many step detection algorithms have been proposed by researchers, including peak detection [47], threshold setting [29], zero velocity update [48], auto-correlation [49] and finite-state machine (FSM) [50]. Among them, the FSM method is easy to implement and more resistant to interference from errors. After detecting a step, step length is estimated by different models. Studies have shown that the step length is related to the acceleration, heights, and strides of different people. Linear models [51], constant models [52], and nonlinear models [53] are the most common methods which are used to estimate the step length. The step length estimated by different methods differs little. For simplicity, we choose the Weinberg model [54] to estimate the step length in our research and the expression for the model is as follows in Equation (6):

$$s_k=K·\sqrt[4]{a_{max}-a_{min}}$$

(6)

where$K$ is the scale factor of the step length, $a_{max}$ and $a_{min}$ are the maximum and minimum acceleration in one step cycle.

Apart from step detection and step length estimation, heading estimate is another important component of PDR. The compass [55] or the gyroscope [56] are usually used to estimate the heading angle. Because of the inherent sensor noise in the smartphone, the accuracy of the heading obtained by the compass is not high but it will not drift for a long time. In contrast, using the gyroscope to estimate the heading, the accuracy in a short time will be high, but it will suffer a drift problem. We also found that different motion states of pedestrians would affect the accuracy of heading estimation. To avoid the shortcomings of compass and gyroscope and take into account the different motion states of people, an improved Mahony complementary filter (AMMCF) based on the motion states had been proposed in our research. The parameter $K_p$ in the filter can adaptively change based on the motion states which are judged according to the acceleration readings. The principle of PDR is illustrated in Figure 3.

3.3. An Improved Mahony Complementary Filter Based on the Motion States

The attitude of the device can be described by Euler angle, rotation matrix, and quaternion methods. The Euler angle is the angle between the three axes of the carrier coordinate system (CCS) and geographic coordinate system (GCS) which include pitch, roll, and yaw. The pitch, roll, and yaw represent rotation around x, y, and z axes as shown in Figure 4 and are denoted by the symbol $\theta$, $\gamma$ and$\psi$, respectively. The Euler angle method only needs three elements to store attitude information, which is simple, intuitive, and easy to understand, but it is easy to generate a universal joint deadlock phenomenon.

Different from the Euler angle, the Euler angle describes the attitude of the device at a certain moment, while the rotation matrix describes the motion process of the device rotation. In a GCS defined by the North-East-Up (NEU), the rotation matrix $C_n^b$ between the two systems can be defined in Equation (7):

$$C_n^b=C_n^{\gamma }{C}_n^{\theta }{C}_n^{\psi }=\left[\begin{array}{ccc}cos\gamma & 0& sin\gamma \\ 0& 1& 0\\ -sin\gamma & 0& cos\gamma \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & sin\theta \\ 0& -sin\theta & cos\theta \end{array}\right]\left[\begin{array}{ccc}cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]$$

(7)

where $C_n^b$ represents the rotation matrix from GCS to CCS, $C_n^{\psi }$,$C_n^{\theta }$,$C_n^{\gamma }$ represent the corresponding matrices for yaw, pitch, and roll in order. As we know, the matrix $C_n^b$ is the orthogonal matrix, so the rotation matrix $C_b^n$ is the inverse matrix of $C_n^b$, which represents the rotation matrix from CCS to GCS. The matrix $C_b^n$ can be defined as following Equation (8):

$$C_b^n={(C_n^b)}^T={(C_n^b)}^{-1}=\left[\begin{array}{ccc}cos\psi cos\gamma -sin\psi sin\theta sin\gamma & sin\psi cos\theta & cos\psi sin\gamma +sin\psi sin\theta cos\gamma \\ -sin\psi cos\gamma -cos\psi sin\theta sin\gamma & cos\psi cos\theta & -sin\psi sin\gamma +cos\psi sin\theta cos\gamma \\ -cos\theta sin\gamma & -sin\theta & cos\theta cos\gamma \end{array}\right]$$

(8)

Because the rotation matrix will suffer universal joint deadlock phenomenon and need 9 parameters to store attitude information, the quaternion is the most commonly used method to calculate the attitude. Assuming that the quaternion vector is $Q={\left[q_0,q_1,q_2,q_3\right]}^T$, the attitude angles expressed in quaternions are as follows in Equation (9):

$$\{\begin{array}{c}\psi =arctan\frac{2q_1{q}_2-2q_0{q}_3}{1-2q_1^2-2q_3^2},\psi \in \left(0,2\pi \right)\\ \theta =\arcsin \left(-2q_2{q}_3-2q_0{q}_1\right),\theta \in (-\frac{\pi }{2},\frac{\pi }{2})\\ \gamma =arctan\frac{-2q_1{q}_2+2q_0{q}_2}{1-2q_1^2-2q_2^2},\gamma \in \left(-\pi ,\pi \right)\end{array}$$

(9)

The Mahony complementary filter (MCF) [39] utilizes the gyroscope to calculate the attitude angle of the device and the accelerometer and magnetometer are used to complement the accumulated error. When a gyroscope raw data $\omega ={\left({\omega }_x,{\omega }_y,{\omega }_z\right)}^T$ was obtained, we can use the first-order Runger-Kutta method to obtain the quaternion update as shown in Equations (10) and (11):

$$Q\left[t+∆t\right]=Q\left[t\right]+∆t·{\mathsf{\Omega }}_w\left[t\right]·Q\left[t\right]$$

(10)

$${\left(\begin{array}{c}\begin{array}{c}{q}_0\\ q_1\end{array}\\ \begin{array}{c}{q}_2\\ q_3\end{array}\end{array}\right)}_{t+∆t}={\left(\begin{array}{c}\begin{array}{c}{q}_0\\ q_1\end{array}\\ \begin{array}{c}{q}_2\\ q_3\end{array}\end{array}\right)}_t+\frac{∆t}{2}·\left(\begin{array}{cc}\begin{array}{cc}0& -{\omega }_x\\ {\omega }_x& 0\end{array}& \begin{array}{cc}-{\omega }_x& -{\omega }_x\\ {\omega }_z& -{\omega }_y\end{array}\\ \begin{array}{cc}{\omega }_y& -{\omega }_z\\ {\omega }_z& {\omega }_y\end{array}& \begin{array}{cc}0& {\omega }_x\\ -{\omega }_x& 0\end{array}\end{array}\right)·{\left(\begin{array}{c}\begin{array}{c}{q}_0\\ q_1\end{array}\\ \begin{array}{c}{q}_2\\ q_3\end{array}\end{array}\right)}_t$$

(11)

where the Equation (11) is an expansion of the Equation (10), $Q\left[t\right]$ refers to the quaternion vector of time $t$, $∆t$ represents tiny time and is often assigned of sampling cycles, ${\mathsf{\Omega }}_w\left[t\right]$ is the corresponding matrix as shown in Equation (11), which is made up of gyroscope raw data at time $t$. As a result, we can obtain the latest quaternion vector by utilizing the gyroscope data based on the Runger-Kutta method. However, as time goes on, it will suffer a drift problem, resulting in huge cumulative error for long-time orientation. To reduce the drift problem, the acceleration and magnetometer field are used to compensate for the cumulative error. Supposed that the gyroscope error correction is $e={\left[e_x,e_y,e_z\right]}^T$, it can be defined as:

$$e=e_a+e_m$$

(12)

where $e_a$,$e_m$ are the error correction items calculated by accelerometer and magnetometer readings, respectively. They are expressed as $e_a={\left[e_{ax},e_{ay},e_{az}\right]}^T$ and $e_m={\left[e_{mx},e_{my},e_{mz}\right]}^T$, and can be obtained by Equation (13):

$$\{\begin{array}{c}{e}_a=a_n^b\times a\\ e_m=m_n^b\times m\end{array}$$

(13)

where $a$, $m$ are the normalized accelerometer and magnetometer readings. $a_n^b$, $m_n^b$ are the normalized vectors of gravity acceleration and magnetic field after matrix $C_n^b$ conversion. The symbol “$\times$” represents the vector cross product. After getting the error correction, the compensated gyroscope value ${\omega }^{\prime }=\left({\omega }_x^{\prime },{\omega }_y^{\prime },{\omega }_z^{\prime }\right)$ can be calculated based on the proportional-integral (PI) method as follows in Equation (14):

$${\omega }^{\prime }=\omega +K_p·e+K_i·{\displaystyle \int }e$$

(14)

where $K_p$, $K_i$ are the proportional and integral control parameters, respectively. The compensated gyroscope value ${\omega }^{\prime }$ is plugged into the quaternion differential Equation (11) and the quaternion is updated. Then the attitude is obtained by Equation (9) based on the updated quaternion.

In general, $K_p$, $K_i$ are fixed empirical values without considering the impact of pedestrian motion status. In our research, we proposed an improved MCF to change the control parameter $K_p$ and $K_i$ adaptively based on the pedestrian motion status. For simplicity, the pedestrian motion status can be divided into three types including static, walking, and running which can be judged by the standard deviation of triaxial acceleration modulus. The triaxial acceleration modulus can be calculated by Equation (15):

$$ac{c}_{mod}=\Vert acc\Vert =\sqrt{ac{c}_x^2+ac{c}_y^2+ac{c}_z^2}$$

(15)

where $acc$ and $\Vert acc\Vert$ refer to the acceleration vector and the corresponding modulus, respectively. Figure 5 shows the performance of the acceleration modulus on different motion statuses. The dotted green line is an artificial boundary between different motion states. The acceleration modulus is collected for 1 min at the frequency of 50 Hz. People in the first 20 s are in a static state, people in the middle 20 s are in a walking state, and people in the last 20 s are in a running state.

We set up a sliding window to store the acceleration modulus and calculate the standard deviation in the window as shown in Equations (16) and (17).

$${\mu }_{acc}=\frac{1}{n}·\sum _{i=1}^n{acc}_{{mod}_i}$$

(16)

$${\sigma }_{acc}=\sqrt{\frac{1}{n}·\sum _{i=1}^n{({acc}_{{mod}_i}-{\mu }_{acc})}^2}$$

(17)

where ${\mu }_{acc}$ is the average acceleration modulus of the sliding window in size $n$, ${\sigma }_{acc}$ refers to the standard deviation of the sliding window. Then, a moving average filter (MAF) is applied to process the ${\sigma }_{acc}$ value to classify the motion state. Figure 6 demonstrates that the standard deviation of the acceleration modulus is smoother and easier to distinguish after MAF. Therefore, ${\sigma }_{acc}$ can be used to judge different motion states of pedestrians.

In our research, we proposed an improved MCF to change the control parameters $K_p$ and $K_i$ adaptively based on the pedestrian motion status. The parameters and $K_i$ can be calculated by Equations (18) and (19):

$$K_p=\{\begin{array}{cc}{K}_p·\left({\sigma }_{acc}+1\right)& {\sigma }_{acc}<0.05 g\\ \frac{K_p}{\left({\sigma }_{acc}+1\right)}& 0.05 g<{\sigma }_{acc}<0.2 g\\ 0& 0.2g<{\sigma }_{acc}\end{array}$$

(18)

$$K_i=\{\begin{array}{cc}{K}_i·\left({\sigma }_{acc}+1\right)& {\sigma }_{acc}<0.05 g\\ \frac{K_i}{\left({\sigma }_{acc}+1\right)}& 0.05g<{\sigma }_{acc}<0.2 g\\ 0& 0.2 g<{\sigma }_{acc}\end{array}$$

(19)

The gyroscope is sensitive to changes in motion status which are often accompanied by changes in acceleration. The value of $0.2 g$ is approximately the average value of running which is almost the boundary from static, walking to fast motion. When the device is in a strenuous state, in which the acceleration mode is bigger than the value $g$, the attitude is mainly calculated by the gyroscope. The experiments show that the improved Mahony complementary filter based on the motion states has a better performance.

3.4. BLE/PDR Integrated System Based on EKF

PDR is a self-contained algorithm that can provide accurate position information at a short distance but it suffers accumulated errors. BLE fingerprint position accuracy is poor without cumulative error. The positioning result obtained by the BLE fingerprint method at present will not be affected by the previous result. To improve the positioning accuracy, continuity, and stability of the system, two positioning methods are often combined as an integrated system based on the Kalman filter (KF). Since the PDR position method is a non-linear algorithm, the Extended Kalman filter (EKF) is often utilized to replace the KF method to fuse two positioning methods. The hybrid position model mainly includes two models, one is the state transition model and the other is the observation model.

The state transition model is mainly to estimate the state vector which is composited of position coordinates, step length, and heading angle. The state vector is expressed by Equation (20):

$$X=\left[N,E,s,\psi \right]$$

(20)

The state transition model is expressed by Equation (21) at time $k$:

$$\{\begin{array}{l}\begin{array}{c} N_k=N_{k-1}+s_{k-1}·cos{\psi }_{k-1}+{\omega }_N\\ E_k=E_{k-1}+s_{k-1}·sin{\psi }_{k-1}+{\omega }_E\end{array}\hfill \\ \begin{array}{c}{s}_k=s_{k-1}+{\omega }_s\\ {\psi }_k={\psi }_{k-1}+{\omega }_{\psi }\end{array}\hfill \end{array}$$

(21)

where $N_k$ and $E_k$ are the position coordinates of the PDR position method in the north and east, respectively. $s_k$ and $\psi$ are the step length and the heading angle calculated by the PDR method at time $k$. Further, ${\omega }_N$,${\omega }_E$,${\omega }_s$,${\omega }_{\psi }$ are the corresponding process noise of the state vector. They conform to Gaussian distribution, and their variances are denoted by ${\delta }_N^2$,${\delta }_E^2$,${\delta }_s^2$,${\delta }_{\psi }^2$, respectively. The state transition matrix $A_k^{-}$ is expressed as Equation (22):

$$A_k^{-}=\left[\begin{array}{cc}\begin{array}{cc}1& 0\\ 0& 1\end{array}& \begin{array}{cc}cos{\psi }_{k-1}& 0\\ sin{\psi }_{k-1}& 0\end{array}\\ \begin{array}{cc}0& 0\\ 0& 0\end{array}& \begin{array}{cc}1& 0\\ 0& 1\end{array}\end{array}\right]$$

(22)

The Jacobi matrix $A_k$ of $A_k^{-}$ is expressed as Equation (23):

$$A_k=\left[\begin{array}{cc}\begin{array}{cc}1& 0\\ 0& 1\end{array}& \begin{array}{cc}cos{\psi }_{k-1}& -s_{k-1}·sin{\psi }_{k-1}\\ sin{\psi }_{k-1}& s_{k-1}·cos{\psi }_{k-1}\end{array}\\ \begin{array}{cc}0& 0\\ 0& 0\end{array}& \begin{array}{cc}1 & 0\\ 0 & 1\end{array}\end{array}\right]$$

(23)

The observation model is mainly to estimate the observation vector which is composited of position coordinates of BLE fingerprint position. The observation vector is expressed by Equation (24):

$$Z=\left[N^{\prime },E^{\prime }\right]$$

(24)

The observation model is expressed as follow (25):

$$\{\begin{array}{l}{N}_k^{\prime }=N_{k-1}^{\prime }+{\omega }_{N^{\prime }}\hfill \\ E_k^{\prime }=E_{k-1}^{\prime }+{\omega }_{E^{\prime }}\hfill \end{array}$$

(25)

where $N_k^{\prime }$ and $E_k^{\prime }$ are the position coordinates of the BLE fingerprint method in the north and east, respectively, ${\omega }_{N^{\prime }}$ and ${\omega }_{E^{\prime }}$ are the corresponding observation noise that conforms to Gaussian distribution and their variances are ${\delta }_{N^{\prime }}^2$, ${\delta }_{E^{\prime }}^2$. The observation matrix $H_k$ is expressed as Equation (26):

$$H_k=\left[\begin{array}{cc}\begin{array}{cc}1& 0\\ 0& 1\end{array}& \begin{array}{cc}0& 0\\ 0& 0\end{array}\end{array}\right]$$

(26)

When the state vector and observation vector are obtained by BLE fingerprint and PDR methods, respectively, the EKF estimation is employed to update the state parameters through time as well as the observation parameters. In the EKF, the process of prior estimation is expressed as follows:

$$X_k=A_k^{-}·{\hat{X}}_{k-1}$$

(27)

$$P_k^{-}=A_k·P_{k-1}·A_k^T+Q_k$$

(28)

The Gain matrix is expressed as (29):

$$G_k=P_k^{-}·H_k^T·\left(H_k·P_k^{-}·H_k^T+R_k\right)$$

(29)

Then, the state vector and the covariance matrix are updated according to the observations. The update process is written as follows:

$${\hat{X}}_k=X_k+G_k·\left(Z_k-H_k·X_k\right)$$

(30)

$$P_k=P_k^{-}-G_k·H_k·P_k^{-}$$

(31)

where $X_k$ and ${\hat{X}}_k$ are the prior and posterior state estimate vector, $G_k$ is the gain matrix of EKF, $P_k^{-}$ and $P_k$ are the prior and posterior system covariance matrix, $Q_k$ is the covariance matrix of the process noise, $R_k$ is the covariance matrix of the observational noise vector. When the integrated system runs, the prior position, step length, and heading angle are obtained by the PDR method. The state vector composed of previous elements and the other observation vector obtained by the BLE position method is input into the fusion method. When the fusion method is cyclically executed, the corrected position results will be obtained.

3.5. A Robust Filter Model

The EKF model described above can effectively suppress the drift error of the PDR method and improve the overall positioning accuracy and stability, but the effect of suppressing the gross error of the BLE fingerprint position is poor. This paper proposes a robust filter model. In the EKF model, the innovation vector $r_k$ is expressed as Equation (32):

$$r_k=Z_k-H_k·X_k=\left[N_k^{\prime }-N_k,E_k^{\prime }-E_k\right]=\left[∆N,∆E\right]$$

(32)

where $∆N$and $∆E$ represent the position coordinate difference in the north and east, respectively. From Equation (32), the $r_k$ represents the position coordinate difference between the BLE and PDR methods.

When there are gross errors in the positioning method, there are the following situations:

• The difference $∆N$ exceeds the limit;
• The difference $∆E$ exceeds the limit;
• The innovation vector $r_k$ exceeds the limit, $∆N$ and $∆E$ are within the acceptable range;
• The difference $∆N$,$∆E,$ and the innovation vector $r_k$ exceed the limit.

Therefore, to judge whether there is a gross error in the process of position, it is necessary to determine the distribution of the $∆N$,$∆E$ and $r_k$.

In the EKF method, $Z_k$ should conform to Gaussian distribution with mean $H_k{X}_k$ and covariance $P_k^r$ as shown in Equation (33). The innovation vector $r_k$ conforms to Gaussian distribution with mean 0, covariance $P_k^r$.

$$P_k^r=H_k·P_k^{-}·H_k^T+R_k$$

(33)

while the difference $∆N$ conforms to the Gaussian distribution with mean 0, covariance ${\delta }_{∆N}^2$ and the difference $∆E$ conforms to the Gaussian distribution with mean 0, covariance ${\delta }_{∆N}^2$.

$${\delta }_{∆N}^2=\left[\begin{array}{cc}1& 0\end{array}\right]·P_k^r·[\begin{array}{c}1\\ 0\end{array}]$$

(34)

$${\delta }_{∆E}^2=\left[\begin{array}{cc}0& 1\end{array}\right]·P_k^r·[\begin{array}{c}0\\ 1\end{array}]$$

(35)

$${\lambda }_{∆N}=∆N \sim N\left(0,{\delta }_{∆N}^2\right)$$

(36)

$${\lambda }_{∆E}=∆E \sim N\left(0,{\delta }_{∆E}^2\right)$$

(37)

The squared Mahalanobis distance $M_k$ of $r_k$ conforms to the chi-square distribution ${\chi }_{m,\alpha }^2$ with the freedom $m$ which is the dimension of the observation $Z_k$.

$${\lambda }_r=M_k^2=r_k^T{\left(P_k^r\right)}^{-1}{r}_k \sim {\chi }_{m,\alpha }^2$$

(38)

The chi-square distribution ${\chi }_{m,\alpha }^2$ is constructed to determine whether the actual vector $r_k$ calculated in the EKF exceeds the limit under the Gaussian assumption. Significance level $\alpha$ is the probability threshold and 5% is adopted in our research.

$$P_r\left[{\lambda }_r>{\chi }_{m,\alpha }^2\right]<\alpha$$

(39)

where $P_r$ represents the probability of a random event that the probability of ${\lambda }_r$ being larger than ${\chi }_{m,\alpha }^2$ is very small. Hence, if the actual $r_k$ is larger than the $\alpha$-quantile, the null hypotheses are rejected and it can be concluded that $r_k$ exceeds the limit and $Z_k$ has a gross error in the positioning. For $∆N$ and $∆E$, when the actual measurements are larger than twice the corresponding standard deviation, the significance level is less than 5%, and it can be concluded that $∆N$ or $∆E$ or both exceed the limit and $Z_k$ has a gross error.

When the vector $Z_k$ has a gross error, the observational covariance matrix $R_k$ should multiply a robust vector:

$${\overline{R}}_k={\beta }_k^T·R_k·{\beta }_k$$

(40)

where $R_k$ represents vector. ${\overline{R}}_k$ is the modified observation covariance matrix. When the vector $Z_k$ has a gross error in the first situation, the robust vector ${\beta }_k$ is constructed as follows:

$${\beta }_k={[\begin{array}{cc}\frac{{\lambda }_{∆N}}{{\delta }_{∆N}}& 1\end{array}]}^T$$

(41)

When the vector $Z_k$ has a gross error in the second situation, the robust vector ${\beta }_k$ is constructed as follows:

$${\beta }_k={[\begin{array}{cc}1& \frac{{\lambda }_{∆E}}{{\delta }_{∆E}}\end{array}]}^T$$

(42)

When the vector $Z_k$ has a gross error in the third situation, the robust vector ${\beta }_k$ is constructed as follows:

$${\beta }_k={[\begin{array}{cc}\frac{{\lambda }_r}{{\chi }_{m,\alpha }^2}& \frac{{\lambda }_r}{{\chi }_{m,\alpha }^2}\end{array}]}^T$$

(43)

When the vector $Z_k$ has a gross error in the fourth situation, the robust vector ${\beta }_k$ is constructed as follows:

$${\beta }_k={[\frac{{\lambda }_{∆N}}{{\delta }_{∆N}}+\begin{array}{cc}\frac{{\lambda }_r}{{\chi }_{m,\alpha }^2}& \frac{{\lambda }_{∆E}}{{\delta }_{∆E}}+\frac{{\lambda }_r}{{\chi }_{m,\alpha }^2}\end{array}]}^{T}ss$$

(44)

According to the above method, the robust filter can effectively restrain the gross error in the observation vector $Z_k$. In practice, it is possible for the matrix $R_k$ to be modified even there is no gross error in the observation $Z_k.$

At last, we proposed the BLE/PDR integrated System localization framework in the following diagram of Figure 7. In the PDR position method, the gyroscope readings are used to compute the heading angle and the accelerations and magnetometer are utilized to compensate for accumulated error as well. To improve the accuracy of the heading angle, people’s motion status is considered in correcting the control parameters of MCF. The accelerations are also used to detect the steps and estimate the step length. With the step length and heading angle, a position is computed by the PDR method. The other position is obtained by the BLE fingerprint method at the same time. Then, two positioning estimations are input into the Extend Kalman filter to resolve the fusion position. To improve the integrated system robustness and scalability, a robust filter is used to restrain the gross error of BLE observational position results.

4. Experiments and Analysis

4.1. BLE Fingerprint Position Experiments and Error Analysis

The experiments were set up on the second floor of the C7 test site which has three floors in the 54th Research Institute of China Electronics Technology Group Corporation as shown in Figure 8. The second-floor test site is 24.92 m long and 27.49 m wide which consists of a rectangular corridor and several rooms. The center part of Figure 8 is a hollow space enclosed with glass. When the radio signal of BLE propagates, there is a severe multipath effect. There are 54 Bluetooth beacons installed on the whole C7 test site and 18 on each floor. The deployment of the Bluetooth beacon in Figure 8 is based on the principle of optimizing beacon placement by maximizing localization accuracy and satisfying a predefined coverage degree [23]. The HUAWEI P20 smartphone was chosen as the test device. In Figure 8, the blue triangle refers to the Bluetooth low energy beacon and there are 18 BLE beacons were installed on the second-floor test site. For the BLE fingerprint position method, all of the BLE beacons installed on the C7 test site were utilized to collect signals for the experiment. Another scenario called 331 test site had been chosen to conduct a comparison experiment. The test site is 21.72 m long and 7.75 m wide. The deployment of the Bluetooth beacon in the 331 test site is upon the same optimization solution as C7.

Then we carried out a BLE fingerprint positioning experiment in this position scenario. The BLE fingerprint database was constructed in advance. People stranded with the HUAWEI P20 at each known RP to collect data for 7 s, the time is set randomly. Then the data was processed per second into real-time fingerprint data. In the actual acquisition process, only 6 s of data were collected at some RPs. There were real-time 332 BLE fingerprint data collected at 48 RPs for fingerprint position. We calculated the positioning results through the KNN matching method and compared them with the true coordinates of RPs to obtain the position error. We analyzed the gross error of the BLE fingerprint method, whose error is larger than twice the standard deviation under the Gaussian distribution. Then we picked out the gross error caused by the few numbers of scanning RSSI which is less than 5 and we eliminate the corresponding fingerprint. Finally, we recalculate the position error, and both position errors were obtained and they were illustrated in

Table 1. Position error comparison/(m).

Method	Min	Max	Mean	RMSE
BLE fingerprint method	0.005	18.289	2.834	3.106
Eliminating the less scanned BLE number	0.005	18.289	2.312	2.042

and Figure 9.

As shown in Table 1, after eliminating the few numbers of scanning RSSI real-time fingerprints, the mean position accuracy and the root-mean-square error (RMSE) were 2.312 m and 2.043 m, respectively. Compared with all data on the BLE fingerprint method, the position accuracy was reduced by 0.5220 m and RMSE decreased by 1.064 m. In Figure 8, the blue line refers to the result of all real-time fingerprint data and the green line refers to eliminating the few numbers of scanning RSSI real-time fingerprints. The red dashed line represents twice the standard deviation which means the position error that exceeds the red dashed line is a gross error. From Figure 9a, the gross error number shown in the green line is greatly reduced compared with the blue line. Figure 9b indicates that the method of eliminating the few numbers of scanning RSSI has a higher confidence level than the BLE fingerprint method. From the statistics of gross errors, the total number of gross errors is 31. Among them, the number of gross errors caused by the few numbers of scanning RSSI is 17, accounting for about 54.8%. Therefore, we can conclude that the causes of gross errors in the BLE fingerprint method are not only caused by signal fluctuations but also affected by the few numbers of scanning RSSI. It is necessary to find a robust filter to restrain the gross error of the BLE fingerprint method.

4.2. Heading Estimation Based on Motion States

The experiment about heading estimation was carried out in the same position scenario as shown in Figure 9.

In Figure 10, the red star refers to the start point and endpoint of the trajectory. The dark green line represents the trajectory and the arrow represents the direction. The current motion state is indicated on each trajectory which includes walking and running. The static state of the pedestrian is contained inside the red ellipse. The pedestrian walked along the dark green trajectory with different motion states.

Then the heading is estimated by different methods as shown in the following Figure 11.

Four methods were utilized to estimate the heading angle. In Figure 11, the blue line refers to the heading angle estimated by the electronic compass while the magenta line, red line, and green line were represented as gyroscope-based, Mahony, and improved Mahony based on motion state methods, respectively. The black line denoted the true heading. From Figure 10, the accuracy of the heading angle estimated by the electronic compass was poor and suffered severe fluctuation, but it would be no drift for a long time. In contrast, the heading estimated by gyroscope did not have severe fluctuation but would suffer huge drift problems that would distort the heading.

For the MCF method, the control parameters $K_p$ and $K_i$ were given two values of 0.001 and 0.000001, respectively. The MCF performed well in the early state but would suffer little drift for a long time without the adaptive parameter adjustment based on the motion state. From the partially enlarged view of the last trajectory, the heading estimated by the improved Mahony based on the motion state was the most accurate, which fluctuated around the true angle and had no cumulative error. Finally, a diagram of different motion states of experiment trajectory is shown in Figure 12 which contained a semi-transparent layer of heading estimation for comparison. It could be concluded that the heading estimation is closely related to the motion state. When stationary, the heading estimation was relatively stable when walking or moving, and the heading would fluctuate.

4.3. BLE/PDR Integrated System Position Experiment

The BLE/PDR integrated system position experiment was carried out in the C7 test site. The BLE AP routers and the smartphone used in the experiment are the same as in the experiment of Section 3.1. The new trajectory was planned and was shown in Figure 13.

In Figure 13, the red star and blue cycle refer to the start point endpoint of the trajectory, respectively. The green dotted line represents the reference trajectory and the arrow represents the direction. During the experiment, the data sampling frequency of PDR was set as 50 Hz and the smartphone scanned the BLE APs per second. The pedestrian started from the start point and reached the endpoint at a constant speed. The pedestrian held the smartphone level and walked 107 steps in total during the experiment. Another integrated system position experiment was conducted at the 331 test site. The comparison results of the different methods were shown in Figure 16.

To validate the efficiency of the robust filter method, another two methods, the BLE fingerprint method and the EKF method, were also utilized for the experiment. Position errors of the three methods were computed concerning the reference points for evaluation. Figure 14a showed the time series of the position errors and (b) showed the corresponding cumulative distribution errors.

In the above figure, the blue line refers to the BLE fingerprint method. The red line and green line represent the EKF and robust filter methods, respectively. From the value and distribution of the position error, the robust filter method denoted by the green line performs best. The cumulative distribution of the robust filter indicates that it has a higher confidence level than the other two methods. About 74% of the position error of points are lower than 1 m and about 86% of the position error of points are lower than 2 m. Compared with the common EKF, these two indicators are increased by 38% and 18%, respectively. Then,

Table 2. Position error comparison/(m).

Method	Min	Max	Mean	RMSE
BLE fingerprint method	0.165	13.033	2.647	2.727
Extend Kalman filter method	0	7.122	1.960	1.727
Robust filter base on EKF	0	2.641	0.844	0.745

shows the detail of the mean error and RMSE of the three methods.

As shown in Table 2. The mean position accuracy and the RMSE of the robust filter method are 0.844 m and 0.745 m respectively. Comparing with the EKF and BLE fingerprint methods, the position accuracies were reduced by 1.116 m and 1.803 m, and the RMSEs were decreased by 1.982 m and 0.982 m. From the perspective of max value, the max value of the robust filter is 2.641 m which is also significantly reduced. The figure and table mentioned above show that the proposed robust filter can not only reduce the position error but also improve the stability.

Figure 15 shows the trajectory of three methods in the true position scenario at the C7 test site. Another fusion method particle filter is also utilized to solve the result as a comparison experiment.

The blue line, red line, green line, and dark green line represent the trajectory of BLE fingerprint, EKF, robust filter methods, and reference trajectory, respectively. In Figure 15a, some jump points, which mean the gross error surrounded by the red circle, appear in the BLE fingerprint method or the EKF method. The robust filter can improve the observation matrix based on detecting the gross errors and get a smooth trajectory without jump points. We can see that the proposed robust filter performs better than the other two methods in (b). (c) and (d) are the results of different hybrid positioning methods under two trajectories. From the comparison of the curves with the real trajectory in (c) and (d), the proposed method is superior to the classical EKF. The particle filter also performs well in some positions compared to the proposed method. However, the particle filter requires the construction of a large number of particles requiring a heavy computational load. The proposed method is more suitable for real-time localization than the particle filter method. Another comparison of the experimental results of the integrated system was conducted in the 331 test site and the results were shown in Figure 16. From Figure 16, the green line that represents the proposed method is closer to the real track and smoother in some corners compared to other methods. Although the test paths are relatively short because of the extent of the experimental scenarios, the two methods of fusion position have different principles, in which PDR has high instantaneous accuracy but suffers cumulative errors, and the real-time Bluetooth fingerprint method can compensate for this shortcoming based on the robust filter. Therefore, even with the test paths becoming longer, there will be no cumulative error to make the path diverge. We can conclude that the proposed method makes it possible to have similar improvement under different circumstances.

5. Discussion

In the process of real-time positioning, poor position results may be obtained by the BLE fingerprint method. The heading of the device is affected by different motion states of people. Then we get rid of the bad results of the BLE fingerprint method when the scanning number of RSSI is smaller than 5. We correct the heading based on different motion states. Even if the BLE fingerprint method and PDR are integrated, the gross error is difficult to be suppressed. Compared with the adaptive and robust filter proposed in the research of Li et al. [27], the robust filter proposed in this paper considering the error distribution in more conditions and provides a robust vector instead of a numerical correction. The experimental results conducted on the true position scenario show that the proposed method can detect, suppress the gross errors and make the results smoother. The mean position accuracy of the proposed robust filter was 0.844 m and RMSE was 0.745. The experiments are in line with expectations. From Figure 14, we can find that the green points obtained by the robust filter are far away from the true position. The reason for this is the bad position results caused by the BLE fingerprint method at the previous few steps. The jump points in blue color have occurred continuously. The robust filter can only suppress gross errors but not eliminate them. If gross errors occur continuously, then the results will deviate from the true trajectory and it would take some time to converge. We will improve the method by considering the situation of continuous gross errors to get better results in future research work.

6. Conclusions

In this paper, we concentrated on the real-time BLE/PDR integrated System and fusion method. For BLE real-time fingerprint, we found that the position error of BLE is not only with the signal fluctuation but also with the scanned number of BLE APs. If the scanned number is too few, there will easily be gross errors. Next, we introduced the method of commonly used attitude methods and an improved Mahony complementary filter is proposed to estimate the heading angle under different motion states. Finally, a robust filter model was proposed to fusion the BLE/PDR methods because of the gross error in the BLE method. We conducted an experiment to validate the efficiency of the proposed method in the true position scenario. The mean position accuracy obtained by the robust filter was 0.844 m and RMSE was 0.745. The experiment showed that the proposed method has better performance in positioning accuracy and stability. The experimental scenario in this paper is surrounded by glass in the center, which has serious multi-path effects for the fingerprint positioning method and can easily cause coarse errors. The classic Kalman filter or Extended Kalman filter is not effective for coarse difference suppression. The proposed method can detect gross errors at different granularities and suppress them. The fusion methods based on the KNN and EKF are suitable for the high-real-time requirements of the positioning applications, keeping a balance between computational efficiency and position accuracy. The estimation of the heading angle is more stable based on the people’s motion states. However, the effect of different pedestrian motion states on hybrid positioning was not analyzed in detail. In addition to the detection and suppression of coarse differences in the observation noise matrix, some modifications of the state transition matrix will be made based on the people’s motion states. How the variance matrix of process noise adaptively changes according to the people’s motion states will be investigated in-depth in future work. Combined with other positioning methods such as map matching, landmarks matching for multi-mode fusion positioning will be considered too.