An indoor location algorithm based on Kalman filter fusion of ultra-wide band and inertial measurement unit Open Access

At present, outdoor positioning technology is developing faster and faster in response to market demand. The Global Positioning System (GPS) of the United States and the BeiDou Navigation Satellite System (BDS), Gronus (GLONASS) of Russia and Galileo positioning and navigation system of Europe as the four main outdoor environmental positioning and navigation systems.¹ Taking an example that the most commonly used GPS system, in the outdoor open environment, its positioning accuracy can reach the meter level, which has mainly met the needs of outdoor positioning. However, under the occlusion of buildings, jungles and other obstacles, the signal of these positioning systems is rapidly weakened, or even interrupted, the receiver will not be able to receive the positioning signal, so the accuracy of determination drops sharply, and can not meet the needs of indoor positioning accuracy. This has prompted the Bluetooth, Wi-Fi, ZigBee, ultrasonic, infrared and other indoor positioning technology came into being and rapid development. Bluetooth, Wi-Fi and ZigBee technologies are located by Received Signal Strength Indicator (RSSI).² This positioning method calculates the distance between the receiving end and the transmitting end by the Eq of signal fading intensity and propagation loss. This method is easy to be interfered by external factors, the positioning accuracy is low, and a large number of reference nodes need to be arranged. Its advantage is that the cost of a single node is low, which provides the possibility for large-scale layout of reference nodes. Ultrasonic and infrared technology, mainly according to Time of Arrival (TOA) positioning,³ this positioning method multiplies the propagation speed of electromagnetic wave or acoustic wave by the propagation time of the signal. The distance between the target node and the reference node is calculated. Compared with the positioning method of RSSI, this method has higher positioning accuracy, but it also has corresponding limitations. The principle of ultrasonic ranging is detected by sound waves, and the transmission speed of sound waves is slow, so it will reduce the real-time positioning. In a NLOS environment, sound waves are more likely to be interfered with, and energy is easily absorbed by objects. Infrared positioning is easy to be interfered by light intensity and object color, and the distance can not be detected accurately for color approximate black object.

Ultra-Wide Band (UWB) and Inertial Measurement Unit (IMU) positioning have significant advantages over the above positioning technology. First of all, UWB technology is through the transmission of nanosecond or even subnanosecond pulse signal to ranging, its time resolution is very high, so the accuracy of ranging information is very high, can reach centimeter level. UWB has a large bandwidth, up to G hertz, at the same time has a wealth of low-frequency components, can pass through wood, glass, jungle and other obstacles, and UWB anti-multipath ability is strong. UWB signals pass through materials in different media and show different signal characteristics at the receiving end, which can be used to detect and identify objects. UWB power consumption is low, so the battery life is strong, can not replace the battery for a long time. Many of the advantages of UWB described above are very suitable for anti-terrorism, rescue, emergency and other emergencies, which can not be replaced by other positioning technologies. The accelerometer, gyroscope, angular velocimeter and magnetometer integrated by IMU itself can provide azimuth, acceleration, angular velocity and other motion state and attitude information.⁴ Therefore, IMU does not need any reference node to provide ranging information. It can also realize autonomous positioning and navigation without interference from external signals. Moreover, IMU has the advantages of low cost, miniaturization of sensors, easy integration and popularization and application, and has a great application prospect. Although UWB and IMU have so many advantages, individual sensors still have some limitations. The one hand, UWB requires high hardware to generate, receive and process subnanosecond pulses, so the cost of a single node is high. The other hand, it is difficult to receive the ranging information of three or more anchor nodes all the time. Because of the drift of IMU, acceleration data, the cumulative error in positioning can not be effectively solved. Therefore, if we can break through the traditional, single positioning method, only need fewer anchor nodes, through the use of UWB and IMU joint positioning can obtain high-precision location information, there will be a great deal of application space and market.

A fusion algorithm of inertial measurement unit and UWB based on extended Kalman filter is proposed in Ref. 5. The algorithm realizes the three-dimensional positioning of 80 Hz and improves the positioning accuracy significantly with almost no delay. However, the hardware cost of this paper is high and the robustness is poor. A kind of A Novel Adaptively-Robust Strategy Based on the Mahalanobis Distance for GPS/INS Integrated Navigation Systems is proposed in Ref. 6. A new method for increasing accuracy of distance measurement based on single visual camera is proposed in Ref. 7. Ultra Wideband Indoor Positioning Technologies: Analysis and Recent Advances is proposed in Ref. 8. If there is no DP, signal between the transmitter and the receiver, more additional delay will be introduced in the process of propagation, resulting in NLOS error.⁹ Therefore, it is very important to eliminate the influence of NLOS error to improve the accuracy of ranging and positioning.¹⁰ The research on eliminating the influence of NLOS mainly focuses on two aspects: NLOS identification and NLOS error elimination.¹¹ If we know whether the signal comes from LOS or NLOS environment before processing the received signal, this is helpful to improve the positioning accuracy.¹² If the received signal comes from the LOS environment, the received signal can be obtained without any processing, and the ranging estimation can be obtained directly, so that the position estimation of the tag to be measured with high accuracy can be obtained.¹³ If the received signal comes from a NLOS environment, the received signal may be processed to eliminate or mitigate NLOS errors, or the signal from the NLOS environment may be discarded directly before positioning.¹⁴ The existing algorithms for distinguishing LOS from NLOS can be divided into two kinds: one based on ranging estimation and the other based on channel statistical characteristics.¹⁵ 

In Ref. 16, a NLOS discrimination algorithm based on the Sup-port Vector Data Description (SVDD) is proposed. from the simulation results in this paper, it can be seen that the algorithm is effective in NLOS discrimination. However, the biggest problem of this algorithm is that it takes too long compared with other methods. however, the author has not solved this problem. In Ref. 17, an automatic data updating algorithm for distinguishing LOS from NLOS is proposed. the algorithm must have prior assumptions, and the purpose of the algorithm is to improve the positioning accuracy, so the complexity of the algorithm is not considered. As a result, the algorithm is laborious in automatically updating the data.

