Non-Line-of-Sight Positioning Method for Ultra-Wideband/Miniature Inertial Measurement Unit Integrated System Based on Extended Kalman Particle Filter

Abstract

In the field of unmanned aerial vehicle (UAV) control, high-precision navigation algorithms are a research hotspot. To address the problem of poor localization caused by non-line-of-sight (NLOS) errors in ultra-wideband (UWB) systems, an UWB/MIMU integrated navigation method was developed, and a particle filter (PF) algorithm for data fusion was improved upon. The extended Kalman filter (EKF) was used to improve the method of constructing the importance density function (IDF) in the traditional PF, so that the particle sampling process fully considers the real-time measurement information, increases the sampling efficiency, weakens the particle degradation phenomenon, and reduces the UAV positioning error. We compared the positioning accuracy of the proposed extended Kalman particle filter (EKPF) algorithm with that of the EKF and unscented Kalman filter (UKF) algorithm used in traditional UWB/MIMU data fusion through simulation, and the results proved the effectiveness of the proposed algorithm through outdoor experiments. We found that, in NLOS environments, compared with pure UWB positioning, the accuracy of the EKPF algorithm in the X- and Y-directions was increased by 35% and 39%, respectively, and the positioning error in the Z-direction was considerably reduced, which proved the practicability of the proposed algorithm.

1. Introduction

In recent years, UAVs have become increasingly active in the military, civil, and commercial fields, in areas such as reconnaissance and detection, aerial photography, and logistics distribution [1,2,3]. As such, UAV navigation problems have become a new research hotspot. In areas with strong satellite signals, UAVs can rely on the Global Navigation Satellite System (GNSS) for localization. However, the GNSS is almost unusable in areas where signals are obscured: if the UAVs do not have a positioning reference before they are transferred from indoors to outdoors and switched to the GNSS, they cannot achieve fully autonomous outbound, take-off, and recovery. UAVs still need to rely on human transfer until they are within strong satellite signal range, which is not conducive to the development of UAV clusters. Therefore, a localization method suitable for UAVs in indoor as well as small outdoor situations must be developed, as shown in Figure 1.

Currently, a combination of UWB and inertial navigation systems (INSs) is the main approach used for obtaining indoor positioning solutions. UWB is a low-cost wireless carrier communication technology that has a fast transmission rate and strong multipath effect. The UWB bandwidth is as large as 7.5 GHz [4], and UAV clusters have sufficient spectrum resources to use UVB [5]. However, this technology also has some shortcomings. First, attenuation occurs during the NLOS propagation of UWB signals, which results in positioning errors [6,7,8,9]. Second, a single UWB system cannot provide attitude information, which is unacceptable for aircraft. For these reasons, a combination of UWB and INS is used for navigation. As a device capable of outputting full navigation parameters, INS is an ideal solution for obtaining real-time UAV attitude measurements. In recent years, micro-electromechanical systems (MEMSs) have been rapidly developed, and the miniature inertial measurement unit (MIMU) presents a new type of inertial device based on MEMSs. Owing to its low cost and small size, MIMUs have been widely used in consumer electronics, industrial manufacturing, and other fields. By combining UWB and MIMU, the advantages of both sensors can be fully employed to overcome the shortcomings of the UWB system in NLOS propagation and to compensate for the error accumulation in MIMUs over time.

Among the data fusion algorithms, the Kalman filter (KF) is an unbiased estimation algorithm applicable to multidimensional states [10,11,12]. Zhang et al. used the KF to fuse UWB positioning results and IMU data to increase the positioning accuracy in underground mining where the GNSS signal is weak [13]. Theoretically, the KF is only suitable for linear systems; thus, to cope with nonlinear problems, improved KF algorithms have been developed, such as the extended Kalman filter (EKF) and unscented Kalman filter (UKF). A nonlinear system can be linearized via Taylor series expansion [14], which is called the EKF algorithm. In [15], Benini proposed a method for fusing UWB and MIMU data using the EKF algorithm to achieve integrated navigation. This method uses the EKF to correct the constant drift and scale factor error of MIMU to increase the positioning accuracy. In [16], Wang developed an improved EKF algorithm based on sigma point sampling. This method uses prior information and UWB observations to adjust the covariance of the observations, which increases the positioning accuracy in NLOS environments. D’ippolito et al. proposed a new hybrid observation method that provides sporadic position information (aperiodic sampling) as a measurement through a UWB system and fuses with noisy IMU data for positioning. The authors compared the developed hybrid method with the classical EKF algorithm, finding that, when the positioning accuracies are similar, the proposed method required considerably less calculation, thus providing a new idea for fusing the information from complex systems [17,18]. However, the EKF algorithm is a first-order approximation that leads to filter divergence when dealing with higher-order nonlinear models [19]. The UKF is based on the UT transform, which does not require the calculation of the Jacobian matrix [20]. Instead, the UKF processes the statistics of the state and is more adaptable to nonlinear models. Krishnaveni [21] designed an UWB/MIMU integrated navigation system, which uses the UKF to increase positioning accuracy in indoor environments. You [22] designed a quadrotor UAV system structure with the UKF, which does not neglect the higher-order terms of the nonlinear observation equations of UWB and MIMU, and thus effectively increased the accuracy of the solution when compared to that of prior methods.

The EKF and UKF, which are not completely nonlinear and non-Gaussian algorithms, are based on the assumption of a Gaussian distribution. As such, a risk of divergence exists when dealing with strong nonlinear systems. The particle filter (PF) algorithm, which is based on Monte Carlo simulation with a large number of samples, is a real nonlinear, non-Gaussian filtering algorithm. When the number of sampling samples tends to infinity, the distribution probability of any random process can be simulated using the PF algorithm with high enough accuracy, indicating its suitability for nonlinear systems [23]. Jourdan [24] proposed an algorithm for fusing UWB/MIMU data based on the PF, which substantially reduced the NLOS propagation error caused by various obstacles in indoor and urban environments compared with that of the original PF algorithm. The experimental environment for this method is two-dimensional, so the scenarios where this algorithm can be applied are limited. As such, Ray [25] designed a weight updating method that uses PF estimation to fuse dead reckoning and map measurements to achieve accurate pedestrian positioning, and they developed a wearable navigation system that does not depend on the physical infrastructure. Usually, the particles need to be updated using the sequential importance sampling (SIS) algorithm [26], and any function with positive posterior estimation can be used as the IDF in the SIS. However, the function may not be close to the posterior probability distribution of the real particles. In this study, we used a separate algorithm to construct the IDF for fusing UWB and MIMU data.

