An Indoor Localization Algorithm of UWB and INS Fusion based on Hypothesis Testing

Abstract

With the rapid development of information technology, people's demands on precise indoor positioning are increasing. Wireless sensor network, as the most commonly used indoor positioning sensor, performs a vital part for precise indoor positioning. However, in indoor positioning, obstacles and other uncontrollable factors make the localization precision not very accurate. Ultra-wide band (UWB) can achieve high precision centimeter-level positioning capability. Inertial navigation system (INS), which is a totally independent system of guidance, has high positioning accuracy. The combination of UWB and INS can not only decrease the impact of non-line-of-sight (NLOS) on localization, but also solve the accumulated error problem of inertial navigation system. In the paper, a fused UWB and INS positioning method is presented. The UWB data is firstly clustered using the Fuzzy C-means (FCM). And the Z hypothesis testing is proposed to determine whether there is a NLOS distance on a link where a beacon node is located. If there is, then the beacon node is removed, and conversely used to localize the mobile node using Least Squares localization. When the number of remaining beacon nodes is less than three, a robust extended Kalman filter with M-estimation would be utilized for localizing mobile nodes. The UWB is merged with the INS data by using the extended Kalman filter to acquire the final location estimate. Simulation and experimental results indicate that the proposed method has superior localization precision in comparison with the current algorithms.

1. Introduction

With the gradual advancement of the Web, people's requirement of indoor positional servicing is becoming more and more intense. At present, in exterior surroundings, based on global positioning system (GPS) or cellular mobile network, positioning and navigation technology has been relatively mature. However, there are many obstructions and barriers in the indoor surroundings, the signal of the satellite or cellular network will be vulnerable. Therefore, indoor positioning can’t be realized through GPS or cellular mobile network. In cases where the straight route between the mobile node (MN) and the base station (BS) is obstructed by impediment, the radio waves can only be reflected and diffracted to arrive at the receiving station. At this time, the measured data will not correctly reflect the actual separation from the transmitter to the receiving device. This phenomenon is referred to as non-line-of-sight propagation. At the same time, the way radio waves travel in straight lines is called line-of-sight (LOS) propagation. NLOS propagation significantly affects the localization precision. So improving the precision of interior localization has become the direction of many scholars' efforts.

As a research hotspot, Wireless sensor networks(WSN) is being extensively utilized to military applications, object following, environmental detection, space exploration and other fields [1,2]. At present, the main localization approaches which are based on wireless sensors are time of arrival (TOA) [3], time difference of arrival (TDOA) [4], and Received Signal Strength Indication (RSSI) [5]. Ultra-wide-band(UWB) technique is a newly emerged wireless propagation technique. It tackles the primary issues concerning propagation that have troubled the conventional wireless techniques over several decades. It has the virtues of being desensitized to channel fading, transferring signals with reduced spectrum intensity of power and providing centimeters of localization precision. However, in the face of NLOS, its positioning effect is greatly reduced. INS [6] works without depending on outside messages, nor does it emits power to the external environment. And it is not vulnerable to being disturbed. It is an independent guidance service. It utilizes inertial components (accelerometers) to gauge the carrying body's self acceleration. And it gets the velocity and location through the integral and calculation in order to realize the aim of the carrying body's guidance and localization. Therefore, the localization error grows over time and the long-term precision is inferior because of the integrative nature. The combined use of UWB and INS achieves the demand of high precision localization.

In recent years, interior positioning algorithms have emerged one after another. In [7], the author proposes an interactive target tracking which uses Markov model to realize the conversion between los and nlos states. And it reduces the measurement distance accuracy due to nlos through mode probability update. In [8][9], the author replaces the nlos extended Kalman filter with a robust extended Kalman filter, and uses M-estimation to enhance the localization precision. In [9], the author makes further improvement and adds fuzzy adaptive. However, the disadvantage of Kalman filter is that the target tracking will be lost when the moving target is blocked for a long time. When NLOS is large, an improved generalized probability data association algorithm proposed in [10] can better alleviate the estimated position error caused by NLOS. Inertial navigation system, which is not dependent on any exterior message, nor does it emits power to the exterior. Integration of various technologies is highly beneficial in enhancing the precision of indoor positioning. In [11],[15]and [18], UWB/INS are integrated for positioning. In which, UWB and particle filter are used for position estimation. And the zero-speed update algorithm of INS is used to obtain navigation information. In [12], the author proposed a UWB/INS loosely coupled algorithm. It uses anticipatory unbiased finite impulse response (UFIR) filter to fuse UWB and INS data. Different from [12], [13] proposed a UWB/INS tight-coupling algorithm. In which, it adds the atlas, and uses adaptive robust extended Kalman filter for adaptive adjustment of UWB measurement noise. Song et al.[14] proposes a UWB/INS tight coupling algorithm based on factor graph optimization. Jiang et al.[16] introduces the UWB/INS/Global Navigation Satellite System (GNSS) tight coupling integrated positioning method. It realizes accurate positioning both indoors and outdoors and ensured the continuity of positioning. The difference between [17] and [16] is that loosely coupled integrated positioning is adopted. Wang et al. [19] proposes the fusion method of UWB and inertial measurement unit [20], and [19] realizes the final positioning through particle filtering. In [21], the authors propose an algorithm based on Wifi, encoder and Inertial Measurement Unit (IMU). And they design an adaptive extended Kalman filter to integrate the obtained distance measurement messages. In recent years, deep learning has slowly entered people's vision. And scholars have also used machine learning and deep learning for indoor positioning. In [22], the authors apply deep learning to device-free localization. In [23], the author uses fingerprint enhancement to apply deep learning to indoor localization. Sung et al. Deng et al. [24] uses deep residual networks and other networks for indoor positioning. However, mathematical methods can also be used to locate well. As mentioned in [25], the author puts forward two methods to determine whether prior knowledge is known. In the case of prior knowledge, the vehicle is located by decision test. While in the case of unknown NLOS, the hypothesis test is utilized to recognize and locate vehicle.