At present, UWB positioning is generally divided into two cases: LOS environment and NLOS.¹⁸ And most of them are based on AOA estimates. If the base station can perform both TOA estimation and Direction-of-Arrival (DOA) estimation, only one base station is needed to achieve positioning.¹⁹ In Ref. 20, a UWB location method based on TOA-DOA joint estimation is proposed. In Ref. 21, a three-step joint estimation algorithm is proposed. firstly, threshold correlation method is used for TOA estimation, and then the least mean square algorithm is used for joint estimation of TOA and arrival time difference. Finally, the DOA estimation is obtained by improving the estimation accuracy of the time difference of arrival. In Ref. 22, another three-step joint estimation algorithm is proposed. firstly, the maximum likelihood estimation algorithm is used for preliminary TOA estimation, and then the further joint estimation of TOA and DOA time difference is carried out. Finally, the DOA estimation is carried out by using the time difference of arrival and the geometric triangular cosine theorem, and a two-step joint estimation algorithm is proposed in Ref. 23, that is, the rough estimation of TOA is carried out by using TC algorithm first. Then the estimation accuracy of TOA is improved and DOA estimation is carried out by finding the maximum value of a logarithmic likelihood equation. The algorithm design of Ref. 24 and Ref. 25 needs three steps, and the algorithm is more complex. the estimation accuracy of Ref. 26 algorithm is limited by sampling frequency, and the sampling frequency is Nyquist rate of GHz level, so the complexity of the system is high.

In the multi-base station multi-label positioning has been a large number of scholars in the research.²⁷ They either use multiple tags or use additional hardware.²⁸ that is, directional antennas, for tag location.²⁹ In the past few years, many algorithms have been proposed to distinguish NLOS states.³⁰ For example, in Ref. 31, a combination of Kurtosis (COK), Mean Excess Delay (MED), and Root-Mean-Square (RMS), is proposed to distinguish NLOS environment.³² If this algorithm uses a single variable, it can not correctly distinguish the NLOS environment; if the use of mixed variables, although good results, but the calculation is complex, the efficiency is reduced.³³ A NLOS environment discrimination algorithm based on received signal TOA, RSSI and RMS is proposed in Ref. 34, which can be used to distinguish NLOS environment when the actual distance is known.³⁵ In practical applications, The distance between the reference label and the tag to be tested can not be known in the first, and the variables are too many, the operation is complex and the efficiency is low. In Ref. 36, a NLOS discrimination algorithm based on machine learning is proposed. although the result of this algorithm is good, it is too complex to achieve. In Refs. 37–38, a NLOS discrimination algorithm based on support vector machine (SVM) is proposed, which is effective, but compared with the method proposed in this paper, it is relatively complex and has a large amount of computation in practical positioning applications.

A recent work has proposed that the direction of the label can be determined by using the human body to block the direction of the wireless receiver in the case of maximum signal strength attenuation, but this scheme does not work properly in a complex indoor environment. There is still a lot of room for improvement in the outdoor environment. Other studies using human blocking effects are used to estimate interference from mobile device signals. In addition, the use of additional hardware (i.e. directional antennas) on receivers or transmitters to locate tags involves higher infrastructure costs that are neither scalable nor portable. The research group of Harbin Institute of Technology is one of the earliest research teams engaged in UWB and IMU positioning technology in China, with profound theoretical and practical experience.³⁹ However, in the complex environment, how to automatically identify the number of received UWB anchor nodes and judge the ranging accuracy of the received UWB signal is not given. On this basis, this paper continues to expand and in-depth research, to achieve a complex environment, the use of UWB and IMU joint positioning technology to improve the positioning accuracy in complex environment, for multi-sensor joint positioning to provide more solutions.

In this paper, based on the existing results, a Kalman filter fusion method of UWB ranging information and IMU acceleration information is proposed to obtain more accurate personnel position information. The main contributions and innovations of this paper are as follows:

1. The UWB trajectory tracking model is established. According to this model, the base station is deployed, and the experimental environment of four base stations and one label UWB trajectory tracking is established.

2. Distinguish LOS environment from NLOS environment, and obtain UWB signal information in NLOS environment.

3. According to the transmission law of UWB signal information, the related signal information is extracted, and the maximum likelihood estimation algorithm is used to eliminate the influence of NLOS on the transmitted signal. More accurate UWB ranging information is obtained.

4. Through the extended Kalman filter information fusion strategy, the UWB ranging information and IMU JY901B angle information are fused to achieve accurate positioning in complex environment.

5. Finally, the experimental results show that, compared with single UWB and single IMU positioning, the joint positioning performance proposed in this paper is obviously better than that of a single sensor. Especially in the complex indoor environment, the performance is more obvious, which provides more solutions for UWB accurate indoor positioning.

The following contents are arranged as follows: section II introduces the definition of system model and related theoretical knowledge. In section III, the joint positioning method of UWB-IMU is given. In section IV, the method and performance analysis are verified by experiments. Section V summarizes this article.

In this paper, UWB positioning mainly includes: first, determine the corresponding number of base stations and tags; secondly, calculate the signal arrival time between base stations; then TDOA algorithm by contrasting the signal arrival time difference, can form a positioning network; finally, calculate the distance between nodes.

In the process of positioning, the Chan algorithm based on TDOA is used to solve the label position. The number of positioning base stations has a certain impact on the positioning accuracy of Chan algorithm. When the number of base stations increases to a certain number, the continued increase of base stations has little impact on the positioning accuracy, and the increase in the number of base stations means an increase in the cost of equipment and the amount of data. From the comprehensive consideration of positioning accuracy and equipment cost, it is more suitable to select four base stations. Therefore, this paper uses four base stations. In the two-dimensional plane Cartesian coordinate system, the coordinates of the i base station are $Buwb,i=[xi,yi]T(i=1,2,⋯,5)$⁠, the coordinates of the label are T_uwb = [x₀, y₀]^T, and the NLOS distance between the base station and the label is $Ri=Buwb,i−Tuwb22(i=1,2,⋯,5)$⁠. Taking the first base station as the common reference node, a set of TDOA observations Δt_i,1 (i = 2, 3, 4, 5) is obtained, indicating the difference of signal arrival time between the i and the first base station.

