Neural network assisted Kalman filter for INS/UWB integrated seamless quadrotor localization

Introduction

Nowadays, the quadrotor has been widely used in many fields (Xu et al., 2020a; Nguyen & Hong, 2019; Kou et al., 2018). Consequently, many approaches have been proposed for the quadrotor (Liang et al., 2019). In order to make the quadrotor have better performance, the accurate localization scheme, which is the key technology of the quadrotor to accomplish other tasks, should be investigated (Camci & Kayacan, 2019).

To the localization technologies for the quadrotor, there are many approaches have been proposed. For instance, a smart quadcopter aircraft navigation system using the global positioning system (GPS) was designed, which can achieve autonomous flight control with smooth and stable maneuvering, see Bonny & Abdelsalam (2019). Global navigation satellite systems (GNSS) intigrating light detection and ranging (LiDAR) scheme was investigated to achieve the autonomous navigation in forests (Chiella et al., 2019). The indoor quadrotor localization integrated by inertial navigation system (INS) and ultra wide band (UWB) was proposed by Xu et al. (2020b). A high-speed autonomous quadrotor navigation through visual and inertial paths was proposed (Do, Carrillo-Arce & Roumeliotis, 2019). Autonomous vision-based micro air vehicle for indoor and outdoor navigation was investigated in Schmid et al. (2014). It should be emphasized that the basic idea of the approaches mentioned above is to replace the unavailable positioning technology with a available one.

In aggregate, the data fusion filter has played an important role in integrated navigation system (Zhao & Huang, 2020; Wang et al., 2018; Li et al., 2019; Liu, Yu & Shuang, 2019). Moreover, the Kalman filter (KF) with its improving filters have been proposed for the data fusion (Liu et al., 2020). For example, the fading cubature Kalman filter (CKF) was designed to the initial alignment of strapdown inertial navigation system (SINS) (Guo et al., 2020). The quadrotor state estimation based on CKF was proposed (Benzerrouk, Nebylov & Salhi, 2016). An improving CKF method was investigated for the the attitude determination system of missile (Liu et al., 2019). The CKF is used for the GNSS/INS under GNSS-challenged environment (Cui et al., 2019). An improved square root unscented Kalman filter was proposed for the localization of the coaxial Quadrotor (Gośliński et al., 2019). A Kalman filter/expectation maximization (EM) integrated frame was proposed in Qin et al. (2020). A new approach for enhancing the indoor navigation of unmanned aerial vehicles (UAVs) with velocity update applied to an extended Kalman filter (EKF) was investigated by Zahran et al. (2019). It should be pointed out that the outage of the data fusion filter’s measurement are not considered by the approaches mentioned above. Meanwhile, in order to ensure that the data fusion filter works, some artificial intelligence (AI)-based methods have been proposed, which have been used used in other fields (Zhang et al., 2021, 2020).

In this paper, we propose a neural network (NN) assisted KF, which is able to deal with the missing data in case of UWB data outage. Neural network is used to build the mapping between states and observations. The performance is verified with real data. Comparison shows that the proposed approach outperforms LS-SVM algorithm significantly in accuracy improvement.

The contributions of this work are listed in the following:

• A new NN assisted KF for fusing the UWB and INS data seamlessly is presented in this work, which employs the NN to build mapping between states and observations offline and predict the observations when the UWB is outage.

• Real tests have been done for demonstrating the effectiveness of the proposed approach.

The remainder structure of this article is sketched as follows. The description of INS/UWB integrated seamless quadrotor localization scheme is given in “INS/UWB Integrated Seamless Quadrotor Localization Scheme”. “Kalman Flilter” and “The Scheme of the NN” investigated the KF and the NN method for the localization scheme of INS/UWB integrated seamless quadrotor. The test is done in the “Test” section. Finally, conclusions are drawn in the “Conclusion” section.

INS/UWB Integrated Seamless Quadrotor Localization Scheme

In this section, the INS/UWB integrated seamless quadrotor localization scheme will be designed in two cases. The integrated seamless scheme proposed in this work are listed in the following:

• When the UWB measurements are available, the data fusion scheme is shown in Fig. 1. In this situation, the INS and UWB localization technologies measure the target quadrotor’s position Po^(I) and Po^(U) respectively. Then, the Kalman filter (KF) estimates the position Po by fusing the Po^(I) and Po^(U).

• Using the outputs and the measurements of the KF when the UWB measurements are available, the NN works in the training stage, it builds the mapping between the KF’s measurement δPo_t, t ∈ [1, +∞) and the data filter’s state vector ${\hat{x}}_{t|t-1},t\in [1,+\mathrm{\infty })$ after normal flight of the quadrotor. Here, the t is the time index. It should be pointed out that both the δPo_t, t ∈ [1, ∞) and the ${\hat{x}}_{t|t-1},t\in [1,\mathrm{\infty })$ are collected when the KF works normally, and the building process of the mapping is off-line.