Wifi-based fingerprinting methods [26][27] have gained extensive applications in interior localization Lately. Localization utilizing fingerprints is typically divided into two phases: offline and online. During the offline phase, surveys are conducted in a specified region to gather fingerprints from different places and create a databank. The collected data is called the training set and is used to train the model. During the online phase, system estimates the position of the moving equipment. Fingerprint positioning decreases the computing complications, obtains the core features of RSS, and improves the location performance. However, fingerprint location requires very tedious data acquisition work, and might have to be regularly refreshed with the surrounding variations. Furthermore, owing to the sophistication of radio transmission, the acquisition of position fingerprints is not a simple issue.

In this paper, a UWB/INS joint location approach based on hypothesis testing is presented. And fuzzy C-means clustering is performed on UWB data before fusion, which is used for indoor location in complex environment. The major contributions of the article are presented as following:

(1) A joint positioning method based on hypothesis testing is proposed, which does not need the previous information about the mobile terminal (MT). Through hypothesis testing, the anchor node where the NLOS path is located is picked out and abandoned. And the remaining anchor nodes are utilized to obtain the location of the mobile terminal by least square method.

(2) UWB data processing adopts fuzzy C-means, and divides the data adaptively. It can better deal with data overlap and fuzziness, and enhance the positioning precision.

(3) In scenarios where the NLOS is high, the proposed algorithm is equally applicable. And the positioning accuracy of the mobile terminal will not be greatly affected.

The article is structured according to the following. Section II describes UWB and INS. Section III details the proposed methodology in this article. Section IV introduces simulation and experiment and Section V presents conclusions.

2. UWB and INS introduction

2.1 Signal model and UWB principle

Suppose there are M base stations (BS) near the mobile node (MN) to locate MN. The state vector of MN is usually modeled as ψ(t) = [x(t), y(t), V_x(t), V_y(t)]' · [x(t), y(t)] represents the position vector of MN in two-dimensional space at time t. And [V_x(t), V_y(t)] represents the two-dimensional velocity vector of MN at time t. MN is moving in two dimensions. The variation of its state vector can be modeled as

Ψ(t)=BΨ(t-1)+CΩ(t-1)

\(\begin{align}B=\left[\begin{array}{cccc}1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 1\end{array}\right] \quad C=\left[\begin{array}{cc}0.5 \Delta t^{2} & 0 \\ 0 & 0.5 \Delta t^{2} \\ \Delta t & 0 \\ 0 & \Delta t\end{array}\right]\end{align}\)      (1)

where, ∆t is the time interval. We assume that the process noise Ω(t) is Gaussian white noise with zero mean and the covariance matrix is Q(t) . B is the state transformation matrix of the MN. C is the noise import matrix. The distance measurement value between MN and M base stations at time t is D(t) = [d₁(t), ...d_m(t)]^T. where, d_m(t) is the estimated TOA value of the mth BS and MN multiplied by the speed of light, the distance measurement value can be expressed as

t_TOA = t_TRUE + n_TOA       (2)

d_m(t) = c⋅t_TOA       (3)

where, t_TRUE is the actual range from BS to MN. n_TOA is the Gaussian white noise with the zero mean. c is signal propagation speed. Formula (3) is the TOA principle of UWB ranging.

\(\begin{align}d_{m}(t)=\left\{\begin{array}{ll}r_{m}(x(t))+\Phi(t) & \text {, if los } \\ r_{m}(x(t))+n_{\text {NLOS }}+\Phi(t) & \text {,if nlos }\end{array}\right.\end{align}\)       (4)

where, Φ(t) is Gaussian white noise with zero mean ,subject to N(0,1²). n_NLOS is the NLOS error and has different distribution in different environments. r_m(x(t)) is the true distance between the M base stations and MN. And the Euclidean distance between the mth BS and MN, defined as

\(\begin{align}r_{m}(x(t))=\sqrt{\left(x(t)-x_{p}\right)^{2}+\left(y(t)-y_{p}\right)^{2}} \quad, p=1, \cdots, M\end{align}\)       (5)

where, (x(t), y(t)) is the two-dimensional coordinate of MN at time t. (x_p, y_p) is the coordinate of the mth BS. And the Euclidean distance between MN and m base stations is r(x(t)) = [r₁(t), ⋯, r_M(t)]'.

2.2 INS Principle

This section designs INS navigation schemes in indoor environments based on the following assumptions [28].

1. Assume that the earth parameters are constant under limited indoor conditions.

2. The angular speed of the Earth's rotation is ignored because it is not sensitive to gyroscopes.

In addition, the error equation of state is adopted. Because it is close to the original state, parameter singularities are less likely to appear. In addition, the error state is always small. It means that the second-order product is negligible, makes it possible to calculate the Jacobian matrix quickly.

Starting from a known starting point, the mobile terminal adopts three-dimensional Cartesian coordinates to describe the mobile terminal's moving state. When the refresh time of the inertial navigation system reaches each time, the attitude message of MN is obtained from the Angle information given by the gyroscope. Here, the Euler Angle attitude vector is used to describe the attitude of MT. E = [φ, θ, ψ], φ is roll angle. θ is pitch angle. ψ is yaw angle. The original gyroscope data is converted into the form of the earth coordinate system. And the rotation matrix of the attitude Angle information is shown in (6), (7), and (8). The direction cosine matrix (DCM) is obtained by Angle information, and the initial DCM matrix is shown in (9).

\(\begin{align}A_{\varphi}=\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \varphi & \sin \varphi \\ 0 & -\sin \varphi & \cos \varphi\end{array}\right]\end{align}\)       (6)