In this paper, the application of UAV autonomous take-off and landing is taken as the research background, and the high-precision positioning target without GNSS-assisted outdoor movement is determined. Aiming at correcting the error caused by NLOS in UWB positioning, the UWB/MIMU integrated navigation method is proposed to improve the positioning accuracy. The improved EKPF algorithm solves the low efficiency of particle sampling and the particle degradation caused by long time filtering, and improves the positioning accuracy and robustness of UWB/MIMU integrated navigation system.

This article includes five main sections. In Section 2, we establish a filtering model for integrated navigation and an improved EKPF algorithm. Section 3 and Section 4 describe the simulation and physical experiments that we conducted to prove the effectiveness and practicability of the proposed algorithm. Section 5 provides a summary of the experimental results and the importance of this study.

2. Integrated Navigation Data Fusion Method

For combined multisensor systems, subsystem information fusion must be reasonable. The method of filtering fused UWB and MIMU data is described in detail for the application scenario considered in this study.

2.1. Construction of UWB/MIMU Integrated Navigation Filter

2.1.1. Selection of the Coupling Framework

The two main types of coupling frameworks applied in the commonly used UWB/MIMU integrated navigation systems are loose and tight coupling. Loose coupling consists of two independent measurement systems, i.e., the UWB positioning system and the INS, where the INS outputs the navigation parameters of the carrier and the UWB system outputs the position, both in real time. The position of the two subsystems is used as the measurement, and the loosely coupled filtering algorithm is constructed to suppress the INS error divergence. The positioning accuracy of the loosely coupled methods mainly depends on the accuracy of the UWB system, and its integrated structure is easy to implement, but this method introduces the error of the subsystem. Additionally, if the subsystem temporarily fails, the entire system fails to output navigation information.

The tight coupling methods require the original range data from each UWB base station rather than the independent position outputs. Combining the INS state variables and the calculated position, the integrated navigation results are obtained via filter fusion, and the output process is as shown in Figure 2. When individual base stations cannot output range data due to obstacles and other reasons, the integrated navigation system can still output navigation information because the tight coupling only involves the processing of the ranging data of the UWB system, so the correlation problem does not arise [27]. Compared with the loose coupling methods, under the same other conditions, the integrated navigation results output by tight coupling methods are more accurate and robust. Therefore, we chose tight coupling as the framework for UWB/MIMU data fusion.

2.1.2. The Establishment of the Filter State Model

In the integrated navigation with INS, the measurement of MIMU is usually used as the prediction update, and the UWB information is used as the measurement update. According to the References [28,29], Wang designed experiments to compare the differences between the direct filtering method (full navigation parameters as state variables) and the indirect filtering method (navigation parameters error as state variables). He found that, in the case of good GNSS signals, the two methods have almost the same improvement in positioning accuracy, but the indirect method requires a separate inertial navigation solution before the formal filtering. The direct method integrates the inertial navigation solution process into the establishment of the state model, and the filtering and solution are merged into a process, which simplifies the calculation steps and improves the operating rate. For applications where the accuracy of inertial devices is not high and need to be solved quickly, it is more suitable to select the full navigation parameters rather than the error of the parameters as the state variables for estimation. The integrated navigation method proposed in this paper is based on the EKPF algorithm. Since the sampling process of the particle filter itself will generate a large delay, the solution speed should be increased as much as possible to improve the real-time positioning. Therefore, the position, velocity, attitude of the carrier in the navigation coordinate system and the bias of the MIMU are selected for estimation. The symbolic representation and meaning of variables are shown in

Table 1. Symbolic representation of state vector.

Variable	Representation
${\mathit{p}}^{\left(n\right)}={\left[\begin{array}{ccc}{p}_x& p_y& p_z\end{array}\right]}^T$	position vector of UAV in n (navigation coordinate system)
${\mathit{v}}^{\left(n\right)}={\left[\begin{array}{ccc}{v}_x& v_y& v_z\end{array}\right]}^T$	velocity vector of UAV in n coordinate
${\mathit{q}}^{\left(n\right)}={\left[\begin{array}{cccc}{q}_W& q_X& q_Y& q_Z\end{array}\right]}^T$	quaternion vector of UAV attitude in n coordinate
${\mathit{b}}_f$	accelerometer bias vector
${\mathit{b}}_{\omega }$	gyro bias vector

, and the state vectors are defined as (1). Moreover, in this paper, all the variables with the superscript of n are represented in the navigation coordinate system, and the variables with the superscript of b are represented in the carrier coordinate system, that is, the UAV platform.

$$\mathit{X}={\left[\begin{array}{ccccc}{\mathit{p}}^{\left(n\right)}& {\mathit{v}}^{\left(n\right)}& {\mathit{q}}^{\left(n\right)}& {\mathit{b}}_f& {\mathit{b}}_{\omega }\end{array}\right]}^T$$

(1)

The influence of Earth’s rotation and curvature can be ignored because the target is moving at a slow speed, and the localization area is small. The differential equation of inertial navigation motion can be simplified as follows [30]:

$$\dot{\mathit{X}}=\left[\begin{array}{c}{\dot{\mathit{p}}}^{\left(n\right)}\\ {\dot{\mathit{v}}}^{\left(n\right)}\\ {\dot{\mathit{q}}}^{\left(n\right)}\\ {\dot{\mathit{b}}}_f\\ {\dot{\mathit{b}}}_{\omega }\end{array}\right]=\left[\begin{array}{c}{\mathit{v}}^{\left(n\right)}\\ {\mathit{a}}^{\left(n\right)}\\ \\ {\displaystyle \frac{1}{2}}{\mathit{q}}^{\left(n\right)}\otimes \left[\begin{array}{c}0\\ {\mathit{\omega }}^{\left(b\right)}\end{array}\right]\\ \\ -{\displaystyle \frac{1}{{\mathit{\tau }}_f}}{\mathit{b}}_f^b+{\mathit{\delta }}_f\\ \\ -{\displaystyle \frac{1}{{\mathit{\tau }}_{\omega }}}{\mathit{b}}_{\omega }^b+{\mathit{\delta }}_{\omega }\end{array}\right]=\left[\begin{array}{c}{\mathit{v}}^{\left(n\right)}\\ {\mathit{R}}_b^n\left({\tilde{\mathit{f}}}^{\left(b\right)}-{\mathit{b}}_f-{\mathit{n}}_f\right)\\ \\ {\displaystyle \frac{1}{2}}{\mathit{q}}^{\left(n\right)}\otimes \left[\begin{array}{c}0\\ {\tilde{\mathit{\omega }}}^{\left(b\right)}-{\mathit{b}}_{\omega }-{\mathit{n}}_{\omega }\end{array}\right]\\ \\ -{\displaystyle \frac{1}{{\mathit{\tau }}_{\mathit{f}}}}{\mathit{b}}_f^b+{\mathit{\delta }}_f\\ \\ -{\displaystyle \frac{1}{{\mathit{\tau }}_{\omega }}}{\mathit{b}}_{\omega }^b+{\mathit{\delta }}_{\omega }\end{array}\right]$$

(2)

In (2), ${\mathit{a}}^{\left(n\right)}$ is the acceleration under the n coordinate, ${\mathit{\omega }}^{\left(b\right)}$ is the angular velocity vector under the b coordinate, ${\tilde{\mathit{f}}}^{\left(b\right)}$ is the value vector measured by the accelerometers, and ${\tilde{\mathit{\omega }}}^{\left(b\right)}$ is the value vector measured by the gyroscopes. ${\mathit{n}}_f$ and ${\mathit{n}}_{\omega }$ denote the Gaussian white noise of the accelerometers and gyroscopes, respectively. The bias of inertial devices can be modeled as a first-order Markov model, where ${\mathit{b}}_f^b$ and ${\mathit{b}}_{\omega }^b$ correspond to the first-order Markov noise of the accelerometers and gyroscopes, respectively; ${\mathit{\tau }}_f$ and ${\mathit{\tau }}_{\omega }$ represent the correlation time constants of the accelerometers and gyroscopes, respectively; ${\mathit{\delta }}_f$ and ${\mathit{\delta }}_{\omega }$ signify the Markov process noise of the accelerometers and gyroscopes, respectively; ⊗ is quaternion multiplication; and ${\mathit{R}}_b^n$ is a rotation matrix from b to n, which is calculated as follows:

$${\mathit{R}}_b^n=\left[\begin{array}{ccc}1-2q_Y^2-2q_Z^2& 2q_X{q}_Y-2q_W{q}_Z& 2q_X{q}_Z+2q_W{q}_Y\\ 2q_X{q}_Y+2q_W{q}_Z& 1-2q_X^2-2q_Z^2& 2q_Y{q}_Z-2q_W{q}_X\\ 2q_X{q}_Z-2q_W{q}_Y& 2q_Y{q}_Z+2q_W{q}_X& 1-2q_X^2-2q_Y^2\end{array}\right]$$

(3)

The update of the state in discrete time is predicted by the recursive relationship, so we use the symbol with a subscript to represent the specific time. For example, $k+1$ represents the time after k, and the prediction matrix is given as follows (4):

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\mathit{p}}_{k+1}^n\\ {\mathit{v}}_{k+1}^n\\ {\mathit{q}}_{k+1}^n\\ {\mathit{b}}_{f,k+1}\\ {\mathit{b}}_{\omega ,k+1}\end{array}\right]& =\left[\begin{array}{ccccc}\mathit{I}& \mathit{I}\Delta t& 0& -{\mathit{R}}_b^n{\displaystyle \frac{{(\Delta t)}^2}{2}}& 0\\ 0& \mathit{I}& 0& -{\mathit{R}}_b^n\Delta t& 0\\ 0& \mathsf{\Omega }(-{\mathit{b}}_{\omega ,k})& 0& 0& 0\\ 0& 0& 0& e^{-{\beta }_1\Delta t}& 0\\ 0& 0& 0& 0& e^{-{\beta }_1\Delta t}\end{array}\right]\left[\begin{array}{c}{\mathit{p}}_k^n\\ {\mathit{v}}_k^n\\ {\mathit{q}}_k^n\\ {\mathit{b}}_{f,k}\\ {\mathit{b}}_{\omega ,k}\end{array}\right]\hfill \\ & +\left[\begin{array}{cc}{\mathit{R}}_b^n{\displaystyle \frac{{(\Delta t)}^2}{2}}& 0\\ {\mathit{R}}_b^n\Delta t& 0\\ 0& {\mathit{q}}_k^n\otimes \\ 0& 0\\ 0& 0\end{array}\right]\left[\begin{array}{c}{\tilde{\mathit{f}}}_b\\ {\tilde{\mathit{\omega }}}_b\end{array}\right]+\left[\begin{array}{cccc}{\mathit{R}}_b^n{\displaystyle \frac{{(\Delta t)}^2}{2}}& 0& 0& 0\\ {\mathit{R}}_b^n\Delta t& 0& 0& 0\\ 0& 0& 0& \mathit{I}\\ 0& \mathit{I}& 0& 0\\ 0& 0& \Delta t{\displaystyle \frac{{\mathit{Q}}_{2:4}}{2}}& 0\end{array}\right]\left[\begin{array}{c}{\mathit{n}}_f\\ {\mathit{n}}_{\omega }\\ {\mathit{\delta }}_f\\ {\mathit{\delta }}_{\omega }\end{array}\right]\hfill \end{array}$$