In the Eq, $Δti,10$ is the true value of Δt_i,1, n_i,1 is the measurement error of the system, and n_NLOS,i is the NLOS error.

Let the signal propagation speed be c, and calculate the difference R_i,1 between the distance from the label to the i and the first base station as follows:

According to the characteristics of hyperbolic, three hyperbolic equations R_i,1 = R_i − R₁ (i = 2, 3, 4, 5) about T_uwb can be established, as shown in Eq (3).

The above Eq is generally dealt with by linearization, and the problem is transformed into the solution of linear equations. In the actual case of TDOA observation error, the position of the site is usually estimated based on the double weighted least square method.

The standard TDOA algorithm is a method that emits UWB pulse signal when four base stations are completely synchronized. The arrival time difference of UWB signal sent by each base station, and then combines the position coordinates of the base station to locate the tag. That is, in the process of arriving at the tag, the signals of each base station have to pass through all kinds of complex indoor channels, which are multipath and have NLOS errors. Therefore, the signal from the base station to the tag often takes much longer than the signal pulse time of one base station, and if the protection time slot is not added, the signal of the former base station will collide with the latter one. Causes the label to fail to distinguish between base stations. TDOA algorithm can form a positioning network by comparing the time difference of signal arrival. The positioning network of the TDOA in this paper is shown in figure 1.

The TDOA equation obtained according to the time difference of arrival is as follows:

Where (x_n, y_n) is the coordinate of the base station; (x, y) is the coordinate of the label; t_n is the time when the n base station arrives at the tag; and c is the velocity of the electromagnetic wave 3×10⁸m/s. The TDOA standard algorithm can derive the coordinates of the label from the equations (4). The TDOA standard algorithm needs to keep the clock synchronization between the base stations, the base station sends the signal, and the label receives. Because of the absolute complete synchronization of the base station, the optimal solution can be obtained by mathematical methods, such as the least square method, linear constraints and so on. The tag calculates its position based on the arrival time difference, the protection time slot, and the coordinates of the base station.

However, for the high requirement of TDOA standard algorithm base station complete synchronization, the imprecision of crystal frequency and the influence of ambient temperature, it is vital for the base station to synchronize completely in the initial time. In addition. Even if the clock synchronization of the initial state is realized. Because the different frequency of the crystal oscillator, with the passage of time, the clock will drift and become out of sync again. The traditional synchronization method of standard TDOA is to use network cable or other wires to avoid non-synchronization. Which has limitations, easy to be limited by geographical factors, can not be deployed on a large scale and do wireless clock synchronization, the convenience is very poor, can not be used commercially. In order to solve this problem, TDOA improved algorithm is used to realize wireless clock synchronization. The improved TDOA algorithm is different from the standard TDOA algorithm, which can synchronize the initial clock of the base station in a easier and more convenient way than the standard algorithm. That is to say, the new algorithm further reduces the difficulty of clock synchronization.

The networking model shown in figure 1, D_A0T0, D_A1T0, D_A2T0, D_A3T0 is the distance between the four base stations and label 0, D_A0A1, D_A0A2, D_A0A3 is the distance between base station 0 and other base stations. Base station 0 transmits a signal to the entire network, and other base stations transmit a signal to label 0 as soon as they receive a signal from base station 0. After receiving the signal of base station 0, label 0 records the arrival time of the signal, and then waits until the signal from base station 1, base station 2, base station 3 arrives, Record the arrival time t ≤ 0 from all base stations to tag 0, calculate base station 1, base station 2, base station 3, the time difference of arrival (t₁ − t₀), (t₂ − t₀),⋯, (t_n − t₀) of base station n. Combine their coordinates to calculate their own coordinates. Since tag 0 records the arrival time of base station 0 through the local clock, for the new algorithm. It is equivalent to the same signal starting from the randomly selected base station 0 and then arriving at label 0 through different paths, and their start time is essentially the same, thus realizing the clock initialization synchronization of the base station. The improved TDOA algorithm realizes synchronization in the algorithm layer, so it does not need to deploy network lines or wires to achieve clock synchronization, and can be deployed wireless. therefore, this TDOA algorithm is used for indoor positioning.

The TDOA algorithm lists the equations as follows:

Where: t_n is the time when base station n arrives at label 0: $D∧AnT0=(x∧−xn)2+(y∧−yn)2$⁠; $(x∧,y∧)$ is the estimator of label 0; (x_n, y_n) is the coordinate of base station n; $D∧AnT0$ is the moment estimate of the distance between the base station and the label. Since the distance between base station 0 and base station n can be measured through the site, there is no need for a moment estimator. In the process of TDOA positioning, the tag receives the signal. However, it does not send the signal and does not produce collision interference. label 0 and tag 1 are relatively independent, so the expansion of multiple tags can be realized. The clock synchronization of the base station has been realized by TDOA algorithm, so as to detect the direct path component in the multipath component.

UWB system has the characteristics of high bandwidth and high time resolution, and has the potential to provide high precision positioning. Various parameters can be extracted from radio signals transmitted between different nodes to calculate node locations, such as TOA, AOA, and RSSI. The UWB system used in our experiment is based on the flight time (TOF) estimated by the bidirectional ranging protocol to calculate the distance between node pairs.³⁹ These ranges can be modeled as:

Where $.2$ is the Euclidean distance, p^L represents the position of the target node to be tracked, and $priL$ represents the position of the reference node, r_i notes that, contrary to GPS, The range from the target node to the reference node is estimated sequentially. The reference node is assumed to be a stationary node with a known position. For a 3D tracking system, at least four reference nodes are required to obtain a unique location. However, if you can apply constraints that the target node must be on one side of the reference node, three reference nodes are sufficient for 3D positioning. The measurement noise e_r is assumed to be zero mean Gaussian noise.

However, when the target node moves close to the ground, wall or ceiling, due to multipath and NLOS propagation, there will be outliers that affect the distance accuracy, resulting in a serious decline in distance accuracy.

Another factor that affects the positioning accuracy is the dilution of the position accuracy of the (POOP), that is to say, the geometry of the reference node and the target node. For the trilateral measurement system defined by (S), the expected positioning error consists of two parts: distance measurement error (⁠$σer$ and PDOP factor).