\(\begin{align}A_{\theta}=\left[\begin{array}{ccc}\cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta\end{array}\right]\end{align}\)       (7)

\(\begin{align}A_{\varphi}=\left[\begin{array}{ccc}\cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1\end{array}\right]\end{align}\)       (8)

A = A_φ· A_θ· A_Ψ       (9)

Compared with Euler Angle, the phenomenon of universal joint deadlock does not exist in quaternions. Therefore, quaternion is employed to express the attitude information. The initial quaternion is shown in (10),

\(\begin{align}b=\left[\begin{array}{l}b 1 \\ b 2 \\ b 3 \\ b 4\end{array}\right]=\left[\begin{array}{l}\cos \frac{\psi}{2} \cos \frac{\theta}{2} \sin \frac{\varphi}{2}-\sin \frac{\psi}{2} \sin \frac{\theta}{2} \cos \frac{\varphi}{2} \\ \cos \frac{\psi}{2} \sin \frac{\theta}{2} \cos \frac{\varphi}{2}+\sin \frac{\psi}{2} \cos \frac{\theta}{2} \sin \frac{\varphi}{2} \\ \sin \frac{\psi}{2} \cos \frac{\theta}{2} \cos \frac{\varphi}{2}-\cos \frac{\psi}{2} \sin \frac{\theta}{2} \sin \frac{\varphi}{2} \\ \cos \frac{\psi}{2} \cos \frac{\theta}{2} \cos \frac{\varphi}{2}+\sin \frac{\psi}{2} \sin \frac{\theta}{2} \sin \frac{\varphi}{2}\end{array}\right]\end{align}\)       (10)

The state vector of joint positioning by UWB and INS is X = [E^T V^T P^T b^T_ω b^T_α], E = [φ, θ, ψ]. v = [v_n v_e v_d] is the velocity vector. P = [P_n P_e P_d] is the position vector. b^T_ω is the inherent error of the gyroscope. b^T_α is the inherent error of the accelerometer. The INS movement model is shown in (11),

\(\begin{align}X=\left[E^{T} V^{T} P^{T} b_{\omega}{ }^{T} b_{\alpha}{ }^{T}\right]=A\left(\alpha^{T}-b_{\alpha}{ }^{T}\right)=\left[\begin{array}{c}0.5 b^{T} \otimes\left(\omega^{T}-b_{\omega}{ }^{T}\right) \\ A\left(\alpha^{T}-b_{\alpha}{ }^{T}\right)+g^{T} \\ V^{T} \\ 0 \\ 0\end{array}\right]\end{align}\)       (11)

where, b is the quaternion form of the attitude information of the mobile node. ⊗ is the quaternion inner product. A is the DCM matrix. ω and α are the measured data of the gyroscope and accelerometer respectively. g is the gravitational acceleration. The error state model of MN is:

\(\begin{align}\delta X=\left[\delta E^{T} \delta V^{T} \delta P^{T} \delta b_{\omega}{ }^{T} \delta b_{\alpha}{ }^{T}\right]^{T}=\left[\begin{array}{c}-\left[\omega^{T}-b_{\omega}{ }^{T}\right]_{X} \delta E^{T}-\omega_{\omega} \\ -A\left[\alpha^{T}-b_{\alpha}{ }^{T}\right]_{X} \delta A^{T}-A \delta b_{\omega}{ }^{T} \\ \delta V^{T} \\ \omega_{\omega} \\ \alpha_{\omega}\end{array}\right]\end{align}\)       (12)

where, δX is the error state vector. δE, δV, δP respectively are the error vector of attitude, velocity and position. δb^T_ω, δb^T_α respectively are the error vector of the gyroscope and accelerometer sensor drift. [⋅]_X is the skew symmetric operator. b^T_ω, b^T_α are the random walk noise of gyroscope and accelerometer respectively.

3. Proposed algorithm

The purpose of the paper is to provide an inertial guidance/override system with higher localization accuracy in complex indoor environments. The mechanism of the proposed localization system is shown below. The algorithm framework proposed in the article is depicted in Fig. 1.

Fig. 1. Flowchart of the presented algorithm

The acceleration and angular velocity of MT are obtained through the inertial measurement unit (IMU). And the message is input into the kinematics model of the inertial navigation system. The system will get the nominal state and error state of MT, which includes the attitude, speed, position of MT and the dynamic deviation information of the inertial sensor.

An ultra-wideband subsystem is established in the wireless sensor network (WSN) framework. And TOA technology is used to obtain distance measurement values. Fuzzy C-means (FCM) has been employed to process ultra-wideband distance measurement results to alleviate the noise generated by NLOS. Then, the position estimate is obtained by least square method. Then, before using KF fusion, the Z hypothesis test has been adopted to verify and recognize the anchor node. And the anchor node where the NLOS link is located is removed. When the total number of remaining anchor nodes is more than three, the location of UWB is obtained by least square method. When the number of remaining anchor nodes is lower than three, the robust extended Kalman filter based on M estimation is employed for processing.