• When the UWB measurements are not available, the data fusion scheme can be designed as Fig. 2. In this situation, the UWB is unable to provide the Po^(U) due to the outage of the UWB. Thus, the KF is unable to work. In this situation, the NN is employed to rebuild the measurement of the KF. It works in prediction stage, which is utilized to provide the estimated position error δPo by using the mapping built in the above stage and the ${\hat{x}}_{t|t-1}$. Then, the δPo is used as the measurement of the KF, which makes the KF can work when the UWB measurement is outage.

Kalman Filter

Based on the seamless integrated scheme, the KF used in this work will be introduced in this section. The state equation of KF used in this work is listed in Eq. (1).

(1) $$\underset{{\mathbf{x}}_{t|t-1}}{\underbrace{\left[\begin{array}{c}\delta \mathbf{P}{\mathbf{o}}_{t|t-1}\\ \delta {\mathbf{V}}_{t|t-1}\end{array}\right]}}=\underset{\mathbf{F}}{\underbrace{\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& \delta t\cdot {\mathbf{I}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right]}}\underset{{\mathbf{x}}_{t-1}}{\underbrace{\left[\begin{array}{c}\delta \mathbf{P}{\mathbf{o}}_{t-1}\\ \delta {\mathbf{V}}_{t-1}\end{array}\right]}}+{\omega }_{t-1}$$

where the time index is denoted as t, δt means the sample time, $\delta {\mathbf{P}\mathbf{o}}_t={\left[\delta x_t,\delta y_t,\delta z_t\right]}^T$ means the position error vector at the time index t, here, the $\left(\delta x_t,\delta y_t,\delta z_t\right)$ means the position error in the east, north, and up direction respectively, $\delta {\mathbf{V}}_t={\left[\delta V{x}_t,\delta V{y}_t,\delta V{z}_t\right]}^T$ means the velocity error vector at the time index t, here, the $\left(\delta V{x}_t,\delta V{y}_t,\delta V{z}_t\right)$ means the velocity error in the east, north, and up direction respectively, ω_t−1 ∼ N(0, Q) is the system noise and Q is its covariance.

The measurement equation of KF used in this work is listed in Eq. (2).

(2) $$\underset{{\mathbf{Y}}_t}{\underbrace{\left[\begin{array}{c}{x}_t^{\left(I\right)}-x_t^{\left(U\right)}\\ y_t^{\left(I\right)}-y_t^{\left(U\right)}\\ z_t^{\left(I\right)}-z_t^{\left(U\right)}\end{array}\right]}}=\underset{\mathbf{H}}{\underbrace{\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]}}{\mathbf{x}}_{t|t-1}+{\upsilon }_{t-1},$$

where $\left(x_t^{\left(I\right)},y_t^{\left(I\right)},z_t^{\left(I\right)}\right)$ is the INS-measured position Po^(I) in east, north, and the upside direction, respectively, $\left(x_t^{\left(U\right)},y_t^{\left(U\right)},z_t^{\left(U\right)}\right)$ is the UWB-measured position Po^(U) in east, north, and the up direction respectively, υ_t ∼ N(0, R) is the measurement noise and R is its covariance. The KF filtering algorithm based on the model (1) and (2) is listed in Algorithm 1.

The Scheme of the Neural Network (NN)

In case of outage in complex indoor environment, due to the lack of UWB measurements, the observation vector in Kalman filter become unavailable. To provide the observation vector for the data fusion filter, the Neural Network (NN) is employed in this work.

However, it should be noticed that it is hard to model mathematically the relation between the measurements of the data fusion filter Y_t and the state vector ${\hat{\mathbf{x}}}_{t|t-1}$. For overcoming this issue, the NN is trained to build the mapping between them using the KF’s measurement Y_t, t ∈ [1, +∞) and the ${\hat{\mathbf{x}}}_{t|t-1},t\in [1,+\mathrm{\infty })$ collected after normal flight of the quadrotor. The input and target of the NN model are chosen as ${\hat{\mathbf{x}}}_{t|t-1}$ and Y_t respectively. In this work, we select the simple BP neural network structure without hidden layer. Build the mapping between ${\hat{\mathbf{x}}}_{t|t-1}$ and Y_t using the δPo_t, t∈ [1,∞) and the ${\hat{\mathbf{x}}}_{t|t-1},t\in [1,\mathrm{\infty })$ via NN.

The NN method is summarised in Algorithms 2 and 3. In the Algorithm 2, the KF provides the ${\hat{\mathbf{x}}}_t$ and the ${\hat{\mathbf{P}}}_t$ normally. Then, the NN is used to build the mapping between ${\hat{\mathbf{x}}}_{t|t-1}$ and Y_t using the δPo_t, t ∈ [1,∞) and the ${\hat{\mathbf{x}}}_{t|t-1},t\in [1,\mathrm{\infty })$ on off-line model.