Where RMSE_POS represents the root mean square error of position estimation. PDOP is then defined as:

As shown in Eq 8, positioning accuracy is reduced if it is greater than 1. That is, the higher the value of “PDOP”, the worse the quality of the geometry, and the smaller the value, the better the geometry. Eq 8 is used but only the vertical position error σ_z is considered. PDOP is only affected by the geometry of the reference node and the target node, so the specified position of the given constellation and target node can be predicted.

Where H is the Jacobian matrix of the Euclidean distance given in above Eq.

The most widely used in the field of positioning and navigation is IMU, with the integration and miniaturization of sensors. in recent years, IMU has been widely used in indoor positioning and navigation.^40,41 The main reason is that IMU does not need anchor nodes to assist positioning, which greatly saves costs and reduces positioning complexity. Among them, the most important thing is that in the unknown complex environment, other positioning methods need additional auxiliary equipment, and IMU can carry out positioning and navigation in a short period of time by collecting and processing the data of its own sensors. This advantage is the main driving force for the rapid development and application of IMU.⁴² According to the principle of IMU positioning and navigation, the indoor positioning methods based on IMU are roughly divided into the following two types: first, the integration method. The integration method is to calculate the position, velocity and other state information by integrating the acceleration in three directions of IMU output. Through two continuous integrations, and combined with the angle information of IMU output. However, due to the data drift of acceleration information, the cumulative error will be very large after two consecutive integrations.^43,44 In the case of low speed, the relative value of noise is large, and this error is more serious. In the smaller indoor environment, the error is even up to tens of meters. Therefore, this method is not widely used. The second is PDR (Pedestrian Dead Reckoning). PDR method is based on pedestrian step size, step frequency, direction and other information to estimate the position of the pedestrian. The step size and frequency of the traveller are estimated by the acceleration of IMU, and the steering of the traveller is estimated by the angular velocity and direction. Because the step size and frequency of people of different ages are usually different, this method needs to combine the prior knowledge of pedestrians.⁴⁵ 

This deduction is that the IMU module is carried in a certain part of the human body, and the stride frequency and stride length of the human body when walking are detected and estimated according to the data characteristics collected by the IMU module. Then combined with the angular velocity information, the heading direction of the pedestrian track is calculated, so as to obtain the position, speed and direction of the pedestrian. The resulting schematic diagram of pedestrian trajectory calculation on a two-dimensional plane is shown in figure 2:

As shown in figure 2: the horizontal axis is in the positive direction F and the longitudinal axis direction is N. Assuming that the starting point coordinate of the pedestrian at the initial time t₀ is M₀ (f₀, r₀), moving along the course deviating from the angle α₀ of the due north direction, at the time of t₁, If the walking distance of the human body is s₀, then the coordinates M₂(f₂, r₂), M₀(f₀, r₀) and M₁(f₁, r₁), The relationship between t₁) is as shown in Eq (11) and Eq (12):

After t₁, the pedestrian continues to move northward in the direction of α₁ angle. If the distance of pedestrian motion is s₁, the coordinates of human movement at t₂ are M₀(f₀, r₀). By the same token, Eq (11) and Eq (12) obtain:

And so on, when t_i moment, the coordinate position M_i(f_i, r_i) of the pedestrian in the coordinate system should satisfy the Eq (15) and the Eq (16):

In the actual process, there is a large angle drift error in the IMU module itself, which leads to a large error in the direction between the actual route of the pedestrian track and the measurement route. In order to further reduce the error, this paper studies a positioning method which combines UWB ranging information and IMU angle information to correct the pedestrian heading angle.

Bayesian estimation theory originates from Bayesian criterion in probability theory. In positioning and navigation, bayesian estimation is to use the system model and observations to estimate the motion state of the target. A time-varying system model can be expressed as a dynamic state space equation, and the state information and observation information can be expressed as Eq 17 and Eq 18, respectively.

Equation of state:

Observation equation:

Where k denotes t_k time, X_k represents state quantity, f_k represents the state transition function of the system from t_k−1 to t_k time, ω_k−1 represents process noise. Z_k is the observation, h_k is the observation function, η_k is the observation noise. The state of the system can be estimated by the equation of state and the equation of observation.

The random estimation of the state quantity X_k can be expressed by the probability density function. Bayesian estimation uses the predicted value and the observed value to iteratively calculate the probability density of the state quantity X_k, so as to estimate the position, velocity and other state information. If the probability density function of the state at time t_k−1 is expressed as p(X_k−1|Z_1:k−1), the state at time k is predicted by the state value at the previous time and the system model. Then the joint probability density function can be expressed by the following Eq.

According to the Chepman-Kolmogorov equation in Markov chain, the predicted value of k − 1 can be expressed as Eq (20).

After the observation value Z_k is obtained at k time, the state prediction value is modified, and all the observations from the initial time to the k time are known. From the conditional probability Eq, the probability density of X_k can be expressed as Eq 21.

According to the joint probability density Eq and splitting the observed values, Eq 21 can be expanded into Eq 22.

According to Markov property, if the present state quantity is known, the past state quantity does not play an important role in the system. Therefore, p(Z_k|Z_1:k−1, X_k) can be simplified to P(Z_k|X_k), then Eq 22 can be simplified to Eq 23.

According to Bayesian Eq expansion Eq 23 and reduced, simplified Eq 24.

Therefore, the state prediction and update based on Bayesian estimation framework are expressed as Eq 25 and Eq 26, respectively.

Forecast phase:

Update phase:

Among them, p(Z_k|X_k) is called likelihood probability, which corresponds to the observation information in positioning and navigation. p(X_k|Z_1:k−1) is called a prior probability, which corresponds to the state prediction information of the system. p (Z_k|Z_1:k−1) is a normalized constant.

Therefore, the state estimation depends on the likelihood function and prior probability. A state estimate that can be obtained iteratively from the prediction information and the observation information.