3.1 Fuzzy C-means

The fuzzy C-mean (FCM) arithmetic is one of the widest and of the most successful in terms of application. It accomplishes the aim of automatic classification of sample data. It determines the categories of the sample points by improving the objective function and acquiring the membership degrees of each sample points to the central of all the categories. With each sample has a membership degree function which belongs to each of the aggregated classes. And the sample is categorized according to the membership degree values.

Fuzzy C-means clustering has three main components. They are a finite set of aggregates, the central point of an individual clustering and every single piece of data point that belongs to the clustering closest to the central point respectively. Fuzzy C-means cluster gets the clustering center by the minimization of an objective function. The objective function is actually the sum of the Euclidean distances between points to categories. The aggregation procedure is the one that makes the minimization of the objective function. After repeatedly iterating the calculation, the error value of the objective function grows smaller and smaller. When the objective function reaches a certain threshold of the convergence, the ultimate cluster result can be attained.

\(\begin{align}A_{s}=\sum_{a=1}^{L} \sum_{b=1}^{D} u_{a b}^{m}\left\|x_{a}-d_{b}\right\|^{2} \quad, 1<s<\infty\end{align}\)       (13)

\(\begin{align}u_{a b}=\frac{1}{\sum_{e=1}^{d}\left(\frac{\left\|x_{a}-d_{b}\right\|}{\left\|x_{a}-c_{e}\right\|}\right)^{\frac{2}{s-1}}}\end{align}\)       (14)

\(\begin{align}d_{b}=\frac{\sum_{a=1}^{L} u_{a b}{ }^{s} \cdot x_{a}}{\sum_{a=1}^{L} u_{a b}^{s}}\end{align}\)       (15)

where, s is the number of aggregates (number of classes). L is the number of samples. D is the number of clustering centers. d_b represents the b_th clustering center, which has the identical feature dimension associated with the sample. x_a represents the a_th sample. u_ab represents the membership degree of the sample x_a to the cluster center d_b (the probability of belonging). ||∗|| could be virtually any metric that represents the closeness (distance) of data, the latest of which is the Euclidean norm. As for an individual specimen x_i, the sum of its membership in each of the aggregate is 1. The class in which each sample point has the largest membership degree is classified into which class. The nearer to 1, the greater the degree of the membership. On the contrary, the lower the degree of membership. Locating the cluster central of every group to minimize the objective function. It aims to keep the clustering policy with the greatest intra-group resemblance and the smallest inter-group resemblance. Fuzzy C-means algorithm steps:

(1) Choose the number of classifications D. Select the suitable m, assign primary values to the matrix that is bounded by the membership function (initialize between arbitrary values [0,1]);

(2) Compute the central value of the cluster;

(3) Compute the incoming membership matrix u_a;

(4) A comparison of u_b and u_(b+1). If the difference between the two is lower than a given threshold, the iterative operation is discontinued, and vice versa, go to (2).

(5) Obtaining processed UWB measurements.

The number of clusters is usually not randomly generated, but is determined based on the actual situation and needs. UWB produces additive errors. There is a big difference between LOS and NLOS measured distances, LOS distances are relatively small and NLOS distances are relatively large, so the clustering is divided into two categories, LOS and NLOS. The number of different clusters does not have much effect on the results. Here the UWB data is initially categorized as LOS or NLOS at multiple moments. After processing it is further processed by z-test.

In non-line-of-sight scenarios, the presence of occlusion, illumination variations, and other factors lead to noise and uncertainty in the data acquired by the sensors. Traditional clustering methods may not be able to deal with these problems effectively. The fuzzy C-mean clustering method is able to take into account the uncertainty and ambiguity of the data. This method is more suitable for dealing with data in non-line-of-sight scenarios.The advantages of using fuzzy C-mean clustering method in this case are shown below:

1. Considering data uncertainty: fuzzy C-mean clustering method introduces the concept of ambiguity. It can better handle data uncertainty and improve the accuracy of clustering;

2. Compared to traditional hard clustering methods, fuzzy C-mean clustering method is able to assign data to multiple categories. Instead of being limited to one defined category, it is more realistic;

3. Better robustness: due to the consideration of data uncertainty and ambiguity, the fuzzy C-mean clustering method has better robustness to noise and outliers. It can better adapt to complex data in non-line-of-sight scenarios.

To summarize, the fuzzy C-mean clustering method can better handle data uncertainty and ambiguity. It improves the accuracy and robustness of clustering in non-line-of-sight scenarios. Therefore, it has certain advantages in non-line-of-sight scenarios.

3.2 Construction of test statistics

In general, the NLOS error is a value greater than zero and greater than the measuring error value. And the measurement error is negligible relative to the NLOS error. It is evident that from (4) that true value is included in multiple measurements between MN and M base stations. According to (5), the distance calculated by the geometric method also contains the real value. In order to ensure accuracy, the position of MN is predicted by Kalman here.

x_t|t-1 = Fx_t-1       (16)

\(\begin{align}y r a n g_{m}=\sqrt{\begin{array}{l}(x(1)-\operatorname{anc}(m, 1))^{2} \\ +(x(2)-\operatorname{anc}(m, 2))^{2}\end{array}}\end{align}\)       (17)

rz_m = d_m(t) - yrang_m       (18)