(4)

2.1.3. The Establishment of the Measurement Equation

The measurement update equation was established according to the actual number of base stations and the layout scheme. Therefore, the distribution of the UWB base stations was designed first. In the UWB system, if the two-dimensional positioning of the tag is required, at least three UWB base stations are required; to achieve three-dimensional tag positioning, at least four UWB base stations are needed. In this study, the tag was attached to the UAV platform, and the three-dimensional positioning of the tag was the aim, so we needed at least four UWB base stations.

A general rule was proposed for base station layout in UWB positioning systems [31]: the spatial layout of the base station should ensure that their projection lengths on the XYZ axes are as equal as possible, that is, that the sides of the polyhedron with the base station as the vertex are equal. However, in autonomous UAV takeoff and landing scenarios, the base station layout can ensure that the XY-axes projection lengths are equal in the horizontal plane, but, in the Z-axis direction, the projection lengths cannot be guaranteed to be equal to the XY-axes projection lengths due to the limitations imposed by erection cost and other factors. The base station layout is often shown as flat, which leads to pure UWB systems having low positioning accuracy in the Z-axis direction. Considering the above factors, and to balance cost with positioning accuracy, the UWB system used in this study consisted of six base stations and one tag. The layout is shown in Figure 3.

In the UWB positioning system, the coordinate system is arranged in the form of east–north–up (ENU), where the positive direction of the X-axis points to the east, the positive direction of the Y-axis points to the north, and Z-axis positive direction points upward. The coordinate system is called the navigation coordinate system, and the coordinate axes are represented using $X^n$, $Y^n$, and $Z^n$ to distinguish it from the carrier coordinate system $X^b$, $Y^b$, and $Z^b$ of the UAV platform, respectively. Here, $A_0$, $A_1$, $A_2$, $A_3$, $A_4$ and $A_5$ are UWB base stations, and $T_0$ is the UWB tag installed on the UAV platform. The coordinates of each base station are $A_0(0,0,z_1)$, $A_1(0,y,z_1)$, $A_2(x,y,z_1)$, $A_3(x,0,z_1)$, $A_4(0,{\displaystyle \frac{y}{2}},z_2)$, and $A_5(x,{\displaystyle \frac{y}{2}},z_2)$, and the coordinates of the tag are $T_0(x_{tag},y_{tag},z_{tag})$. The real distance between each base station and the tag is denoted as $d_0$, $d_1$, $d_2$, $d_3$, $d_4$, $d_5$, and the measurement distance is denoted as $r_0$, $r_1$, $r_2$, $r_3$, $r_4$, $r_5$.

The filter uses the distance information from the base station to the tag as the measurement. The measurement equation can be listed according to the geometric relationship in Figure 3 as follows:

$$\left[\begin{array}{c}{r}_0\left(k+1\right)\\ r_1\left(k+1\right)\\ r_2\left(k+1\right)\\ r_3\left(k+1\right)\\ r_4\left(k+1\right)\\ r_5\left(k+1\right)\end{array}\right]=\left[\begin{array}{c}\sqrt{{\left(x_{tag}\left(k+1\right)\right)}^2+{\left(y_{tag}\left(k+1\right)\right)}^2+{\left(z_{tag}\left(k+1\right)-z_1\right)}^2}\\ \sqrt{{\left(x_{tag}\left(k+1\right)\right)}^2+{\left(y_{tag}\left(k+1\right)-y\right)}^2+{\left(z_{tag}\left(k+1\right)-z_1\right)}^2}\\ \sqrt{{\left(x_{tag}\left(k+1\right)-x\right)}^2+{\left(y_{tag}\left(k+1\right)-y\right)}^2+{\left(z_{tag}\left(k+1\right)-z_1\right)}^2}\\ \sqrt{{\left(x_{tag}\left(k+1\right)-x\right)}^2+{\left(y_{tag}\left(k+1\right)\right)}^2+{\left(z_{tag}\left(k+1\right)-z_1\right)}^2}\\ \sqrt{{\left(x_{tag}\left(k+1\right)\right)}^2+{\left(y_{tag}\left(k+1\right)-y/2\right)}^2+{\left(z_{tag}\left(k+1\right)-z_2\right)}^2}\\ \sqrt{{\left(x_{tag}\left(k+1\right)-x\right)}^2+{\left(y_{tag}\left(k+1\right)-y/2\right)}^2+{\left(z_{tag}\left(k+1\right)-z_2\right)}^2}\end{array}\right]+\left[\begin{array}{c}{\upsilon }_0\left(k+1\right)\\ {\upsilon }_1\left(k+1\right)\\ {\upsilon }_2\left(k+1\right)\\ {\upsilon }_3\left(k+1\right)\\ {\upsilon }_4\left(k+1\right)\\ {\upsilon }_5\left(k+1\right)\end{array}\right]$$

(5)

where ${\upsilon }_i(k+1)(i=0,1,2,3,4,5)$ is the measurement noise, and $r_i(k+1)(i=0,1,2,3,4,5)$ represents the ranging information of the base station.

2.2. Improved Filtering Algorithm of UWB/MIMU Integrated Navigation System Based on EKPF

We constructed the IDF based on the EKF filtering results to reduce the influence of nonlinear NLOS error on the positioning accuracy of the system and suppress the particle degradation phenomenon of the PF algorithm and to ensure the particles move to the high likelihood region to form the EKPF algorithm. The steps of the algorithm are as follows:

$Step1$: Initialize particle sampling.

When $k=0$ (k denotes the filtering time), set the number of particles $i=1,2,\cdots ,N$, according to the prior probability sampling $X_0^{\left(i\right)}\sim p\left(X_0\right)$, calculate ${\omega }_0^{\left(i\right)}=p\left[Z_0|X_0^{\left(i\right)}\right]$, obtain the total weight ${\omega }_T={\sum }_{i=1}^N{\omega }_o^{\left(i\right)}$, and calculate the normalized weight ${\omega }_0^{\left(i\right)}={\displaystyle \frac{{\omega }_0^{\left(i\right)}}{{\omega }^T}}$;