The existing algorithms for distinguishing NLOS states are all based on likelihood ratio test. in this paper, in order to compare with the existing NLOS state recognition algorithms based on K, MED, RMS and the Number of Significant Path (NSP). The likelihood ratio test algorithm is still used. In the process of likelihood ratio test, the probability density distribution function of parameters must be obtained in advance. Such as the existing NLOS state recognition algorithm based on K, MED, RMS and NSP.

In order to verify the positioning accuracy of UWB technology, according to the principle of TOA positioning, the weighted least square algorithm is used to solve the position coordinates, and the positioning system and software are designed. Before designing the system. First, you need to build the map according to the positioning scenario, and then assign the anchor node location. Combined with the research results of NLOS discrimination and error elimination, the actual positioning is carried out. The detailed positioning system process is shown in figure 3.

As shown in figure 3, the positioning system is divided into four parts.

Step1: Likelihood ratio test was used to distinguish NLOS environment from LOS environment.

Step2: The effect of NLOS environment on UWB positioning signal was observed. Then the maximum likelihood estimation algorithm is used to eliminate the influence of NLOS environment. If the obtained UWB signal information is in the reference range, the Step1, is continued or Step1; is returned.

Step3: First, the acceleration and angle information are measured by JY901B navigation and positioning module, and the IMU angle and acceleration information are output by PDR algorithm. Then, the error correction of angle and acceleration information is carried out. Finally, the extended Kalman filtering algorithm is used to process the IMU angle and acceleration information and UWB ranging information. Determines whether the result is within the reference range and returns Step1; if Step4, is in progress.

Step4: The GUI interface of the positioning system displays the current positioning information in real time.

Through the upper computer interface to observe the difference between the actual running trajectory and the ideal motion trajectory, compare the change of error distance and angle information value, so as to obtain more accurate trajectory tracking results.

The existing algorithms for distinguishing NLOS states are all based on likelihood ratio test. in this paper, in order to compare with the existing NLOS state recognition algorithms based on K, MED, RMS and NSP, The likelihood ratio test algorithm is still used. In the process of likelihood ratio test, the probability density distribution function of parameters must be obtained in advance, such as the existing NLOS state recognition algorithm based on K, MED, RMS and NSP.

NSP-based differentiation:

K-based differentiation:

MED-based differentiation:

RMS-based differentiation:

It is known that the PDF of the above parameters satisfies lognormal distribution, and the LOS and NLOS environments can be distinguished by likelihood ratio test. Let $PlosNSP(x)$⁠, $PnlosNSP(x)$⁠, $PlosK(y)$⁠, $PnlosK(y)$⁠, $PlosMED(τ)$⁠, $PnlosMED(τ)$⁠, $PlosRMS(γ)$⁠, $PnlosRMS(γ)$ denote the PDF of NSP, K, MED and RMS in LOS and NLOS environments, respectively. The skewness normal distribution test is shown in figure 4.

As shown in figure 4, the probability that the sample value of the skewness conforms to the lognormal distribution is more than 95%, so it can be considered that the skewness of the channel impulse response can be well fitted as a random variable of the lognormal distribution.

The PDF of channel impulse response skewness can be modeled as:

In the Eq, μ_s, σ_s is the average and standard deviation of ln(s), respectively.

In the actual positioning process, if a person moves, due to the influence of the object, the signal from the target node to the anchor node can not be reached directly, but has to be transmitted or penetrated. The test path distance must be longer than the direct path field. as shown in the previous section, the UWB algorithm without considering NLOS environment obviously can not meet the actual positioning requirements. The error caused may exceed the range of reception, and the influence of NLOS propagation on positioning accuracy must be taken into account. When NLOS propagation exists, there is:

Add relaxation variables to Eq (32), as follows:

Obviously, there is M + 4 unknown in Eq (33), so this is an indeterminate equation.

If the maximum likelihood estimator used in the previous method is used to solve the problem, the singular matrix will be formed. So in this paper, only one relaxation variable is considered as an unknown quantity, and the remaining four relaxation variables are taken as known, so there are:

Let Z_a = [x, y, z, R, v₁, v₂, v_M−4]^T, then the error vector is:

Among them:

By using the maximum likelihood estimation pair (36), it is estimated that:

In the Eq:

Q is a covariance matrix for estimating noise. In practical location, a set of values estimated by TOA algorithm without NLOS interference can be used as the initial value in Eq (38), and then iterated repeatedly to obtain higher precision values. The value of Za calculated above is a function of v_M−3, v_M−2, v_M−1, and Za can be decomposed into:

Among them:

Since v₁, v₂, ⋯, v_M−4 can be expressed as a function of v_M−3, v_M−2, v_M−1, v_M, the constraint $0≤vi≤ri2$ in the equations (40) is:

Among them: C′ = C(5: M, 1:3), Z′ac = Zac (5: M, 1:3)

In most cases, the range of solution space defined by Eq (41) is relatively large, resulting in a great difference between some solutions in the solution space and the true values of the target node, Therefore, this paper needs to find the solution close to the target position to the greatest extent in this solution space. Assuming that the target node calculated without considering NLOS propagation is (x′, y′, z′), the problem can be transformed into a problem of finding the optimal solution. $min(x−x′)2+(y−y′)2+(z−z′)2$⁠, Eq (41) is qualified, where:

The optimization problem of Eq (41) is solved, and then Za and its covariance matrix are obtained by substituting it in (42):

Similarly, x, y, z, R is related to each other in practical application, and then this paper combines this relationship to estimate. Assuming that the estimation errors of x, y, z, R are e₁, e₂, e₃, e₄, the following are:

Set vector error:

Among them:

Substitute Eq (44) and (45), available:

If the above Eq holds when e₁, e₂, e₃ is very small, then the covariance matrix of φ′ is:

Among them: B′ = diag{x⁰, y⁰, z⁰, 0.5}, P = cov(Za)(1:4, 1:4), Then similarly, the maximum likelihood estimate of Z_p is:

The final estimated position is $Z=Zp$ or Z = −$Zp$⁠, and the positive and negative signs are consistent with the corresponding symbols of Za.

Suppose the equation of state of a discrete control process system is:

Where y_n+1 is the clock state of time n+1; ω_n+1 is the Gaussian random variable of process noise; u_n+1 is the control quantity of n+1 time to the system; M and N are system parameters, and for multimode systems, they are matrices. The corresponding observation matrix is:

In the Eq: Z_n+1 is the measured value of n+1 time; H_n+1 is the unit matrix; v_n+1 is the Gaussian random variable of measuring noise. The process model of the system is used to simulate and predict the next state. The iterative process of Kalman filtering principle is as follows:

Estimate the current state based on the previous state of the system:

Where $y∧n+1|n$ is the result of prediction using the previous state; y_n|n is the optimal result of the previous state; and u_n+1 is the control quantity of the present state.

The minimum mean square error matrix is:

Where P_n|n is the minimum mean square error matrix of the estimate y_n|n; P_n+1|n is the covariance of $y∧n+1|n$⁠; and Q is the covariance of the system process. Kalman gain is:

Where R_n+1 is the covariance matrix of the observed noise u_n+1. The optimal estimate of the K + 1 state is:

Maintain the continuity of the system until the end, update the covariance of $y∧n+1|n+1$ in state n + 1:

After Kalman filter modified $y∧n+1=θ∧n+1γ∧n+1T$⁠, so that $un+1=y∧n+1$ to compensate for clock drift so that the clock between the master and slave base stations to maintain synchronization. The calculation of Kalman filter is based on the assumption that all the measurement results are composed of real signal and additive Gauss noise. If the above assumptions are true, Kalman filter can effectively obtain signal information from the measurement results with noise.

One of the Y information matrices defining the Kalman gain matrix is:

Where Y represents the difference between the actual clock deviation and the predicted clock deviation. Can be used to indicate how well the current input matches the current filter state:

In Eq (57), if the Outlier Metric (OM) is greater than a preset threshold, the current input is considered untrusted, the current state is not updated, and the data is discarded directly. In order to avoid the error packet on the filter has a great impact.

Through the above complete process, the interference caused by NLOS environment and synchronous clock deviation can be eliminated.

After all measurements are represented in the same coordinate system, Kalman filtering is applied to them. Kalman filter is a recursive stochastic technique, which estimates the state n ∈ ℜx f a dynamic system from a set of incomplete and noisy measurement data. The dynamic system is modeled by a state transition equation (state model) at the following time k:

In the above Eq, M is the state transition matrix of n×n, M is the n×p matrix, u is the vector with system input p×1, w is the process noise vector n×1 with covariance matrix k (zero mean multivariate normal distribution), ω is other noise.

In the work, the state vector x consists of the coordinates p = (x, y, z) of the user’s global position in the environment. M is a 3 × 3 identity matrix to directly contain the measurement results of IMU JY901B; N is an empty matrix because there is no control input. Because the state vectors are not correlated, the process noise covariance matrix K is a diagonal matrix. The diagonal term of the matrix corresponds to the average error of IMU JY901B measurement. The $z∈Rm$ of sensor measurement at time k is modeled in KF by the following equation (measurement model):

In Eq (59), H is a m × n observation matrix indicating how the state of the system is recorded by the sensor, and v is the m × 1 measurement noise vector (with a zero mean multivariate normal distribution of the covariance matrix R_k). In this paper, H is a 3 × 3-identity matrix and R is a diagonal matrix, whose diagonal term corresponds to the average error of the positioning system measurement.

Kalman filtering algorithm consists of two steps: prediction step and correction step. In the prediction step, the prior estimation $P^k$ (Eq 60) and the error covariance matrix p_k of the user’s global position are obtained by combining the position measurement from IMU JY901B.

Finally, the transformation matrix $TGU$ is recalculated by the position estimation $P^k$ obtained by Eq (62). Therefore, the prediction step will be performed at the IMU JY901B rate, while the correction step will be performed using PDR.

The experimental environment selected in this paper is a laboratory full of obstacles, the size is set to 3 m *3 m, in order to verify the signal transmission law of the proposed algorithm in complex environment and the performance of joint positioning of UWB and IMU. The experimental platform we selected is the I-UWB LPS positioning system, and the specific hardware is shown in figure 5 (a). On the basis of INFAssistant serial port debugging terminal provided by Infinite Future official website, INFAssistant, is improved to realize the functions of real-time updating of motion track, loading of experimental environment map, two-dimensional and three-dimensional display of signal waveform, serial port debugging and so on. It provides a more intuitive form of expression for trajectory tracking. At the same time, through the improvement of our team, the system has achieved the estimation and tracking accuracy of a single tag 5cm. The JY901B inertial navigation module used by the IMU module is shown in figure 5 (b). JY901B adopts high performance microprocessor and advanced dynamic solution and Kalman dynamic filtering algorithm, which can quickly solve the current azimuth, acceleration, angular velocity and other information of the module. JY901B can transmit data through Bluetooth in real time, which can transmit acceleration and angle information to the upper computer. The Bluetooth module uses a MPU6050, transmission rate set to 20 Hz.

In order to show the positioning results more intuitively, the IOT UWB positioning system software is designed and developed. The software is written in C #, and the GUI interface displays the trajectory tracking results in real time.

The positioning results of this paper are obtained by combining UWB ranging information with IMU acceleration and angle information, in which UWB ranging information can get more accurate results through the accurate movement of tags. Therefore, the accuracy of the positioning results mainly depends on the acceleration and angle information of the IMU. The CDF diagram of acceleration and angular velocity information measured by IMU inertial navigation module is obtained by experiment, and the comparison is shown in figure 6.

The acceleration and angular velocity information of IMU can be seen from figure 6. Figure 6 the acceleration information measured by (a) IMU, from its CDF diagram, shows that the acceleration is characterized by uniform acceleration, and the change range is relatively small, which indicates that the acceleration information can be accurately measured under the action of the IMU inertial navigation module, It provides theoretical support for UWB precise positioning. Figure 6 (b) is the angle information measured by IMU, from the CDF diagram, it can be seen that during the walking process of the tester, the angle change between the position of the person and the actual spatial Cartesian coordinate system presents the law of periodic change because of the uniform acceleration motion of the person. Through the IMU inertial navigation module to measure the acceleration and angle information in the process of walking, the personnel position can be accurately estimated, the UWB position information can be fused, and more accurate position information can be obtained.