In the above equation, F is MN's state transition matrix. x_t-1 is updated state estimation. x_t|t-1 is the state estimation that predicts the next moment. x(1), x(2) are the two-dimensional coordinates predicted by Kalman filter. anc(m,1), anc(m,2) respectively denote the 2D coordinates of the m_th anchor node. Obviously, d_m(t), yrang_m both contain true distances r_m(x(t)). And it is clear that in (18) means r_m(x(t)) is eliminated in rz_m . d_m(t) potentially impacted by noise, but because the NLOS error is greater than the measurement error. Therefore, the distribution of rz_m in the case of NLOS and LOS is very different. So, it is employed as the check statistic for NLOS recognition.

3.3 Z-test identification

For cases where the NLOS condition is unknown, the anchor node under LOS conditions is chosen based on Z-test identification. It is a well-known parametric hypothesis test method. It is employed to determine whether a set of samples is derived from a Gaussian distribution. And it is employed to determine a population of which the parameters are generally know. In fact, the nearer the sample of rz_m is to the Gaussian distributed with a mean of zero and a variance of 2σ²_m, the more likely the link corresponding to the corresponding anchor node is in the LOS state. Therefore, the Z-test can be used to recognize and reject the NLOS link. And Z-test statistic is constructed as follows:

\(\begin{align}p=\frac{\bar{z}-0}{\sqrt{2 \sigma_{m}^{2}}} \sqrt{M}=\frac{\bar{z}}{\sigma_{m} \sqrt{2 / M}}\end{align}\)       (19)

In the above equation, \(\begin{align}\bar {z}\end{align}\) is the mean of the sample rz. And M is the number of anchor nodes. For a given significance level α, we can get a threshold u_α. When p < u_α, we can assume that the anchor nodes of the link we are on are all available for LOS positioning. Conversely, the link in these anchor nodes has an NLOS condition that requires further verification and identification. For the efficiency of the algorithm, the following algorithm steps for screening NLOS are proposed. Table 1 is the process of the z-test algorithm.

Table 1. Z-test Algorithm

1) Set the set of paths containing all M anchor nodes to S = {L₁, ⋯L_M}, where L_i represents the path.

2) Through the Z test, identify which links in the set S are in the LOS state. That is, if the hypothesis test is passed, it proves that all the links in the group are in the LOS state. And the anchor nodes can also be used for accurate positioning. Conversely, implement procedure 3).

3) Since the overall mean is zero, it is assumed that the greater the value of |rz_i|, the larger the likelihood that the i_th link is NLOS. Therefore, the probability of an NLOS link can be reduced by filtering out the maximum |rz|. While the anchor node and ultra-wideband measurements where the path is located are also filtered out. Thus a new set of links is obtained. Then perform the Z-test again to see if the remaining paths in the group of links meet the LOS status. Repeat this step until the Z-test is satisfied. When the anchor node where the remaining links are located is greater than or equal to three, the position of the MN-positioned by UWB is obtained by the least squares method. And the fusion with INS is completed. Conversely, a robust extended Kalman filter based on M estimation is applied to correct the UWB position. And then the final MN position is obtained by Kalman fusion with INS.

3.4 Robust localization based on enhanced iterative M-estimation

Here, a measurement preprocessing method is used to filter the raw distance measurements. The two-dimensional location of MN at the time t in (5) is given by the following equation. And the following screening of the original measured values is obtained from (4) and (5).

x(t)_pred = x(t - 1) + v_x⋅T_s       (20)

y(t)_pred = y(t - 1) + v_y⋅T_s       (21)