$Step2$: Predict and update with EKF.

When $k\ge 1$,

(1) EKF filtering is performed on the sampled particles:

$$\left\{\begin{array}{c}{\widehat{\mathit{X}}}_k^{\left(i\right)}(-)=f\left[{\widehat{\mathit{X}}}_{k-1}^{\left(i\right)}\right]\\ {\mathit{P}}_k^{\left(i\right)}(-)={\mathit{F}}_k^{\left(i\right)}{\mathit{P}}_{k-1}^{\left(i\right)}{\left[{\mathit{F}}_k^{\left(i\right)}\right]}^T+{\mathit{Q}}_k\\ {\mathit{K}}_k^{\left(i\right)}(-)={\mathit{P}}_k^{\left(i\right)}(-){\left[{\mathit{H}}_k^{\left(i\right)}\right]}^T{\left\{{\mathit{H}}_k^{\left(i\right)}{\mathit{P}}_k^{\left(i\right)}(-){\left[{\mathit{H}}_k^{\left(i\right)}\right]}^T+{\mathit{R}}_k\right\}}^{-1}i=1,2,\cdots ,N\\ {\widehat{\mathit{X}}}_k^{\left(i\right)}={\widehat{\mathit{X}}}_k^{\left(i\right)}(-)+{\mathit{K}}_k^{\left(i\right)}\left\{{\mathit{Z}}_k-{\mathit{H}}_k^{\left(i\right)}\left[{\widehat{\mathit{X}}}_k^{\left(i\right)}(-)\right]\right\}\\ {\mathit{P}}_k^{\left(i\right)}={\mathit{P}}_k^{\left(i\right)}(-)-{\mathit{K}}_k^{\left(i\right)}{\mathit{H}}_k^{\left(i\right)}{\mathit{P}}_k^{\left(i\right)}(-)\end{array}\right.$$

(6)

(2) For $i=1,2,\cdots ,N$, the IDF is constructed using the EKF results, and the particles are updated according to the function to obtain the posterior estimation $X_k^{\left(i\right)}\sim N\left[X_k^{\left(i\right)};{\widehat{X}}_k^{\left(i\right)},{\widehat{P}}_k^{\left(i\right)}\right]$. ${\widehat{X}}_k^{\left(i\right)}$, and ${\widehat{P}}_k^{\left(i\right)}$ are the state estimation and response covariance of the particles after the application of the EKF algorithm; update the weight of the particle ${\omega }_k^{\left(i\right)}={\overline{\omega }}_{k-1}^{\left(i\right)}p\left[Z_k|X_k^{\left(i\right)}\right]$, obtain the total weight ${\omega }_T={\sum }_{i=1}^N{\omega }_k^{\left(i\right)}$, and calculate the normalized weight ${\omega }_k^{\left(i\right)}={\displaystyle \frac{{\omega }_k^{\left(i\right)}}{{\omega }^T}}$;

$Step3$: Determine whether to resample.

The threshold $N_{th}$ is set according to experience, which is usually set to ${\displaystyle \frac{2}{3}}N$. If the effective sample size $N_{eff}$ is less than $N_{th}$, resampling is performed, and the weight ${\overline{\omega }}_{k-1}^{\left(i\right)}=1/N$ is reset.

$Step4$: The filtering result is output according to the weight fusion posterior particle information:

$$\left\{\begin{array}{c}\widehat{p}\left({\mathit{X}}_k|{\mathit{Z}}_k\right)=\sum _{i=1}^N{\overline{\omega }}_k^{\left(i\right)}\delta \left[{\mathit{X}}_k-{\mathit{X}}_k^{\left(i\right)}\right]\hfill \\ \\ {\widehat{\mathit{X}}}_k=E\left({\mathit{X}}_k|{\mathit{Z}}_k\right)\approx \sum _{i=1}^N{\overline{\omega }}_k^{\left(i\right)}{\mathit{X}}_k^{\left(i\right)}\hfill \end{array}\right.$$

(7)

$Step5$: Enter the next moment cycle until the end.

The above five steps outline the specific process of the EKPF algorithm, and the algorithm processing flow chart is visualized in the following Figure 4.

The designed algorithm is estimated using a PF, and the role of the EKF is constructing the IDF. The particles are sampled a posteriori via the probability density function (PDF), which involves the unknown information that needs to be estimated. Therefore, sampling is performed using the IDF instead of directly sampling from the posterior PDF using the relationship between the weight, the posterior PDF, and the IDF [32]. The real-time measurement information is processed using the EKF, providing the latest posterior probability distribution for PF sampling to obtain the optimal estimation. The filtering result of the navigation information is still obtained by the PF, which avoids the influence of the strong nonlinear error caused by NLOS propagation on filtering when only the EKF is used for data fusion.

3. Simulation Experiment of UWB/MIMU Integrated Navigation System

3.1. Construction of Simulation Environment

Simulation experiments were conducted to verify the effectiveness of the UWB/MIMU data fusion algorithm based on the EKPF proposed in this paper. The size of the experimental site was set to 100 m × 100 m × 10 m, and the number of base stations in the UWB positioning system was set to six, which were distributed at six positions at the experimental site. The coordinates were $A_0(0,0,0)$, $A_1(0,100,0)$, $A_2(100,100,0)$, $A_3(100,0,0)$, $A_4(0,50,10)$, and $A_5(100,50,10)$. The tag was installed on the UAV carrier.

The simulation trajectory was specified in advance. The trajectory was roughly divided into seven stages: take-off, acceleration, turning, circular motion, high rise, turning, deceleration, and landing. The starting point of the motion was the origin of the coordinate system. The simulation trajectory is shown in Figure 5, where the red hexagonal star mark is the end point of the motion.

We added errors to the simulation experiment according to the common low-cost MIMU accuracy index and the attenuation of the UWB signal in the NLOS cases to ensure the generated data would be closer to the outputs in real situations. The random error model of the MIMU adopts the first-order Markov noise. The model contains two parts: Gaussian white noise and Gaussian–Markov noise. The MIMU sensor data are modeled as follows:

$$\left\{\begin{array}{c}{\tilde{\mathit{f}}}_k={\mathit{f}}_k+{\mathbf{\nabla }}_r+{\mathit{w}}_a\hfill \\ {\tilde{\mathit{\omega }}}_k={\mathit{\omega }}_k+{\mathit{\epsilon }}_r+{\mathit{w}}_g\hfill \end{array}\right.$$

(8)

${\tilde{\mathit{f}}}_k$ and ${\mathit{f}}_k$ are the measured and theoretical values of the accelerometer, respectively; ${\tilde{\mathit{\omega }}}_k$ and ${\mathit{\omega }}_k$ denote the gyroscope-measured and theoretical values, respectively; ${\mathit{w}}_a$ and ${\mathit{w}}_g$ represent the accelerometer’s and gyroscope’s white noise, respectively; ${\mathbf{\nabla }}_r={\left[\begin{array}{ccc}{\nabla }_{rx}& {\nabla }_{ry}& {\nabla }_{rz}\end{array}\right]}^{\mathrm{T}}$ and ${\mathit{\epsilon }}_r={\left[\begin{array}{ccc}{\epsilon }_{rx}& {\epsilon }_{ry}& {\epsilon }_{rz}\end{array}\right]}^{\mathrm{T}}$ are the Markov noise of the accelerometer and gyroscope, respectively.

In the UWB positioning system, if no occlusion arises between the base station and the tag, the UWB ranging noise can be modeled as Gaussian white noise and random noise. With obstacle occlusion, the NLOS propagation phenomenon appears in the UWB signal. In this study, the following assumptions were made in the simulation experiment:

(1) The probability of NLOS propagation at moment t follows a uniform distribution;

(2) When NLOS propagation occurs, the error caused follows a bimodal Gaussian distribution, and the error is approximately 0.3 m or 0.5 m. Therefore, the UWB data generated by simulation are modeled as follows:

$$\mathit{r}=\mathit{d}+\mathit{w}+{\mathit{e}}_{NLOS}$$

(9)

$\mathit{r}$ and $\mathit{d}$ are the UWB-measured and real values of the distance between the base station and the tag, respectively; $\mathit{w}$ denotes Gaussian white noise; ${\mathit{e}}_{NLOS}$ represents the measurement error caused in NLOS situations.

3.2. Analysis of Simulation Results

The navigation information during the UAV flight, the ranging information of each UWB base station, and the data output by the gyroscope and accelerometer in the MIMU were obtained according to the trajectory and parameters in

Table 2. Parameter settings in simulation experiment.

Parameter	Value
Total simulation time (s)	480
UWB sampling frequency (Hz)	20
IMU sampling frequency (Hz)	20
Bias of gyroscope (°/h)	5
One-order Markov process correlation time of gyroscope (s)	360
Angle random walk (°/$\sqrt{\mathrm{h}}$)	$0.45$
Bias of accelerometer (mg)	15
One-order Markov process correlation time of accelerometer (s)	180
Speed random walk ($\mu g/\sqrt{\mathrm{Hz}}$)	200
Ranging error of UWB (m)	0.1
NLOS occurrence probability	$10\%$

. The output navigation information was obtained by inputting the data into the filtering algorithm. The positioning errors of the various filtering algorithms were obtained by comparing the integrated navigation solution and the theoretical value obtained from the simulation data.

The EKF and UKF were compared with the proposed method in this paper to highlight the effectiveness of UWB/MIMU data fusion technology based on the EKPF algorithm. The standard PF was not compared with the EKPF algorithm due to the presence of particle dispersion, which is unsuitable for direct data fusion.

The navigation position errors among the EKF, UKF, and EKPF algorithms are compared in Figure 6. The figure shows that the position errors of the different UWB/MIMU data fusion algorithms in the X- and Y-axes directions are maintained within ±0.3 m, and the position errors in the Z-axis direction are maintained within ±0.6 m. In contrast, the position error fluctuations of the EKF and UKF algorithms were higher than that of the EKPF algorithm. The reason for this phenomenon is that when the UWB positioning system experiences NLOS propagation error, the filter corrects the position error; when the UWB positioning system does not encounter NLOS propagation error, the measurement stage of the filter fluctuates during nonlinear processing due to the large approximation error. Therefore, both the EKF and UKF algorithms considerably impact the nonlinear NLOS error. The EKPF algorithm is not obtained by linearizing the nonlinear system model but by using a large number of particles to simulate the random process of arbitrary distribution. The EKPF algorithm is a real nonlinear filtering algorithm, so the position error solved by the EKPF algorithm was the smallest.

The Z-axis errors of the different algorithms are relatively large due to the position error of the INS in the Z-axis rapidly diverging, and the UWB positioning system cannot provide Z-axis information direction. As such, reliable Z-axis measurement information can be provided to reduce the position error of the algorithm in the Z-axis direction. For example, by adding a barometer, the position error in the Z-axis direction of the integrated navigation system can be directly reduced to achieve the same accuracy as that in the XY-axes direction. The maximum position error of the EKF and UKF algorithms in the Z-axis was approximately 0.6 m, whereas the maximum error of the EKPF algorithm was within 0.3 m. The root mean square errors (RMSEs) for each position for each algorithm are shown in

Table 3. The RMSEs for each position for each algorithm in the simulation experiment.

Positioning Algorithm	Position Error (m)
	X-Axis	Y-Axis	Z-Axis
UWB	0.178	0.299	2.104
EKF	0.116	0.159	0.234
UKF	0.103	0.143	0.186
EKPF	0.074	0.092	0.151

.

The errors in the X-, Y-, and Z-directions of the EKPF algorithm were lower by 58.4%, 59.8%, and 92.9%, respectively, compared with those of pure UWB positioning. Compared with the EKF algorithm, the errors of EKPF algorithm were lower by 36.2%, 42.1%, and 35.9%, respectively. Compared with the UKF algorithm, the three errors of EKPF algorithm were lower by 28.2%, 35.7% and 19.4%, respectively. Therefore, the positioning accuracy of the proposed EKPF algorithm was the highest.

In this task, the computational effort of the filter plays a vital role in the real-time performance of the algorithm. All programs were run with the same software, with an Intel Core i5-12500H processor (Intel Corporation, California, America) and 16 GB of memory, to compare the algorithms. For all algorithms, the same parameters were used to run the Matlab code, and the only difference was the filtering algorithm. The total and average running times of each algorithm are shown in

Table 4. Simulation duration for each algorithm.

	EKF	UKF	EKPF
Total algorithm duration (s)	3.021	11.809	33.054
Single-step algorithm duration (ms)	0.31	1.23	3.44

. The EKF algorithm was the fastest, followed by the UKF algorithm, and the EKPF algorithm was the slowest. In this simulation for the EKPF algorithm, the total running time was 480 s, the total time was approximately 33 s, and the average running time of a single step was 3.44 ms. As the sampling rate of the UWB sensors was 20 Hz, the single-step running time of the EKPF algorithm is within an acceptable range, meeting the requirements of the data fusion algorithm during autonomous UAV take-off and landing.

4. Physical Experiment with UWB/MIMU Integrated Navigation System

Because the intended application of the designed algorithm is the movement of a UAV from indoor to outdoor environments, an outdoor experiment was conducted to prove the practicability of the proposed algorithm for UWB/MIMU integrated systems in the field.

4.1. Site Layout of the Outdoor Experiment

Following the layout scheme described in Section 2.2, the size of the experimental site was set to 50 m × 50 m, and the location coordinates of the UWB base station nodes were recorded as follows: $A_0(0,0,1)$, $A_1(0,50,1)$, $A_2(50,50,1)$, $A_3(50,0,1)$, $A_4(0,25,2)$, and $A_5(50,25,2)$. The layout diagram is shown in Figure 7.

4.2. The Implementation Steps in the Outdoor Experiment

The MIMU selected for this experiment was an MPU6050, which was integrated in the UAV; the MIMU parameters are shown in

Table 5. The MPU6050 parameter configuration.

Parameter	Type	Numerical Magnitude
Accelerometer	Range ability (g)	±2, ±4, ±8, ±16
	Sensitivity (LSB/g)	16,384, 8192, 4096, 2048
Gyroscope	Range ability (°/s)	±250, ±500, ±1000, ±2000
	Sensitivity (LSB/°/s)	131, 65.5, 32.8, 16.4
Working voltage	(V)	[2.375, 3.46]
Working temperature	(°C)	[−40, 105]

. The MIMU was connected to the magnetometer as an auxiliary alignment device due to the difficulty of the low-precision azimuth alignment of the gyroscope. The solution process of the initial alignment was packaged, and only initialization was required before the experiment. The operational part of the initialization process is shown in Figure 8 and Figure 9, limited to space, each figure only shows three locations. The six-position method was used to calibrate the accelerometer’s errors such as the zero deviation and scale factor [33]. The magnetometer data were collected via the rotation method and calibrated using the UAV’s flight control algorithm [34]. Due to the low precision of the gyroscope itself, it cannot be sensitive to the rotation of the Earth. Therefore, the gyroscope and the accelerometer were calibrated together under static conditions. The MPU6050 is based on the initialized coordinates as the zero degree of the Euler angle, which needed to be aligned at the origin of the navigation system.

The parameters of the UWB sensor selected for this experiment are shown in

Table 6. The parameters of the UWB sensor.

Type	Numerical Magnitude
Propagation distance (m)	500
Measuring accuracy (cm)	10
Operational frequency band (MHz)	[3744, 4243], [4243, 4742]
Power voltage (V)	[3.6, 5.5]
Working temperature (°C)	[−20, 85]
Communication interface	USB, UART

. This sensor had decimeter-level ranging accuracy, a high data refresh frequency, and a long transmission distance.

The structure of the UAV used in this experiment is shown in Figure 10. The built-in MIMU was removed and fixed under the roof, and the UWB tag was placed vertically above the roof, which coincided with the center of the MIMU. The lever arm error could be ignored compared with the device error.

This experiment was conducted to simulate the UAV outbound process; GNSS cannot theoretically be used for this process. We used accurate single-point information as the position reference for the integrated navigation system.

First, the distance information among the 10 points and the constructed benchmark coordinate system was accurately measured at the outdoor site using high-precision laser ranging. The laser ranging sensor used in this experiment adopted the TOF ranging principle, and its accuracy was 1.5 cm. A schematic diagram of the obtained position reference is shown in Figure 11. The distance designed in advance was measured with a baffle plate and then marked on the ground as a reference point. During each operation, the UAV took off from a known point, hovered in the air for a period of time, then flew to the next known point in a curvilinear motion, hovered again at a known altitude, and after the UAV landed, the precise positional coordinates of the end point were measured. As such, the positioning error was obtained by subtracting the distance between the calculated displacement and the ground position point.

Figure 12 shows the position coordinates of the selected 10 known points in detail. The UAV departed from point 01 in a set order and flew to the vicinity of point 02, which was recorded as route 1; after recording the necessary data, the UAV took off from point 02 and flew to point 03, which was recorded as route 2, and so on. Finally, the UAV took off from point 10 and returned to point 01, thus successfully accomplishing the mission of 10 flights with a total of 10 routes. This outdoor experiment mainly focused on the practical feasibility of the proposed EKPF algorithm and its performance under NLOS situations, because the performance of the different fusion algorithms was compared in the simulation. As such, we compared the proposed method only with the traditional UWB positioning in this experiment.

In this experiment, the flight environment of the UAV was divided into LOS and NLOS routes. The 01–05 routes were in LOS environments, with no obstacles between the base station and the tag, as shown in Figure 13.

The 06–10–01 routes involved NLOS environments, with obstacles between the base stations and the tag. Different from indoor NLOS environments, due to the openness of the experimental site, placing fixed obstacles to directly affect the ranging of the UWB system was difficult. Thus, we manually held obstacles to test the performance of the system in a large, outdoor NLOS environment. The experimenter held obstacles at a distance of 1–2 m from the UWB base station to block the propagation path, thereby creating NLOS propagation conditions. Jaturatussanai et al. analyzed the signal under the shelter of common building materials (such as glass, wood, etc.) through a vector network analyzer because the influence of NLOS is related to the material of occlusion, and they obtained the time dispersion and penetration loss for each material [35]. Xie et al. measured the UWB positioning results after occlusion caused by a wooden board, iron board, and human body, finding that the NLOS effect caused by the human occlusion was the strongest, followed by that of the iron board. The wooden board had almost no effect [36]. In this study, considering the existing research, an iron plate, cardboard, and a human body were selected as obstacles to produce an effective experimental NLOS environment. The occlusion method is shown in Figure 14.

4.3. Analysis of the Experimental Results

(1) Data processing and analysis of outdoor LOS environment experiment.

The RMSEs of the position of each algorithm in the LOS environment are shown in

Table 7. The RMSEs of the position error of each algorithm in the simulation experiment.

Flight Path	Pure UWB (m)			EKPF (m)
	X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Route 1	0.112	0.096	1.369	0.083	0.064	0.195
Route 2	0.094	0.105	0.853	0.065	0.072	0.162
Route 3	0.102	0.096	1.112	0.072	0.060	0.179
Route 4	0.106	0.118	1.073	0.075	0.079	0.183
Route 5	0.098	0.071	1.093	0.068	0.051	0.201
Mean	0.102	0.097	1.100	0.073	0.065	0.184

. The positioning error of the pure UWB algorithm in the X- and Y-directions was roughly maintained at approximately 10 cm. The height difference in the Z-axis in the outdoor experiment was 1 m due to the limitations of the equipment and site. Compared with the distance difference of nearly 50 m in the X- and Y-axes, the positioning error of the pure UWB algorithm in the Z-axis was considerably higher. Therefore, in large outdoor large sites, the proposed UWB/MIMU integrated navigation method provides accuracy advantages.

In comparison, the position error of the EKPF algorithm in all axes remained low. The root mean square error (RMSE) of the positioning of the EKPF algorithm in the X-axis was 0.073 m, which represents an accuracy increase of 28% compared with that of the pure UWB algorithm. In the Y-axis, the RMSE of the positioning of the EKPF algorithm was 0.065 m: an accuracy increase of 33% compared with that of the pure UWB algorithm. For the Z-axis, the positioning RMSE of the EKPF algorithm was 0.1838 m, whereas that of the pure UWB algorithm was 1.1 m, showing that, in the Z-axis direction, the EKPF algorithm was more reliable than the pure UWB algorithm.

Overall, the experimental results showed that UWB/MIMU integrated navigation was more accurate than the traditional UWB solution in the outdoor LOS environment. Additionally, attention had to be paid to the pure UWB algorithm in the Z-axis direction in the case of large height differences, whereas the fused multisensor method was more robust and accurate.

(2) Data processing and analysis of outdoor NLOS environment experiment.

The RMSEs of the positioning of each algorithm in the outdoor NLOS environment are shown in

Table 8. The RMSEs of positioning for different algorithms under outdoor NLOS conditions.

Flight Path	Pure UWB (m)			EKPF (m)
	X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Route 6	0.150	0.141	1.344	0.091	0.091	0.242
Route 7	0.154	0.152	1.201	0.094	0.094	0.189
Route 8	0.149	0.175	1.125	0.098	0.101	0.232
Route 9	0.113	0.158	1.745	0.081	0.097	0.178
Route 10	0.135	0.145	2.045	0.094	0.085	0.192
Mean	0.141	0.154	1.495	0.091	0.094	0.207

. The error of the pure UWB algorithm on the X- and Y-axes in the NLOS environment was higher by approximately 5 cm than in the LOS environment, and the positioning error on the Z-axis was substantially larger. In contrast, the integrated system successfully reduces the error growth in the X- and Y-directions by fusing MIMU sensor data. This proves the effectiveness of integrated navigation in correcting UWB system errors in NLOS environments.

In terms of numerical performance, the RMSE for the X-axis position using the EKPF algorithm was 0.091 m, which is an accuracy increase of 35% compared with that of the pure UWB algorithm. On the Y-axis, the positioning RMSE of the EKPF algorithm was 0.094 m, an increase of 39%. On the Z-axis, the RMSE of the positioning of the EKPF algorithm was 0.207 m, whereas that of the pure UWB algorithm was 1.5 m. This comparison shows that the EKPF algorithm provides considerable advantages over the pure UWB algorithm in the vertical direction and proves the effectiveness of the UWB/MIMU integrated system.

5. Conclusions

In this study, focusing on the NLOS error propagation problem encountered in pure UWB positioning without GNSS assistance, an UWB/MIMU integrated navigation method was developed to increase positioning accuracy. The traditional PF algorithm used for the data fusion process was improved. Obtaining the posterior probability distribution during particle sampling is difficult, which leads to low sampling efficiency and particle degradation, which are caused by an excessively long filtering time. EKF uses real-time measurement information to set the IDF for sampling, which achieves the optimal estimation of the PF. In this study, the EKPF algorithm was compared with the traditional EKF and UKF algorithms through simulation, and the effectiveness of the proposed algorithm was illustrated. Considering the need for fully autonomous UAV operation, a more practical integrated navigation experiment was conducted in an outdoor environment. Through comparison, we found that, in NLOS situations, the positioning accuracy of the improved filtering fusion algorithm in the X- and Y-axes was 35% and 39% higher than that of the pure UWB algorithm, respectively. In the Z-axis direction, the RMSE of the pure UWB position was as large as 1.5 m, whereas that of the proposed algorithm was 0.207 m, which presents a considerable increase in vertical positioning accuracy. The experimental results showed that the positioning algorithm proposed in this paper maintains reliability across a wide range of sites and NLOS environments. This conclusion provides important guidance and reference for the development of positioning systems in complex environments.