In order to study the distribution of acceleration, the effective acceleration information is extracted, and the scatter diagram of acceleration is shown in figure 7. The scatter plot of the angle is shown in figure 8.

Figure 7 shows the distribution law of the scattered points of acceleration. in the first stage, the personnel start, the acceleration is not obvious, the data distribution is relatively scattered, and the position of the personnel is not fixed; in the second stage, the personnel walk smoothly and the acceleration changes show regularity. The data distribution is more centralized; in the third stage, the walking of personnel is over, the acceleration is reduced, and the distribution of scattered points is more concentrated. Figure 8 shows the distribution pattern of the scattered points of the walking angle. in the first stage, the personnel start, the angle changes to 0, the data distribution is scattered and irregular, and the position of the personnel is at the starting point. In the second stage, the personnel walked smoothly and the angle changed regularly. The data show centralized and uniform distribution; in the third stage, the walking of personnel is over, the angle is reduced, and the distribution of scattered points is irregular. By comparing the scatter plot of figure 7 and figure 8, it is obvious that JY901B can obtain more accurate results of acceleration and angular velocity information of people during walking, which provides a reference for accurate positioning of IMU and UWB information fusion.

Because the walking process of the measured person is divided into three parts: the beginning, the middle and the end, the time series change of acceleration in different stages is also obviously different, and the time series diagram of acceleration information is obtained as shown in figure 9. The spectrum is shown in figure 10.

The temporal variation trend of acceleration in three different stages can be seen from figure 9. In the first stage, the initial stage personnel walk slowly, the acceleration is almost 0, the amplitude basically does not change with time, and fluctuates about minus 0.5, indicating that the acceleration value is small; In the second stage, the acceleration of the acceleration stage increases obviously. The amplitude increases; the third stage, the deceleration stage, enters the amplitude value change which is completely opposite to the first stage, at this time the amplitude change is the largest. From the acceleration amplitude variation law of figure 9, in order to obtain the complete acceleration information change law, remove the acceleration information of the first and third stages, and only retain the acceleration information of the second stage. The correlation can reach 74.99%, and the effective acceleration information is the most, so the more accurate the acceleration information is. Figure 10 shows the acceleration spectrum, which shows the trend of acceleration in the three stages more intuitively, which further confirms that the second stage is the best acceleration extraction period.

It is also divided into three stages to study the changing trend of the angle value at this time, and the time series diagram of the personnel angle information is obtained as shown in figure 11. The spectrum of the angle information is shown in figure 12.

The temporal variation trend of angle information in three different stages can be seen from figure 11. The correlation of the third stage is as high as 85.52%, which indicates that the eigenvalues of angle information are obviously easy to extract at this time. In the first stage, the angle between the motion position and the initial coordinate is 0, and the data at this time have no reference value, and in the second stage, the angle between the approximately uniform acceleration motion coordinate and the initial coordinate remains a fixed value, and the angle between the motion position and the initial coordinate is 0, and the angle between the personnel approximately uniform acceleration motion coordinate and the initial coordinate is kept unchanged. In the third stage, the personnel change from uniform acceleration to deceleration until they return to the origin of coordinates, and the angle information is the most representative. By comparing the angular velocity spectrum of figure 12, it is observed that the change trend of angle information in the third stage is normal and representative, which provides more accurate angle information for information fusion.

In order to further verify the trajectory tracking effect of joint positioning, two sets of comparative experiments are designed to verify the effectiveness of the proposed algorithm. The first set of experiments: let the testers move along the delineated area in an elliptical trajectory, as shown in figure 13, including two cases where the speed is 1m/s as shown in figure 13 (a) and the speed is 2m/s as shown in figure 13 (b).

As shown in figure 13, the actual motion trajectory is not very different from the ideal motion trajectory in the elliptical motion trajectory, mainly because in the elliptical motion trajectory, it is mainly the acceleration that affects the tracking effect of the trajectory. Although the walking speed of the measured personnel is different, the acceleration remains basically unchanged, and the error value of IMU processing acceleration information is relatively small, so the actual motion trajectory is close to the ideal trajectory.

The second set of experiments: let the testers move along the zigzag trajectory along the delineated area, as shown in figure 14, including two cases where the speed is 1m/s as shown in figure 14 (a) and the speed is 2m/s as shown in figure 14 (b).

As shown in figure 14, the difference between the actual motion trajectory and the ideal motion trajectory is obvious in the Z-shaped motion trajectory, which is mainly due to the change of the angle size that affects the tracking effect of the Z-shaped motion trajectory. Because the acceleration of the measured personnel remains basically unchanged, the error value of IMU processing acceleration information is relatively small, so the effect of positioning is affected by the change of the angle between the coordinates and the original coordinates in the process of walking. With the increase of the angle, the angle information error of IMU processing is relatively large, and the actual motion trajectory and the ideal motion trajectory appear a relatively large error.

Comparing figure 13 and figure 14, it can be seen that the final trajectory tracking effect is mainly affected by acceleration and angle size, in which the angle size has a greater impact. In the experiment, we assume that the height of the handheld label is always the same as the person moves, so the Z value is always 100cm. Comparing the experimental results of the four experiments, we know that the minimum error distance is 5.5cm when the Z shape walks at the speed of 2m/s. This far exceeds the accuracy of single-sensor positioning, indicating that multi-sensor fusion positioning technology has a better performance in improving positioning accuracy.

Kalman filter is used to fuse UWB ranging information and IMU angle information to obtain more accurate personnel position information. The most important factors affecting the measured personnel location information are the number of samples collected and the number of spatial anchor node deployment. The priority of this method is also reflected in these two aspects, which show better performance than traditional Zee and Unloc (no additional device support is required). Therefore, the next two algorithms are compared with the two algorithms on the number of samples and the difference of anchor nodes. The error rates of the three algorithms with different sample numbers are shown in figure 15. The location error rate comparison diagram of the three algorithms with different number of anchor nodes is shown in figure 16.