\(\begin{align}r_{-} t o l=\left\{\begin{array}{ll}d_{m}(t) & \text { if }\left|d_{m}(t)-r_{m}(x(t))\right|<\sigma_{m} \\ r_{m}(x(t))_{\text {pred }} & \text { otherwise }\end{array}\right.\end{align}\)       (22)

Since the filtered measurements will most likely to remain inclusive of non-Gaussian noise. Therefore, here we incorporate gradient descent iterative M-estimation with extended Kalman filter steps. It augments the state estimation of the system.

P_t|t-1 = FP_t-1F^T + CQC^T       (23)

In the above equation, F is the state transition matrix. P_t−1 is the state error covariance matrix. P_t|t-1 is prediction state error covariance matrix. C is the noise input matrix. And Q is the covariance matrix of the process noise Ω(t). Jacobi matrix H is given by (24). We express equation (22) as follows (25).

\(\begin{align}H= \begin{bmatrix} \frac{x_{t \mid t-1}-x_{B S_{1}}}{\sqrt{\left(x_{t \mid t-1}-x_{B S_{1}}\right)^{2}}} & \frac{y_{t \mid t-1}-y_{B S_{1}}}{\sqrt{\left(x_{t \mid t-1}-x_{B S_{1}}\right)^{2}}} & 00\\ \cdot & \cdot & \cdot\cdot \\ \cdot & \cdot & \cdot\cdot \\ \frac{x_{t \mid t-1}-x_{B S_{M}}}{\sqrt{\left(x_{t \mid t-1}-x_{B S_{M}}\right)^{2}}} & \frac{y_{t \mid t-1}-y_{B S_{M}}}{\sqrt{\left(x_{t \mid t-1}-x_{B S_{M}}\right)^{2}}}& 00 \end{bmatrix}\end{align}\)        (24)

r_tol = h(ψ_t) + b_t       (25)

\(\begin{align}b_{t}=\sqrt{a_{i, t}} \cdot\left(\sqrt{3} \sigma_{m}\right) \cdot \omega_{i, t} \quad i=1, \cdots, M\end{align}\)       (26)

In the above equation, b_t is the filtered remaining NLOS noise vector. σ_m is the standard deviation of noise in the LOS case. And ω_i,t is the random NLOS measured noise. The approximation of the first-order Taylor series of (25) is expressed as follows:

r_tol = h(ψ_t|t-1) + H_t(ψ_t - ψ_t|t-1) + b_t       (27)

Based on (1) and (27), we get the following:

\(\begin{align}\left[\begin{array}{l}\psi_{t \mid t-1} \\ r_{-} t o l-h\left(\psi_{t \mid t-1}\right) \\ +H \psi_{t \mid t-1}\end{array}\right]=\left[\begin{array}{l}I_{4} \\ H_{t}\end{array}\right] \psi_{t}+\left[\begin{array}{l}-F \psi_{t-1}+\psi_{t \mid t-1} \\ -C v_{n-1} \\ b_{t}\end{array}\right]\end{align}\)       (28)

The second term on the right has a covariance:

\(\begin{align}\left[\begin{array}{cc}P_{t \mid t-1} & 0_{4 * \mathrm{M}} \\ 0_{M * 4} & \mathrm{R}_{\mathrm{t}}\end{array}\right]=C_{t} C_{t}^{T}\end{align}\)       (29)

0_4∗M and 0_M∗4 refers to the zero matrices of 4×M and M×4,respectively. Matrix C_t comes from the Cholesky decomposition. (28)both sides are multiplied by C^-1_t as follows:

z_t = D_tψ_t + e_t       (30)

where, \(\begin{align}z_{t}=C_{t}^{-1}\left[\begin{array}{c}\psi_{t \mid l-1} \\ r_{-} t o l-h\left(\psi_{t \mid l-1}\right)+H \psi_{t \mid l-1}\end{array}\right]\end{align}\), \(\begin{align}D_{t}=C_{t}^{-1}\left[\begin{array}{l}I_{4} \\ H_{t}\end{array}\right]\end{align}\), E[e_t] = 0, E[e_te^T_t] = I_(M+4). The least squares method yields a state estimate ψ_t as follows:

ψ_t = (D^T_t D_t)^-1 D^T_tz_t       (31)

where, [z_t - D_tψ_t]_i represents the i_th element of (z_t - D_tψ_t), which represents the position residuals. As you know, least squares estimation is sensitive to noise density deviations from the Gaussian distribution. So robust estimation could be achieved by M-estimation. The updated state error covariance matrix is as follows:

P_t = (P_t|t-1^-1 + H^T_t R^-1_t H_t)^-1       (32)

R_t = 3σ²_m · diag([a_1,t⋯a_M,t])^T       (33)

In the above equation, a_i,t is a stationary or time-dependent positive value. It is utilized to override the uncertain standard deviation of the NLOS noise retained in the filtered measures.

Function \(\begin{align}d(X)=\sum_{i=1}^{M+4} \rho\left(\left[z_{t}-D_{t} X\right]_{i}\right)\end{align}\) produces a robust estimate:

X = arg min d(X)       (34)

Hampel's M-estimator is used here. ψ([z_t - D_tX]_i) and ψ'([z_t - D_tX]_i) represent the first and second derivatives of ρ([z_t - D_tX]_i). (35)(36) denotes ρ([z_t - D_tX]_i) and its first derivative respectively. In (35)(36), b, c₁, c₂ are greater than 0. And they satisfy b > c₁,c₂ > c₁, \(\begin{align}\ln \left(\frac{b-c_{1}}{b+c_{1}}\right)=-b\left(c_{2}-c_{1}\right)\end{align}\). The solution of (34) is iteratively approximated by the use of the gradient descent method. Suppose the estimated value produced by iteration l_th is X_l. In order to improve the estimates, an iterative approach is used.

\(\begin{align} \begin{array}{l}\rho (\left[z_{t}-D_{t} X\right]_{i})=\\ \left\{\begin{array}{ll}\left(\left[z_{t}-D_{t} X\right]_{i}\right)^{2} / 2 & \text { if }\left|\left[z_{t}-D_{t} X\right]_{i}\right| \leq c_{1} \\ \left(\left[z_{t}-D_{t} X\right]_{i}\right)^{2} / 2-2 \ln (\cosh \\ \left(0.5 b\left(c_{2}-\left|z_{t}-D_{t} X\right|_{i}\right)\right. & \\ \left.\left.\times \operatorname{sgn}\left(\left|z_{t}-D_{t} X\right|_{i}\right)\right)\right) & \text { if } c_{1} \leq\left|\left[z_{t}-D_{t} X\right]_{i}\right| \leq c_{2} \\ +2 \ln \left(\cosh \left(0.5 b\left(c_{2}-c_{1}\right)\right)\right) \\ \left(c_{2}\right) 2 / 2+2 \ln \left(\cosh \left(0.5 b\left(c_{2}-c_{1}\right)\right)\right) \text { otherwise }\end{array}\right. \end{array}\end{align}\)       (35)

\(\begin{align} \begin{array}{l} \psi\left(\left[z_{t}-D_{t} X\right]_{i}\right)=\\ \left\{ \begin{array}{ll} \begin{array}{cc}{\left[z_{t}-D_{t} X\right]_{i}} & \text { if }\left|\left[z_{t}-D_{t} X\right]_{i}\right| \leq c_{1} \\ b \cdot \tanh \left(0.5 b\left(c_{2}-\left|\left[z_{t}-D_{t} X\right]_{i}\right|\right) \operatorname{sgn}\left(\left[z_{t}-D_{t} X\right]_{i}\right)\right) & \text { if } c_{1} \leq\left|\left[z_{t}-D_{t} X\right]_{i}\right| \leq c_{2} \\ 0 & \text { otherwise } \end{array} \end{array} \right. \end{array} \end{align}\)        (36)

X^l+1 = X^l + µ(D^T_tD_t)^-1 D^T_t ψ(z_t - D_tX^l)       (37)

µ = 1.25 max(|ψ'((z_t - D_tX^l)/σ_v|)       (38)

where, X^l = (D^T_t D_t)^-1 D^T_t z_t, \(\begin{align}\sigma_{v}=1.48 \mid \overline{\left(z_{t}-D_{t} X^{l}\right)-\overline{\left(z_{t}-D_{t} X^{l}\right)} \mid}\end{align}\), μ > 0, ψ(z_t - D_tX^l) obtained from (36). When |X^l+1 - X^l| < ε, stop the iteration to get the final state estimate Ψ_t = X^l+1. Table 2 summarizes the flow of the M-robust extended Kalman filter (M-REKF) algorithm.

Table 2. M-REKF Algorithm

4. Simulation and Experiment

4.1 Simulation

In this chapter, the properties of the presented approach will be evaluated through simulations. During the experiment, beacon nodes are arbitrarily arranged in the scene. Using NaveGo [29] as a simulation tool. For the data of the simulation, please refer to the reference [29]. And the target node moves on a predetermined trace in the modeled scenario. The range of the simulated scenario is 100m×100m. The positioning accuracy of the two-dimensional plane is only considered in the experiment. So the z-coordinate is set to a constant of 2m in the simulation. Extended Kalman filter (EKF) [30], Robust Iterative Extended Kalman Filter (RIEKF) [31], Sage-Husa fuzzy adaptive filter (SHFAF) [32] are used as comparison algorithms to verify the properties of the proposed algorithm. The root mean square error (RMSE) and cumulative error distribution function (CDF) shown in (39) are used as evaluation criteria.

\(\begin{align}R M S E=\sqrt{\frac{1}{T \times t_{s}} \sum_{c=1}^{T} \sum_{d=1}^{t_{s}}\left(x_{c}^{d}(7: 9)-\hat{x}_{c}^{d}(7: 9)\right)^{2}}\end{align}\)       (39)

In the above formula, x^d_c(7 : 9) refers to the real coordinates of the target node in the c_th simulation time. And \(\begin{align}\hat{x}_{c}^{d}\end{align}\) is the target node coordinates obtained by the algorithm in this paper. t_s is the total number of samples in a simulation. And T = 100 is the total number of samples in Monte Carlo simulation. The UWB and INS simulation parameter settings are depicted in Table 3 and Table 4.

Table 3. SIMULATION PARAMETER OF UWB

Table 4. SIMULATION PARAMETER OF INS

In Fig. 2(a) above, when the average noise of NLOS grows up from 1 to 8, the RMSE of EKF, SHFAF and RIEKF grows rapidly. While the RMSE of the proposed approach increases gently. Through comparison, it is proved that the approach has better localization precision and robustness.

Fig. 2. RMSE with different means or NLOS probabilities

As can be seen from Fig. 2(b), the RMSE of EKF, SHFAF and RIEKF increases swiftly by the gradual growing of NLOS probability. While the RMSE of the proposed algorithm grows slowly. The average RMSE of EKF, SHFAF and RIEKF are 1.269m, 1.454m and 1.208m, respectively. And the average RMSE of the proposed algorithm is 0.644m, which increases by 49.25%, 55.71% and 46.69%, respectively.

From Fig. 3(a), with the gradual enlarge of the standard deviation of NLOS noise, the RMSE of EKF, RIEKF and SHFAF gradually increased. While the RMSE of the proposed algorithm did not change much. This is because in clustering algorithms, clustering works better when there are large differences between different classes. Therefore, the larger the standard deviation of NLOS noise, the better the clustering effect and the smaller the change in positioning accuracy.

Fig. 3. CDF and RMSE with different standard deviation

The cumulative error distribution (CDF) of EKF, RIEKF, SHFAF and this paper's algorithm is given in Fig. 3(b). When the cumulative probability reaches 90%, the localization errors of EKF, RIEKF, SHFAF and this paper's algorithm are no more than 1.5 m, 2.4 m, 2.5 m, and 3 m. As the cumulative error probability approaches 1, the positioning error of this paper's algorithm is only 3 m. The experimental findings indicate that the method has an enhanced convergence and better capability.

In Fig. 4(a), the results of each algorithm have several distinct peaks. The peak occurs when the mobile node turns. Since the target state changes rapidly when turning, the beacon node will be affected by the INS positioning results, nevertheless. From the previous simulation findings and analysis, it is evident that the proposed algorithm still provides a good positioning effect.

Fig. 4. Average RMSE over time and Trajectories

In Fig. 4(b), EKF, RIEKF, SHFAF and the proposed algorithm Z-test M-robust extended Kalman filter (ZMREKF) track the trajectory of MN. Since the state of the target changes rapidly when turning, the beacon nodes are affected by the INS localization results. Overall, the tracking properties of the proposed method is superior.

Under the noise environment of Gaussian noise, as shown in Fig.5(a), with the gradual growth of the standard deviation of NLOS noise, the RMSE of EKF,SHFAF and RIEKF gradually increases. While the RMSE of the proposed algorithm does not change much. Through comparison, it is proved that the method has improved localization precision.

Fig. 5. RMSE with different standard deviation or means

In Gaussian noise environment, as shown in the Fig. 5(b) above, when the average noise of NLOS grows up by 3 until 10, the RMSE in EKF, SHFAF and RIEKF grows rapidly. While the RMSE of the proposed method increases gently. Through comparison, it is proved that the presented method offers better localization stability.

Under the noise environment of Gaussian noise, as can be seen from Figure.6(a), the RMSE of EKF, SHFAF and RIEKF grow swiftly with the gradual growth of the NLOS probability. While the RMSE of the proposed algorithm in the paper grows slowly. The average RMSE of EKF, SHFAF and RIEKF are 2.757m,3.119m and 2.603m, respectively. And the average RMSE of the proposed algorithm is 1.757m, which increases by 36.27%, 43.67% and 32.50%, respectively.

Fig. 6. Trajectories and RMSE with different NLOS probabilities

In Gaussian noise environment, EKF, RIEKF, SHFAF and the proposed algorithm ZMREKF track the trajectory of MN in Fig. 6(b). Since the state of the target changes rapidly when turning, the beacon nodes are affected by the INS localization results. Overall, the tracking properties of the proposed approach is superior.

In a Gaussian noise environment, there are several distinct peaks in the results of each algorithm in Fig. 7(a). The peaks appear when the mobile node turns. Due to the fast change of the target state during the turn, the beacon node will be affected by the INS localization results. But from the previous simulation findings and analysis, it is obvious that the proposed algorithm still has good localization results.

Fig.7. Average RMSE over time and CDF

Under Gaussian noise environment, Fig. 7(b) illustrates the cumulative error distribution (CDF) of EKF, RIEKF, SHFAF and the algorithm in this paper. When the cumulative probability reaches 90%, the positioning errors of EKF, RIEKF, SHFAF and this paper's algorithms are no more than 3m, 4.4m,4.5m and 5m. The experimental outcomes indicate that the method has outstanding convergence and capability.

Due to the fuzzy C-mean clustering method, the uncertainty and ambiguity of the data is taken into account. The method works well with UWB data. Also the use of Z-test improves the accuracy of localization even more. Plus the combination of INS can provide real-time position tracking, which assists UWB to improve the accuracy of localization. Therefore the accuracy of the proposed method is greatly improved compared to other methods.

4.2 Experiment

To demonstrate the positioning precision in the proposed approach, a true simulation experiment is devised. The propagation between BS and MN has been developed on the basis of UWB carrierless technology. It uses asynchronous narrowband impulse signals to convey the values of the signals. In late times, UWB is extensively applied to the fields of interior distance measurement. The UWB equipment and INS Robotics using in the present article are depicted schematically in Fig. 8.

Fig. 8. The UWB device and the INS robotics

As shown in Fig. 9, there are seven base stations, and the MN keeps moving at a steady velocity following the path seen from the figure. For the sake of preventing the floor reflects of UWB message, the MN is put to the position of one meter height from the floor to the moving position. The space dimension in the cubicle is 5m× 6m, the desk is set at a depth of 1.3 m, and the desk measures 1m. The inertial navigation sampling rate is specified at 50hz. The UWB sampling rate is specified at 5hz, and speed is set to 0.5m/s. During the whole track walking process, there are 276 sampling points for UWB and 2760 sampling points for inertial navigation.

Fig. 9. Experimental scenario

The original state of the MN is given as X(0) = [2.41m 1.8m 0.5m/s 0m/s]^T. The remaining parameters are equal to those of the simulation.

The CDF of the sample plot is demonstrated in Fig. 10(a). When the cumulative probability reaches 90%, the localization error of ZMREKF, i.e., the approach proposed in the article, is less than 2.5 m. And the localization errors of EKF, RIEKF, and SHFAF are less than 3.1 m, 3.2m, and 4m. The actual empirical outcomes indicate that the method has favorable convergence and good properties.

Fig. 10. The CDF and trajectories

In Fig. 10(b), EKF,RIEKF,SHFAF and the proposed algorithm ZMREKF track the MN with larger localization errors at corners due to the rapid changes in the target state. The real empirical outcomes demonstrate that the paper has excellent localization performance and higher accuracy than the other algorithms.

In Fig. 11, there are several distinct peaks in the results of each algorithm. The peaks appear when the mobile node turns a corner. Since the state of the target changes rapidly when turning, the beacon node is affected by the INS localization results. The real experimental results indicate that the proposed algorithm has better positioning performance.

Fig. 11. Localization error at individual sampling point

5. Conclusion

The paper aims to enhance the precision of indoor localization. And the letter proposes an inertial navigation system and ultra-wideband joint positioning algorithm. In the positioning, fuzzy C-means clustering method is used to preprocess the ultra-wideband measurement values first. Then the z-test is used to correct the UWB position and finally the fusion positioning is carried out. In this paper, the UWB positioning outcomes are utilized to periodically correct the localization results of the inertial navigation system. It overcomes the cumulative error problem caused by the integration of the inertial navigation system. Integrated INS positioning alleviates the impact of NLOS transmission on localization precision. The simulation findings illustrate that compared with several comparison algorithms, not only the accuracy is greatly improved, but also the robustness is higher.