In the Algorithm 3, the KF works normally when the Po^(U) is available. Here, the KF is used to provide the estimation of the δPo using the observation vector ${\mathbf{Y}}_{\mathbf{t}}={\left[\begin{array}{ccc}{x}_t^{(\mathrm{I})}-x_t^{(\mathrm{U})}& {\mathrm{y}}_t^{(\mathrm{I})}-y_t^{(\mathrm{U})}& z_t^{(\mathrm{I})}-z_t^{(\mathrm{U})}\end{array}\right]}^T$. Once the Po^(U) is unavailable, the proposed NN assisted Kalman filtering algorithm estimate Y_t using ${\hat{\mathbf{x}}}_{t|t-1}$ and previously built mapping via NN.

Test

In order to demonstrate the effectiveness of the proposed method, the real test will be investigated in this section.

Experimental settings

In this section, the real test will be considered to show the validity of the proposed method. The real test is done in the No. 1 building, University of Jinan, China, the test environment is displayed in Fig. 3. The quadrotor used in this work is shown in Fig. 4. Here, we employ the quadrotor to carry UWB blind node (BN) and the inertial measurement unit (IMU). The UWB BN fixed on the target quadrotor is able to collect the distances d_i,i ∈ [1, 6] between the target quadrotor and the UWB reference node (RN). Here, the i has the same number as the UWB RN. Then, the UWB position Po^(U) can be computed via the the d_i,i ∈ [1, 6]. And the INS position Po^(I) is provided by the IMU. The difference δPo between the Po^(I) and Po^(U) is used as the measurement of the KF. In the test, the quadrotor runs following the reference path, which is shown in Fig. 5. In this work, the sample time is set to 0.02s. In order to indicate the effect of the proposed method, four UWB outage areas (#1, #2, #3, and #4) are simulated as shown in Fig. 5.

Localization errors

In this subsection, the performance of the proposed NN assisted KF will be investigated. Here, we compare the NN assisted KF’s performance with the least squares support vector machine (LS-SVM) assisted KF. In this work, we employ the mean square error (MSE) at each time index, which is calculated by the follows:

(3) $$\begin{array}{c}\mathrm{M}\mathrm{S}\mathrm{E}{\left(\mathbf{P}\mathbf{o}\right)}_t\\ ={\displaystyle \frac{1}{3}\left({\left(x_t-x_t^{ref}\right)}^2+{\left(y_t-y_t^{ref}\right)}^2+{\left(z_t-z_t^{ref}\right)}^2\right)}\\ \end{array},$$

where $\mathrm{M}\mathrm{S}\mathrm{E}{\left(\mathbf{P}\mathbf{o}\right)}_t$ means the MSE of the position at time index t, $\left(x_t,y_t,z_t\right)$ is the estimated position in x, y, and z directions at the time index t, $\left(x_t^{ref},y_t^{ref},z_t^{ref}\right)$ is the reference position in x, y, and z directions at the time index t.

Figure 6 shows the trajectories estimated by the LS-SVM and the NN in outage areas #1, #2, #3, and #4. From the figures, one can see easily that in the outages areas #1, #2, #3, and #4, when UWB measurements are unavailable, the NN can still make decisions that are close to the reference path, while the LS-SVM algorithm gives a large accumulated error.

The MSEs estimated by NN (green line) and LS-SVM (blue line) in the outages areas #1, #2, #3, and #4 are shown in Fig. 7. From the figures, one can see that the MSE of the LS-SVM algorithm has a larger accumulated error compared with the NN. The average MSEs Produced by NN and LS-SVM in the outages areas #1, #2, #3, and #4 are listed in Table 1. It can be infered from the table that the average MSEs of the NN are smaller than the LS-SVM in the outages areas #1, #2, #3, and #4. Compared with the LS-SVM, the proposed NN reduced the localization error by about 54.34%. Thus, we can conclude that the proposed NN-based method can effectively reduce the localization error.

Table 1:

Average MSEs produced by NN and LS-SVM in outages #1–#4.

Method	MSE (m2)
	#1	#2	#3	#4	Mean
LS-SVM	2.7445	0.1453	2.7147	16.6635	5.5670
NN	0.0190	0.0524	0.0422	0.0537	2.5418

DOI: 10.7717/peerj-cs.630/table-1

Conclusion

In this work, in order to make the data fusion filter work properly under the condition that the UWB data is unavailable due to some harsh indoor environments, the NN assisted KF for fusing the UWB and the INS data seamlessly has be investigated. The contributions of this work are summarized as following:

• An NN assisted KF scheme has been designed for fusing the INS and UWB measurement.

• The model of the KF for the integrated scheme has been investigated.

• The NN assisted KF for fusing the UWB and the INS data seamlessly has been investigated. In the proposed approach, the KF provides the localization information when the UWB data is available. Meanwhile, the KF is used to assist the NN to build the mapping between the ${\hat{\mathbf{x}}}_{t|t-1}$ and Y_t off-line. The NN can estimate the measurement vector of the KF when the UWB data is unavailable.

• Real tests have been done to show better performance of the proposed approach.

Based on the results presented in this work, we are now working on further developments of the proposed algorithms to build the mapping with the deep learning and plan to report the results in the near future.

Supplemental Information