Figure 15 shows the trend of positioning error rates of the three algorithms when the number of samples is 300, 600, 900, 1200, respectively. As shown in figure 15 (a), when the sample number is 300, the UWB-IMU location error rate is slightly higher than that of two of the algorithms, because the ranging information of the UWB is easily affected in the case of a small number of samples, Therefore, it is impossible to obtain more accurate ranging information. On the other hand, the error rate of Unloc algorithm is the lowest at this time, showing the best performance. When the sample number is 600, that is, as shown in figure 15 (b), the error rate of the fusion algorithm in this paper is obviously lower than that of Zee and Unloc algorithms, that is, when the acceleration of the second stage measured by IMU is fixed, the fusion effect is the best at this time. When the sample number is 900 and 1200, the error rate of the proposed algorithm is relatively stable, and the performance is stable compared with Zee and Unloc algorithms. In summary, when the number of samples is different, the proposed algorithm is obviously superior to Zee and Unloc when 600∼1200 samples or more samples are taken.

Figure 16 compares the variation of the error rate of the three methods at different numbers of anchor nodes in space. As shown in figure 16 (a), when the number of anchor nodes changes, the error rate of the proposed algorithm is much lower than that of the Zee and Unloc algorithms, followed by the Unloc algorithm, and finally the Zee algorithm. Especially after the number of anchor nodes is 80, the error rate of the algorithm is significantly reduced, the best error rate is less than 0.1%, that is, the accuracy is as high as 99.99%. As can be seen from figure 16 (b), when the number of anchor nodes is randomly distributed, the error rate of the proposed algorithm is also relatively lower than that of the other two algorithms, followed by the Zee algorithm, and finally the Unloc algorithm, compared with figure 16 (a), In this paper, the algorithm is less affected by the order of the number of anchor nodes or not, and all of them show good performance. However, the performance of Zee algorithm and Unloc algorithm is obviously affected by this factor. In summary, the Kalman filter fusion algorithm processed by the algorithm synthesis and UWB ranging information and IMU acceleration angle information, the position information is more accurate.

From the content of the previous section, it can be seen that compared with the previous positioning algorithm, the algorithm in this paper shows better performance. In this section, we compare the proposed method with single UWB positioning effect and single IMU algorithm. The experiment is carried out by the commonly used control variable method. Firstly, the running time of the three algorithms is compared, and the running time of the three algorithms is compared with figure 17 when the number of anchor points is different.

As can be seen from figure 17, the running time of the three algorithms becomes longer with the increase of the number of samples, but the running time of the algorithm in this paper is always less, followed by the UWB method, and finally the single IMU method. It shows that the effect of increasing the number of samples on the single IMU is mainly due to the fact that the acceleration and angle information, which measured by IMU is relatively less accurate than the number of small samples with the increase of the number of samples. The more speed data directly affects the position determination effect of the navigation module of the final JY901B.

Similarly, the positioning accuracy of the three methods is significantly different when the number of samples is different, and the accuracy comparison diagram is shown in figure 18.

Figure 18 shows the accuracy comparison effect of the three methods. As shown in figure 18, the positioning accuracy of the single UWB and single IMU algorithms decreases significantly with the increase in the number of samples, Especially after the number of samples exceeds 500, the accuracy has been less than 50%, indicating that the location information obtained at this time has seriously deviated from the actual position of the person moving. However, the accuracy of the UWB and IMU methods fused by Kalman filter is almost negligible due to the number of samples, and the accuracy is significantly higher than that of the other two methods.

In this paper, 1200 pieces of data are selected and divided into 8 groups, each group is 150. the CDF diagram of the data of the three methods is shown in figure 19.

Figure 19 shows a CDF diagram of eight sets of data error distances in three ways. Comparing 19 figure (a), (b) and (c), we can see that the data error rates of the sixth, seventh and eighth groups are basically the same, because at this time in the third stage, the testers walk slowly and the data influence reaches the peak. Comparing the three diagrams, we can see that there is a significant gap between the first group and the second group, and the error rate of the method in this paper is significantly lower than that of the other two methods. In the third group, the fourth group and the fifth group, the error rate of the algorithm in this paper remains basically unchanged, and the distance error is about 0.4 m, while the error rate of the single UWB method is 0.5m∼0.8m. The error of the single IMU method is relatively large due to the increase of data. The error distance is obviously higher than 0.5 m. In summary, the positioning error distance of this method is less than 0.4 m, and the effect is obviously higher than that of single UWB and single IMU positioning, which further proves that multi-sensor fusion has a good effect on improving the positioning accuracy.

In summary, through the comparative experiments as shown in figure 18 and figure 19, it can be seen that the location information obtained by the fusion multi-sensor location method is not greatly affected by the number of samples or other factors, but shows a more stable performance. It shows that the positioning method of fusion multi-sensor has good processing performance in improving the positioning accuracy, which provides an updated solution for multi-sensor joint positioning.

In complex environment, the serious attenuation or even interruption of the positioning signal will lead to the reduction of positioning accuracy and even positioning failure. The positioning method of a single type of sensor can not fundamentally solve this problem, and combined with the advantages of various sensors for joint positioning. Which is a feasible method. Because UWB signal has high ranging accuracy and strong penetration ability, IMU can carry out autonomous positioning and navigation according to acceleration, angle and other information, so this paper combines the advantages of ultra-wideband and IMU to achieve accurate positioning in complex environment. Firstly, the signal transmission law in complex environment is obtained by distinguishing LOS from NLOS environment. secondly, the maximum likelihood estimation algorithm is used to eliminate the influence of NLOS on the transmitted signal, and then the extended Kalman filter information fusion strategy is used. The ranging information of UWB and the angle information of IMU are fused to realize the accurate positioning of UWB in complex environment. Finally, the experimental results show that the performance of the joint positioning method proposed in this paper is obviously better than that of a single sensor compared with single UWB and single IMU positioning. It provides more solutions for UWB precise indoor positioning. However, the cost of UWB is relatively high, and the power supply problem is difficult to solve, so the research focus of future work based on IMU and PDR positioning algorithm.

This work was supported by the National Natural Science Foundation of China under Grant No. 61762079, and No. 61662070; the Key Science and Technology Support Program of Gansu Province under Grant No. 1604FKCA097 and No. 17YF1GA015.