Introduction

Location-based services (LBSs) have become a popular focus of research1,2. Wireless positioning methods can be divided into outdoor positioning and indoor positioning methods. At present, outdoor positioning technology has reached a very mature state of development. Global navigation satellite systems (GNSSs) can provide users with submeter- or even centimetre-level navigation and positioning services3,4,5,6, effectively meeting the needs of outdoor users. In an indoor environment, however, it is necessary to use other signal sources for localization. Ultrawideband (UWB) signals have ultrahigh temporal resolution, ultrastrong penetration, extremely low power spectral density, and an ultrahigh transmission rate and are widely used in indoor positioning7,8. In an indoor line-of-sight (LOS) environment, the plane positioning accuracy can reach the centimetre level9,10. Nevertheless, although the strong penetration ability of UWB signals allows them to easily penetrate common objects such as wood and glass11, building facilities composed of bricks, steel bars and concrete can readily interfere with UWB signals, resulting in large errors in non‒line-of-sight (NLOS) ranging values12,13. Moreover, microelectromechanical system (MEMS)-based inertial measurement units (IMUs) have the advantages of a low price, small size, strong anti-interference capabilities, and the ability to produce a large amount of navigation information at a high information output rate14,15. However, owing to the limitations imposed by the device material itself and the influence of the inherent integration principle, the positioning errors of an IMU accumulate rapidly over time, meaning that such a device cannot be used independently to achieve long-term navigation and positioning16,17. Nevertheless, considering the respective characteristics of UWB signals and MEMS IMUs, these two technologies can be combined to exploit their complementary advantages17,18,19.

At present, many teams have carried out research on integrated UWB/MEMS IMU positioning20. On the basis of the standard Kalman filter, several researchers have extended various combined filtering methods and applied them in integrated UWB/MEMS IMU positioning systems. Zhang and Guo21 designed a fusion algorithm based on the extended Kalman filter (EKF) that suppresses the position offset of the carrier by estimating the error of the inertial device online. Finally, a parallel sliding window is used to smooth the fused positioning results. The simulation results revealed that the error of this method is 38% lower than that of the traditional filtering method21. Zhu et al.22 used the EKF to realize tight integration of UWB and IMU signals and used chi-square detection to discriminate NLOS signals. After detecting abnormal observations, these authors used reduced weight processing and then designed a filter to smooth the position estimates obtained via the Newton iteration method22. Although this algorithm can suppress the influence of NLOS errors to a certain extent, it does not perform well in addressing UWB ranging values that are strongly affected by noise. In accordance with the particular characteristics of various indoor application scenarios, appropriate constraint algorithms can be introduced to improve the robustness of integrated positioning systems. Du23 applied zero-velocity correction and autonomous integrity monitoring in integrated UWB/MEMS IMU positioning, achieving the ability to maintain the positioning continuity of the system when the UWB signals suffer from gross errors or even signal loss. However, in the case of the long-term absence of UWB signals, a drift problem that depends only on the zero-speed correction of the IMU will continue to exist. To address the problem of UWB signal interruption during carrier movement, Koppanyi et al.24 added a motion constraint algorithm to an integrated UWB/MEMS IMU positioning system and solved the problem of navigation divergence of the IMU under conditions of UWB signal interruption by exploiting predefined dynamic features such as straight, turning and stopping. However, in this algorithm, two neural networks need to be trained on sufficient data for motion state recognition, and the workload for such training is large. The topology information of an indoor plane map can be used to judge the intervisibility between UWB modules, thereby improving the accuracy and stability of an integrated positioning system. Following this principle, Wang et al.25 used map information to identify NLOS conditions, adjusted the weights of UWB ranging values in accordance with the recognition results, and used a multithreshold robust Kalman filter for integrated positioning. However, studies have shown that when an obstacle is located in the Fresnel zone of a UWB mobile station and base station, even if a corresponding NLOS ranging value is generated, the ranging value will be at the same level as the error on the measured value under LOS conditions26; therefore, it is not sufficient to use map information to judge the influence of NLOS conditions on the ranging value. To address this challenge, through experiments, this paper proves that the degree of UWB signal occlusion increases as the horizontal angle decreases and reports the angle limit beyond which no UWB ranging value is available. Then, by incorporating map database information, a tightly integrated UWB/MEMS IMU positioning method that combines NLOS angle discrimination and map (MAP) constraints is proposed, and the positioning results of this algorithm are compared with those of other classical algorithms.

Tightly integrated UWB/MEMS IMU positioning model

State model

The state vectors in the tightly integrated UWB/MEMS IMU positioning model are the position error \(\delta r^{n}\), the velocity error \(\delta v^{n}\), the attitude error \(\phi\), the gyroscope bias \(b_{g}\) and the accelerometer bias \(b_{a}\), that is:

$$ X = \left[ {\begin{array}{*{20}l} {\delta r^{n} } \hfill & {\delta v^{n} } \hfill & \phi \hfill & {b_{g} } \hfill & {b_{a} } \hfill \\ \end{array} } \right] $$
(1)

The error equations are rewritten as follows:

$$ \left\{ {\begin{array}{*{20}l} {\delta \dot{r}^{n} = \delta v^{n} } \hfill \\ {\delta \dot{v}^{n} = C_{b}^{n} f^{b} \times \phi + C_{b}^{n} (b_{a}^{b} + w_{ab} )} \hfill \\ {\dot{\phi } = - C_{b}^{n} (b_{g}^{b} + w_{gb} )} \hfill \\ {\dot{b}_{g} = 0} \hfill \\ {b_{a} = 0} \hfill \\ \end{array} } \right. $$
(2)

where \(C_{b}^{n}\) is the attitude matrix, \(f^{b}\) is the specific force, and \(w_{ab}\) and \(w_{gb}\) represent the white noise affecting the accelerometer and gyroscope measurements, respectively. The state equation of the Kalman filter can be expressed as follows:

$$ \dot{X} = FX + GW^{b} $$
(3)

where the expressions for the state transition matrix \(F\), the noise allocation matrix \(G\) and the system noise matrix \(W^{b}\) are as follows:

$$ \begin{aligned} G & = \left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\mathop 0\limits_{3 \times 3} } & {\mathop 0\limits_{3 \times 3} } \\ \end{array} } \\ {\begin{array}{*{20}c} {\mathop 0\limits_{3 \times 3} } & {C_{b}^{n} } \\ \end{array} } \\ {\begin{array}{*{20}c} { - C_{b}^{n} } & {\mathop 0\limits_{3 \times 3} } \\ \end{array} } \\ {\mathop 0\limits_{6 \times 6} } \\ \end{array} } \right] \\ W^{b} & = \left[ {\begin{array}{*{20}c} {w_{ab} } \\ {w_{gb} } \\ \end{array} } \right] \\ \mathop F\limits_{15 \times 15} & = \left[ {\begin{array}{*{20}l} {\mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop I\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill \\ {\mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad C_{b}^{n} f^{b} \times } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad C_{b}^{n} } \hfill \\ {\mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad - C_{b}^{n} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill \\ {\mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill \\ {\mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill & {\quad \mathop 0\limits_{3 \times 3} } \hfill \\ \end{array} } \right] \\ \end{aligned} $$
(4)

Measurement model

The measurement equation for tightly integrated UWB/MEMS IMU measurements is as follows:

$$ Z_{k} = H_{k} X_{k} + V_{k} $$
(5)

where \(H_{k}\) represents the measurement matrix and where \(V_{k}\) represents the measurement noise vector. The corresponding covariance matrix is \(R_{k}\). The expression for each matrix is as follows:

$$ Z_{k} = \left[ {\begin{array}{*{20}c} {Z_{k,1} } \\ {Z_{k,1} } \\ \vdots \\ {Z_{k,M} } \\ \end{array} } \right],\quad H_{k} = \left[ {\begin{array}{*{20}c} {H_{k,1} } \\ {H_{k,2} } \\ \vdots \\ {H_{k,M} } \\ \end{array} } \right],\quad V_{k} = \left[ {\begin{array}{*{20}c} {V_{k,1} } \\ {V_{k,1} } \\ \vdots \\ {V_{k,M} } \\ \end{array} } \right] $$
(6)

where \(M\) denotes the number of UWB base stations, and the individual elements are expressed as follows:

$$ \left\{ {\begin{array}{*{20}l} {Z_{k,j} = d_{INS,J} - d_{UWB,j} ;V_{k,j} = \varepsilon_{UWB,j} } \hfill \\ {H_{k,j} = \left[ {\begin{array}{*{20}l} {\frac{{\left( {r_{true,m}^{n} - r_{base,j}^{n} } \right)^{T} }}{{d_{true,j} }}} \hfill & {\mathop 0\limits_{1 \times 3} } \hfill & {\frac{{\left( {r_{INS,m}^{n} - r_{base,j}^{n} } \right)^{T} \left( {C_{b}^{n} l^{b} \times } \right)}}{{d_{true,j} }}} \hfill & {\mathop 0\limits_{1 \times 6} } \hfill \\ \end{array} } \right]} \hfill \\ \end{array} } \right. $$
(7)

where \(r_{INS,m}^{n}\) represents the position of the UWB antenna as calculated by the inertial navigation system (INS), \(r_{base}^{n}\) represents the position of the UWB base station, \(r_{true,m}^{n}\) represents the real position of the UWB rover station, \(d_{INS}\) represents the distance obtained via inverse calculation using the coordinates of \(r_{INS,m}^{n}\) and the UWB base station, \(d_{true}\) represents the real distance between the UWB rover station and the base station, \(l^{b}\) represents the lever arm of the inertial navigation centre and the UWB antenna, \(d_{UWB}\) represents the distance value measured on the basis of the UWB signal, and \(\varepsilon_{UWB}\) denotes the measurement noise of the UWB measurements. The subscript \(j\) represents that the information corresponds to the relationship between the UWB rover and the \(j\)-th base station in a certain epoch, and its value ranges from 1-\(M\).

The Integrated UWB/MEMS IMU Positioning system Model implemented according to Algorithm 1.

Tightly integrated UWB/MEMS IMU positioning method based on NLOS angle discrimination and MAP constraints

Dynamic NLOS angle discrimination method

To improve the robustness of UWB ranging values in an NLOS environment, experiments were conducted to verify the relationship between the UWB ranging error and the horizontal angle in a dynamic scene (in this paper, the horizontal angle refers to the angle formed by connecting the feature point of the obstacle, as the vertex, to both the UWB rover station and the reference station). For this purpose, given the layout of an indoor underground parking lot, three different motion scenarios were selected for the experiments. In scene (a), the moving station and the reference station are placed on both sides of the load-bearing column, the experimental platform is slowly moved close to the load-bearing column in the direction of the connection between the reference station and the moving station until the UWB ranging fails and the experiment for scene 1 then ends. In scene (b), the base station is placed on one side of the load-bearing column, and the experimental platform moves in a direction perpendicular to the moving direction of the experimental platform in Scene 1. During this period, the relation between the UWB transceiver modules undergoes LOS-NLOS-LOS changes. In scene (c), the UWB base station is placed on one side of the wall. Before the experiment, the UWB rover station and the base station are in a state of mutual visibility, and the experimental platform is pushed back and forth in the direction parallel to the wall on the other side of the wall. During the whole experiment, the visibility between the UWB rover station and the base station underwent an LOS-NLOS-LOS transition. A top view of each scenario is shown in Fig. 1, where the red arrows represent the movement routes of the rover station.

Fig. 1
figure 1

Top views of three motion scenarios: (a) Scenario 1, (b) Scenario 2, (c) Scenario 3.

Full size image

When a wireless signal is occluded during propagation, it tends to escape in the direction of least occlusion. In each of the three motion scenarios, the horizontal angle changes during the movement of the experimental platform (the construction of the experimental platform is described in detail in the third section), as does the degree of occlusion of the UWB signal. Taking Scenario 1 as an example, the change in angle is shown in Fig. 2 and is further described below.

Fig. 2
figure 2

Schematic diagram of angle changes.

Full size image

In Scenario 1, during the movement of the experimental platform, the degree of UWB signal occlusion is the same on both sides of the load-bearing column. Therefore, it is advisable to set point B as the vertex and determine the horizontal angle formed by the line segments connecting it to the UWB rover station and the base station. In the second scenario, as the experimental platform moves, point A is selected as the vertex of the horizontal angle during the process of LOS-to-NLOS conversion, and point B is selected as the vertex of the horizontal angle during the process of NLOS-to-LOS conversion. In the third scenario, since the UWB signal only undergoes a change in visibility on one side of the wall and point A is the feature point of the occlusion at both the beginning and the end, point A is selected as the vertex of the horizontal angle.

Given the coordinates of the UWB reference station, the rover station and the obstacle feature point, the coordinate azimuth angle of the two adjacent line segments can be inversely calculated. In accordance with the relationship between the coordinate azimuth angle and the turning angle of the two line segments, the horizontal angle can then be calculated indirectly. The azimuth angle calculation formula is as follows:

$$ \alpha_{CD} = \arctan \left( {\frac{{y_{D} - y_{C} }}{{x_{D} - x_{C} }}} \right) = \arctan \left( {\frac{{\Delta y_{CD} }}{{\Delta x_{CD} }}} \right) $$
(8)

where \(\alpha_{CD}\) is the coordinate azimuth angle of line segment \(CD\); \((x_{C} ,y_{C} )\) represents the planar coordinates of point \(C\); \((x_{D} ,y_{D} )\) represents the planar coordinates of point \(D\); and \(\Delta y_{CD}\) and \(\Delta x_{CD}\) represent the coordinate changes in the \(y\) and \(x\) directions, respectively. The values of \(\alpha_{CD}\) are shown in Table 1.

Table 1 Coordinate azimuth value method.
Full size table

If \(\alpha_{CD} < 0\), then \(\alpha_{CD} = \alpha_{CD} + 360^\circ\); if \(\alpha_{CD} > 0\), then \(\alpha_{CD} = \alpha_{CD} - 360^\circ\).

Once the coordinate azimuth angles of line segments CD and DE are known, a direction reference is determined with the UWB reference station as the starting point and the mobile station as the endpoint. The angle on the left side of the direction reference is called the left angle, and the angle on the right side of the reference is the right angle, as shown in Fig. 3.

Fig. 3
figure 3

Relationship between the azimuth and horizontal angles.

Full size image

If \(\beta\) is the left angle:

$$ \beta = \alpha_{DE} - \alpha_{CD} + 180^\circ $$
(9)

If \(\beta\) is the right angle:

$$ \beta = \alpha_{CD} - \alpha_{DE} + 180^\circ $$
(10)

In formulas (9) and (10), \(\beta\) denotes the horizontal angle, \(\alpha_{CD}\) represents the coordinate azimuth angle of line segment CD, and \(\alpha_{DE}\) represents the coordinate azimuth angle of line segment DE.

To verify the correlation between the horizontal angle and the degree of UWB signal occlusion, UWB ranging values were collected during the movement of the experimental platform in each of the three motion scenarios introduced above. The error curves of the difference between the UWB ranging value and the total station measurement reference value over time are shown in Fig. 4, and the curves obtained by plotting the same ranging errors with the horizontal angle as the abscissa are shown in Fig. 5.

Fig. 4
figure 4

Curves of the UWB ranging error over time in the three motion scenarios: (a) Scenario 1, (b) Scenario 2, (c) Scenario 3.

Full size image
Fig. 5
figure 5

Curves of the UWB ranging error as a function of the horizontal angle in the three motion scenarios: (a) Scenario 1, (b) Scenario 2, (c) Scenario 3.

Full size image

The green and red markers in Fig. 4b,c indicate the time points and error values corresponding to the beginning and end of UWB occlusion, respectively. The green and red circles in Fig. 5b,c similarly indicate the horizontal angles at which the UWB signal begins to be blocked and ceases being blocked, respectively, and the corresponding ranging errors. No such markers appear in Figs. 4a, 5a because the UWB signal is always occluded in Scenario 1. The missing data in Figs. 4c, 5c indicate that signal loss was caused by severe occlusion of the UWB signal. Figure 5 shows that when the horizontal angle is less than 180°, there are still some ranging errors within the nominal accuracy range. As the experimental platform moves, the horizontal angle gradually decreases, and the ranging error continues to increase. When the horizontal angle decreases below 167°, the ranging error jumps to the metre level. The above data and analysis reveal that the horizontal angle reflects the degree of UWB signal occlusion. When the horizontal angle is less than 167°, the UWB ranging error is too large, and no ranging value is available.

Establishment of the map database and the line segment matching algorithm

For a local indoor area, the feature points of the obstacles in the area can be extracted via traditional digital mapping methods to form the corresponding planar map. The resulting map database is composed of line segment vectors defined in terms of the coordinate information of these feature points.

To use this map to assist in the identification of NLOS conditions, once the map database has been established, it is necessary to determine whether the line between the UWB rover and the base station intersects with any line segment in the map database and the number of such intersections by means of a line segment matching algorithm.

Let A \((X_{mobile} ,Y_{mobile} ,Z_{mobile} )\) denote the spatial position of the UWB rover, let B \((X_{base} ,Y_{base} ,Z_{base} )\) denote the spatial position of the UWB base station, and let C \((X_{i,s} ,Y_{i,s} ,Z_{i,s} )\) and D \((X_{i,e} ,Y_{i,e} ,Z_{i,e} )\) represent the three-dimensional coordinates of the starting and ending points, respectively, of the first line segment vector in the map database, as shown in Fig. 6. Owing to the large size of the map database for the indoor environment, the mixed product is used in this paper to determine whether the line segments intersect.

Fig. 6
figure 6

Diagram illustrating the principle of the mixed product criterion.

Full size image

In Fig. 6, \({\varvec{AZ1}} = \user2{CZ2 = }\left[ {\begin{array}{*{20}c} 0 & 0 & 1 \\ \end{array} } \right]\) is an auxiliary vector. On the basis of vector AB, the coordinates of vectors AB, AC and AD are \((X_{base} - X_{mobile} ,Y_{base} - Y_{mobile} ,Z_{base} - Z_{mobile} )\), \((X_{i,s} - X_{mobile} ,\,Y_{i,s} - Y_{mobile} ,Z_{i,s} - Z_{mobile} )\), and \((X_{i,e} - X_{mobile} ,\)\(Y_{i,e} - Y_{mobile} ,Z_{i,e} -\)\(Z_{mobile} )\), respectively. According to the mixed product criterion, the intersection of two line segments is equivalent to the two endpoints of the component line segments of one line segment lying on both sides of the other line segment. The corresponding expressions are as follows:

$$ \begin{aligned} D1 & = ((X_{base} - X_{mobile} ) \times (Y_{i,s} - Y_{mobile} ) - (X_{i,s} - X_{mobile} ) \times (Y_{base} - Y_{mobile} )) \ge 0 \\ D2 & = ((X_{base} - X_{mobile} ) \times (Y_{i,e} - Y_{mobile} ) - (X_{i,e} - X_{mobile} ) \times (Y_{base} - Y_{mobile} )) \le 0 \\ \end{aligned} $$
(11)

When \(D1 > 0\) and \(D2 < 0\), endpoints C and D of line segment CD lie on opposite sides of line segment AB; when \(D1 = 0\) and \(D2 < 0\), endpoint C of line segment CD lies on line segment AB; when \(D1 > 0\) and \(D2 = 0\), endpoint D of line segment CD lies on line segment AB; and when \(D1 = 0\) and \(D2 = 0\), line segment AB and line segment CD are collinear.

Similarly, on the basis of vector CD, the coordinates of vectors CD, CA and CB are \((X_{i,e} - X_{i,s} ,Y_{i,e} - Y_{i,s} ,Z_{i,e} - Z_{i,s} )\), \((X_{mobile} - X_{i,s} ,\)\(Y_{mobile} - Y_{i,s} ,Z_{mobile} - Z_{i,s} )\), and \((X_{base} - X_{i,s} ,Y_{base} - Y_{i,s} ,Z_{base} -\)\(Z_{i,s} )\), respectively. The following expressions can be constructed:

$$ \begin{aligned} D3 & = ((X_{i,e} - X_{i,s} ) \times (Y_{mobile} - Y_{i,s} ) - (X_{mobile} - X_{i,s} ) \times (Y_{i,e} - Y_{i,s} )) \ge 0 \\ D4 & = ((X_{i,e} - X_{i,s} ) \times (Y_{base} - Y_{i,s} ) - (X_{base} - X_{i,s} ) \times (Y_{i,e} - Y_{i,s} )) \le 0 \\ \end{aligned} $$
(12)

When \(D3 > 0\) and \(D4 < 0\), endpoints A and B of line segment AB lie on opposite sides of line segment CD; when \(D3 = 0\) and \(D4 < 0\), endpoint A of line segment AB lies on line segment CD; when \(D3 > 0\) and \(D4 = 0\), endpoint B of line segment AB lies on line segment CD; and when \(D3 = 0\) and \(D4 = 0\), line segment CD and line segment AB are collinear.

In accordance with whether \(D1\), \(D2\), \(D3\) and \(D4\) take positive or negative values, the intersection of line segments AB and CD can be judged as shown in Table 2.

Table 2 Criteria for determining the intersection of line segments AB and CD.
Full size table

When \(D1 = D2 = D3 = D4 = 0\), the two lines are collinear and separated. To exclude this category, the four endpoints of the two line segments are used to construct virtual line segments and obtain the lengths thereof, denoted by \(l_{AB}\), \(l_{CD}\), \(l_{AC}\), \(l_{AD}\), \(l_{BC}\), \(l_{BD}\), and \(l_{AD}\). The maximum length value among the six line segments is then obtained:

$$ \max l = \max (l_{AB} ,l_{CD} ,l_{AC} ,l_{AD} ,l_{BC} ,l_{BD} ) $$
(13)

If \(l_{AB} + l_{CD} < \max l\), then line segment AB and line segment CD are collinear and separated and do not intersect; if \(l_{AB} + l_{CD} = \max l\), then line segment AB intersects with the head or tail of line segment CD; and if \(l_{AB} + l_{CD} > \max l\), then line segment AB and line segment CD overlap locally. A flowchart of the NLOS recognition process is shown in Fig. 7.

Fig. 7
figure 7

Flowchart of NLOS identification.

Full size image

Tightly integrated UWB/MEMS IMU positioning method based on NLOS angle discrimination and MAP constraints

The degree of UWB signal occlusion is judged on the basis of a combination of the map database and the horizontal angle to optimize the tightly integrated UWB/MEMS IMU positioning model. The specific process is shown in Fig. 8.

Fig. 8
figure 8

Flowchart of tightly integrated UWB/MEMS IMU positioning.

Full size image

The UWB measurement values are expressed as follows:

$$ \tilde{l} = l + v_{i} $$
(14)

where \(\tilde{l}\) represents the actual measured value, \(l\) denotes the true value corresponding to the measurement, and \(v_{i}\) represents the measurement error (including ranging outliers, NLOS errors and measurement noise). Accordingly, the variance \(\sigma_{ii}^{2}\) of the ranging values can be expressed as:

$$ \sigma_{ii}^{2} = E\left[ {v_{i} v_{i} } \right] $$
(15)

If there are \(n\) UWB observations in an epoch and the measurement error vector is \({\varvec{V}} = \left[ {\begin{array}{*{20}c} {v_{1} } & {v_{2} } & \cdots & {v_{n} } \\ \end{array} } \right]^{T}\), then the measurement error matrix \(R\) for that epoch can be expressed as follows:

$$ R = E\left[ {VV^{T} } \right] = \left[ {\begin{array}{*{20}c} {\sigma_{11}^{2} } & {\quad \cdots } & {\quad \sigma_{1n}^{2} } \\ \vdots & {\quad \ddots } & {\quad \vdots } \\ {\sigma_{n1}^{2} } & {\quad \cdots } & {\quad \sigma_{nn}^{2} } \\ \end{array} } \right] $$
(16)

In formula (16), the values of the elements on the diagonal of the matrix are determined by the UWB ranging accuracy under LOS conditions. Since observations in the same epoch are independent of each other, to simplify the calculation, the nondiagonal elements can be set to 0.

Since the horizontal angle reflects the degree of occlusion of the UWB signal, a UWB error weakening model can be established by incorporating the map information as follows:

$$ V_{i} = \left\{ {\begin{array}{*{20}l} {v_{i} } \hfill & {n = 0} \hfill \\ {\left\{ {\begin{array}{*{20}l} {v_{i} ,d_{rad} > threshold} \hfill \\ {n\varepsilon *v_{i} ,d_{rad} \le threshold} \hfill \\ \end{array} } \right.} \hfill & {n = 1,2} \hfill \\ { + \infty } \hfill & {n \ge 3} \hfill \\ \end{array} } \right. $$
(17)

In formula (17), \(V_{i}\) represents the measurement noise of the tightly integrated UWB/MEMS IMU positioning model; \(n\) represents the number of line segments in the map database intersecting with the line segment between the UWB rover station and the current reference station; \(v_{i}\) denotes the measurement noise of UWB observations in the LOS case; and \(threshold\) represents the horizontal angle threshold below which the UWB observation error exceeds its nominal accuracy, the value of which is 167°. \(\varepsilon\) is a measurement noise expansion factor, which is an empirical constant that generally takes values in the range of 10–20. Its specific value needs to be determined in accordance with the conditions of the observation environment. Formula (17) shows that when the number of line segments in the map database intersecting with the line segment between the UWB rover station and the current reference station is 0, the UWB observation value is considered a normal observation value, and the measurement noise remains unchanged. When the number of intersecting line segments is 1 or 2, the degree of UWB signal occlusion must be judged. When the horizontal angle is greater than 167°, the ranging value is considered to contain only normal measurement noise. A horizontal angle of less than 167° indicates that the UWB signal is severely occluded and that its measurement noise needs to be adjusted. When the number of intersecting lines is greater than or equal to 3, the UWB signal is considered affected by multiple obstacles during propagation, and the ranging value strongly deviates from the true value. This ranging value is discarded, and the measurement noise is set to infinity.

In Kalman filtering, theoretically, an optimal estimate of the state parameters can be obtained only when the structural parameters of the stochastic dynamic model and the corresponding statistical characteristics of the noise are known. However, in a specific implementation, there will be certain deviations in the acquisition of the above two types of parameters, which can even lead to divergence of the filter in severe cases. It is often unrealistic to estimate all of the noise parameters in the Kalman filter. Because the UWB signal propagation environment is constantly changing as the experimental platform collects data along a preset trajectory, the quality of the UWB measurements also changes; accordingly, the change in the measurement noise is the main factor influencing the whole process. To better allocate the weights between the values predicted by the measurement model and the actual measurement values, the adaptive robust Kalman filter (ARKF) is used in this paper to adjust the measurement noise matrix in real time during the measurement update process to obtain more accurate positioning results.

The calculation formula for the robustness factors \(\gamma_{ii}\) introduced by the filtering method is as follows:

$$ \gamma_{ii} = \left\{ {\begin{array}{*{20}l} 1 \hfill & {\quad \left| {v_{k,i} } \right| \le k_{0} } \hfill \\ {\frac{{\left| {v_{k,i} } \right|}}{{k_{0} }} \times \left( {\frac{{k_{1} - k_{0} }}{{k_{1} - \left| {v_{k,i} } \right|}}} \right)} \hfill & {\quad k_{0} < \left| {v_{k,i} } \right| \le k_{1} } \hfill \\ { + \infty } \hfill & {\quad \left| {v_{k,i} } \right| > k_{1} } \hfill \\ \end{array} } \right. $$
(18)

where \(k_{0}\) and \(k_{1}\) represent empirical constants that take values in the ranges of 2.0–3.0 and 4.5–8.5, respectively, and where \(v_{k,i}\) denotes the innovation value of the \(i\)-th measurement value in an epoch. The innovation value is calculated as follows:

$$ v_{k,i} = \frac{{(Z_{k} - H_{k} X_{k/(k - 1)} )}}{{\sqrt {(H_{k} P_{k/(k - 1)} H_{k}^{T} + R)_{i,i} } }} $$
(19)

When the absolute value of the innovation value is less than \(k_{0}\), the measurement is considered normal, and the value of the robustness factor \(\gamma_{ii}\) is 1. When the absolute value of the innovation value is between \(k_{0}\) and \(k_{1}\), the value of the robustness factor \(\gamma_{ii}\) increases, and the weight of the corresponding measured value decreases. When the absolute value of the innovation value is greater than \(k_{1}\), there is a large measurement error in the measured value, and the robustness factor is set to infinity. The functional relationship between \(\gamma_{ii}\) and \(v_{k,i}\) is shown in Fig. 9.

Fig. 9
figure 9

Robustness factor curve.

Full size image

The following formula is used to introduce the robustness factor \(\gamma_{ii}\) into the matrix \(R\) to adjust its elements:

$$ \tilde{R} = \left[ {\begin{array}{*{20}c} {\gamma_{11} \sigma_{11}^{2} } & {\quad \cdots } & {\quad \gamma_{1n} \sigma_{1n}^{2} } \\ \vdots & {\quad \ddots } & {\quad \vdots } \\ {\gamma_{n1} \sigma_{n1}^{2} } & {\quad \cdots } & {\quad \gamma_{nn} \sigma_{nn}^{2} } \\ \end{array} } \right] $$
(20)

where \(\tilde{R}\) represents the measurement noise matrix after adaptive adjustment. In the formula above:

$$ \gamma_{ij} = \sqrt {\gamma_{ii} \gamma_{jj} } $$
(21)

where \(i\) and \(j\) correspond to the \(i\) th row and \(j\) th column, respectively, of the matrix \(R\). After the above adjustment, the gain matrix \(K_{k}\) is calculated, and then the optimal estimate \(\hat{X}_{k}\) of the state parameters at this time and its variance–covariance matrix \(P\) are obtained.

Results

To verify the positioning performance of the UWB/MEMS IMU compact combination positioning method proposed in this paper, which is based on NLOS angle discrimination and MAP constraints in complex indoor environments, experiments were carried out in the underground parking lot of a shopping mall and multiscene office building via a self-built experimental platform. Since there are many obstacles, such as load-bearing columns, walls, doors and windows, in the experimental scene, different degrees of NLOS occlusion can be created so that the algorithm can be effectively verified. The experimental scene is shown in Fig. 10:

Fig. 10
figure 10

The experimental scene. (a) Experiment I: underground parking lot environment. (b) Experiment II: Multi-scenario office building environment.

Full size image

In the integrated UWB/MEMS IMU positioning system implemented in these experiments, an independent coordinate system was adopted, and the time reference was the number of GPS seconds within a week. The reference trajectory was automatically tracked by a Leica TS50 total station. For the UWB transceiver, the Pulse On 440 module produced by the time domain was used. For the MEMS IMU, the Ellipse-N product produced by SBG was adopted, the sampling frequency of which is 200 Hz. Photographs of the experimental platform are shown in Fig. 11.

Fig. 11
figure 11

Experimental platform: (a) front view, (b) left view, (c) right view.

Full size image

Underground parking lot experiment

In the underground parking lot, an area with a length of approximately 30 m and a width of approximately 20 m was selected. Considering the layout structure of the building in the area, five UWB base stations were selected for deployment. To establish NLOS conditions, several UWB base stations were deployed behind load-bearing columns. The experimental platform moved along a preset trajectory with a shape similar to that of a paper clip in the experimental area. The experiment lasted for 10 min. A relevant schematic diagram is shown in Fig. 12.

Fig. 12
figure 12

The experimental trajectory: a ‘paper clip’ shape.

Full size image

In Fig. 12, the blue line represents the actual movement trajectory of the experimental platform, the red triangles represent the UWB reference stations, and the black rectangles correspond to planar representations of the load-bearing columns.

Figure 13 shows the occlusion of several base stations during the movement of the experimental platform. From left to right, the plots represent base stations 101, 103, and 202. The blue markers indicate the numbers of line segments in the map database intersecting with the line segment formed by the UWB rover and the base station in different epochs. The red markers indicate epochs in which ranging failed, that is, epochs for which there is no ranging value. During the experiment, in addition to the reduction in positioning constraints caused by ranging failure, the UWB signals were frequently blocked by load-bearing columns.

Fig. 13
figure 13

Status of some UWB base stations blocked by obstacles: (a) base station 101, (b) base station 103, (c) base station 202.

Full size image

Multiscene office building experiment

Eight UWB base stations are evenly distributed on the first floor of an office building with a length of 50 m and a width of 25 m. To create certain NLOS conditions, some UWB base stations are deployed behind the load-carrying columns and at the corner of the wall. First, the experimental platform is moved from the awning area at the intersection of the indoor and outdoor areas through the cloister with glass doors and then clockwise in the foyer area for two weeks after passing through the cloister. Then, the platform is moved one round trip down the narrow corridor on the south side, another round trip down the narrow corridor on the north side, and finally, to the foyer, through the cloister, and to the awning; the experiment lasted 25 min. The office building includes an indoor scene, an indoor-outdoor junction scene, a narrow corridor and a cloister with a glass door. A schematic diagram is shown in Fig. 14.

Fig. 14
figure 14

Schematic diagram of the multiscene office building track.

Full size image

In Fig. 14, the blue line represents the actual movement track of the experimental platform, the red triangle represents the UWB reference station, and the black line represents the map database, such as the wall, glass door and load-bearing column.

Figure 15 shows that during the movement of the experimental platform, all the base stations were shielded to varying degrees. The green markers indicate the numbers of line segments in the map database intersecting with the line segment formed by the UWB rover and the base station in different epochs, and the red mark indicates the ranging failure at different epochs.

Fig. 15
figure 15

UWB base station blocked by obstacles. (a) Beacon 101; (b) Beacon 102; (c) Beacon 103; (d) Beacon 104; (e) Beacon 201; (f) Beacon 202; (g) Beacon 203; (h) Beacon 204.

Full size image

Comparative analysis of the experimental results

To verify the tightly integrated UWB/MEMS IMU positioning method based on NLOS angle discrimination and MAP constraints (ARKF + MAP + RAD method) proposed in this paper, the original MEMS IMU and UWB observations were collected simultaneously during the experiment, and the ARKF + MAP + RAD algorithm was compared with three other algorithms. The algorithms considered for comparison were an EKF algorithm based on the UWB data alone, an ARKF algorithm based on the tightly integrated UWB/MEMS IMU data, and a map-assisted tightly integrated UWB/MEMS IMU positioning algorithm (ARKF + MAP). The different algorithms were used to solve the problem, and the whole trajectories and local trajectories obtained with the different algorithms are shown in Fig. 16.

Fig. 16
figure 16

Trajectory comparisons: reference, EKF, ARKF, ARKF + MAP, and ARKF + MAP + RAD. (a) Comparison of trajectories of different algorithms in Experiment I. (a) Comparison of trajectories of different algorithms in Experiment I.

Full size image

The trajectory comparison graphs of the two groups of multiscene experiments clearly reveal that the localization effect is worst when the EKF algorithm is used to solve the UWB alone, and the localization results obviously shift when the experimental platform is in the area where the UWB reference station is obscured, which indicates that there are limitations in using only UWB information when the indoor environment is poor. Compared with the EKF algorithm, the ARKF algorithm has no localization point dispersion and yields a smoother overall local trajectory, mainly because of the addition of the MEMS IMU; additionally, the antidifferential algorithm weakens the effect of the UWB NLOS ranging error on the localization to a certain extent, but there is still a large error in the NLOS region. Compared with the ARKF algorithm, the ARKF + MAP algorithm significantly improves the positioning effect in the NLOS region, and the positioning result of the algorithm is obviously closer to the reference trajectory in the NLOS region, particularly in cases with obstacle obstructions, but some positioning points are unstable in the regions where the UWB signal is beginning to be obstructed and fully obstructed; this indicates that the line segment intersection bars obtained via the UWB transceiver module and matched with the map database are more stable than those obtained via the map database. This indicates the limitation of using only the number of intersecting lines obtained by matching the UWB transceiver module with the map database to determine the degree of occlusion of the UWB signal. Moreover, the NLOS ranging values clearly display large errors. In contrast, the ARKF + MAP + RAD algorithm proposed in this paper yields better localization performance for both the overall trajectory and local positioning.

In addition, to obtain a more intuitive and detailed understanding of the localization effects of different algorithms, the root-mean-square error, average error and maximum error of the four algorithms in the northwards, eastwards and planar directions are determined for the two groups of experiments, and the statistical results are shown in Table 3.

Table 3 Error statistics for the different methods.
Full size table

Table 3 shows that the errors of the EKF algorithm are significantly larger than those of the other three algorithms. In terms of positioning accuracy, compared with the ARKF algorithm, the ARKF + MAP algorithm improves the average localization by 16% and 20% in the Experiment 1 and Experiment 2 scenarios, respectively, and the use of map information to identify the NLOS enhances the localization results. In addition, when the ARKF + MAP + RAD algorithm is compared with the ARKF + MAP algorithm, the maximum localization error is reduced by 22% and 30% in Experiments I and II, respectively. In terms of algorithm complexity, compared with the ARKF algorithm and the EKF algorithm, the longest solving time for a single calendar element of the ARKF + MAP + RAD algorithm grows, and the longest solving time for the Experiment I and II scenario are 3.599 ms and 8.243 ms, respectively, which are much smaller than the data sampling interval of UWB.

Since the UWB signal is occluded at times throughout the experiment, the optimization of the ARKF + MAP + RAD algorithm compared with the ARKF + MAP algorithm is reflected mainly in the points at which the UWB signal is occluded and the horizontal angle is greater than 167°, which tend to be located in the area where the UWB signal begins to be occluded and is slowly recovered; thus, although the statistics indicate good accuracy in some cases, the results do not reflect the actual conditions well. Therefore, to better reflect the superiority of the ARKF + MAP + RAD algorithm, the points where the UWB measurement signals for the two sets of experiments are occluded and the horizontal angle is greater than 167° are counted, and the error curves in the corresponding northwards, eastwards and planar directions are obtained, as shown in Fig. 17.

Fig. 17
figure 17

North, east, and plane errors of the ARKF + MAP and ARFK + MAP + RAD algorithms at positions with partial occlusion. (a) Partial occlusion point error plot for experiment I. (b) Partial occlusion point error plot for experiment II.

Full size image

Figure 17 shows that the accuracy of the ARKF + MAP + RAD algorithm in the north, east, and plane directions is significantly better than that of the ARKF + MAP algorithm at the targeted points. The above results were quantitatively characterized in terms of the root mean square errors, average errors and maximum errors of the ARKF + MAP algorithm and the ARKF + MAP + RAD algorithm in different directions, and the statistical results are shown in Table 4.

Table 4 Error statistics of the results for partially occluded points.
Full size table

As shown in Table 4, compared with the ARKF + MAP algorithm, the ARKF + MAP + RAD algorithm yields significantly improved indicators. In the underground parking lot environment, the improvement in the root mean square value is 18.3%, 26.5% and 20.8% in the northwards, eastwards and planar directions, respectively; the improvement in the average error is 18.2%, 27% and 21.6% in the northwards, eastwards and planar directions, respectively; and the improvement in the maximum error is 22.5%, 29.2% and 25.9% in the northwards, eastwards and planar directions, respectively. In the multiscene office building environment, the improvement in the root mean square value is 37.3%, 34.2% and 35.9% in the northwards, eastwards and planar directions, respectively; the improvement in the mean error is 33.5%, 29.5% and 33.5% in the northwards, eastwards and planar directions, respectively; and the improvement in the maximum error is 38.0%, 52.3% and 37.1%, respectively.

Algorithm 1
figure a

The Integrated UWB/MEMS IMU Positioning system Model

Full size image

Discussion

In this work, three different experimental scenarios were selected for the dynamic NLOS experiments. In accordance with the experimental results, a dynamic NLOS angle discrimination method was proposed on the basis of the conclusion that the smaller the horizontal angle is, the more severe the occlusion of UWB signals. When the horizontal angle is less than 167°, the UWB ranging error is too large, and no ranging value is available. To improve the positioning continuity and stability of integrated UWB/MEMS IMU positioning systems in complex indoor environments, a tightly integrated UWB/MEMS IMU positioning method based on NLOS angle discrimination and MAP constraints was proposed. In this algorithm, the dynamic NLOS ranging error characteristics of UWB signals are introduced into the tightly integrated UWB/MEMS IMU positioning model to optimize the NLOS identification process for UWB signals. Finally, a robust Kalman filter algorithm is used to estimate the position of the target carrier. The experimental results show that in the underground parking lot experimental environment and the multiscene office building experimental environment, the accuracy improvement of the ARKF + MAP + RAD algorithm compared with the ARKF algorithm in the northwards, eastwards, and planar directions is 18.9% and 33.3%, 32.0% and 22.1%, and 22.9% and 28.5%, respectively, in the area where the UWB signal is blocked and the horizontal angle is greater than 167°. The proposed algorithm yields higher accuracy and stability than the ARKF + MAP algorithm in the north, east and planar directions, with improvements of 18.2% and 33.5%, 27% and 29.5%, 21.6% and 33.5%, respectively.