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

Abstract

Due to some harsh indoor environments, the signal of the ultra wide band (UWB) may be lost, which makes the data fusion filter can not work. For overcoming this problem, the neural network (NN) assisted Kalman filter (KF) for fusing the UWB and the inertial navigation system (INS) data seamlessly is present in this work. In this approach, when the UWB data is available, both the UWB and the INS are able to provide the position information of the quadrotor, and thus, the KF is used to provide the localization information by the fusion of position difference between the INS and the UWB, meanwhile, the KF can provide the estimation of the INS position error, which is able to assist the NN to build the mapping between the state vector and the measurement vector off-line. The NN can estimate the KF’s measurement when the UWB data is unavailable. For confirming the effectiveness of the proposed method, one real test has been done. The test’s results demonstrate that the proposed NN assisted KF is effective to the fusion of INS and UWB data seamlessly, which shows obvious improvement of localization accuracy. Compared with the LS-SVM assisted KF, the proposed NN assisted KF is able to reduce the localization error by about 54.34%.

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 [Formula omitted. See PDF.] 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 [Formula omitted. See PDF.] 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 [Formula omitted. See PDF.] . Then, the δPo is used as the measurement of the KF, which makes the KF can work when the UWB measurement is outage.

Figure 1: The data fusion scheme when the UWB measurements are available. DOI: 10.7717/peerj-cs.630/fig-1

Figure 2: The data fusion scheme when the UWB measurements are unavailable. DOI: 10.7717/peerj-cs.630/fig-2

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).

[Formula omitted. See PDF.]

where the time index is denoted as t, δt means the sample time, [Formula omitted. See PDF.] means the position error vector at the time index t, here, the [Formula omitted. See PDF.] means the position error in the east, north, and up direction respectively, [Formula omitted. See PDF.] means the velocity error vector at the time index t, here, the [Formula omitted. See PDF.] 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).

[Formula omitted. See PDF.]

where [Formula omitted. See PDF.] is the INS-measured position Po^(I) in east, north, and the upside direction, respectively, [Formula omitted. See PDF.] 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.

Algorithm 1: The KF filtering algorithm based on the model (1) and (2).

Data: Yt, Q, R

Result: x^t ,P^t

1 begin

2 for t = 1: ∞ do

3 x^t|t−1=Fx^t−1 ;

4 P^t|t−1=FP^t−1FT+Q ;

5 Kt=P^t|t−1HT(HP^t|t−1HT+R)−1 ;

6 x^t=x^t|t−1+Kt[Yt−Hx^t|t−1] ;

7 P^t=(I−KtH)P^t|lt−1 ;

8 end for

9 end

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

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 [Formula omitted. See PDF.] . 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 [Formula omitted. See PDF.] collected after normal flight of the quadrotor. The input and target of the NN model are chosen as [Formula omitted. See PDF.] and Y_t respectively. In this work, we select the simple BP neural network structure without hidden layer. Build the mapping between [Formula omitted. See PDF.] and Y_t using the δPo_t, t∈ [1,∞) and the [Formula omitted. See PDF.] via NN.

The NN method is summarised in Algorithms 2 and 3. In the Algorithm 2, the KF provides the [Formula omitted. See PDF.] and the [Formula omitted. See PDF.] normally. Then, the NN is used to build the mapping between [Formula omitted. See PDF.] and Y_t using the δPo_t, t ∈ [1,∞) and the [Formula omitted. See PDF.] on off-line model.

Algorithm 2: NN assisted Kalman filtering algorithm (off-line model).

Data: Yt, Q, R

Result: x^t ,P^t the mapping between X^t |t −1 and Yt

1 begin

2 for `t = 1:∞ do

3 x^t|t−1=Fx^t−1 ;

4 P^t|t−1=FP^t−1FT+Q ;

5 Kt=P^t|t−1HT(HP^t|t−1HT+R)−1 ;

6 x^t=x^t|t−1+Kt[Yt−Hx^t|t−1] ;

7 P^t=(I−KtH)P^t|lt−1 ;

8 end for

9 Build the mapping between x^t|t−1 and Yt using the δPot, t ∈ [1,∞) and the x^t|t−1,t∈[1,∞) via NN;

10 end

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

Algorithm 3: NN assisted Kalman filtering algorithm (on-line model).

Data: Yt, Q, R, the mapping between x^t|t−1 and Yt

Result: x^t ,P^t

1 begin

2 for t = 1:∞ do

3 x^t|t−1=Fx^t−1 ;

4 P^t|t−1=FP^t−1FT+Q ;

5 if Po(U) is available then

6 Yt=[xt(I)−xt(U)yt(I)−yt(U)zt(I)−zt(U)] ;

7 else

8 Estimate Yt using x^t|t−1 and previously built the mapping between x^t|t−1 and Yt via NN;

9 end if

10 Kt=P^t|t−1HT(HP^t|t−1HT+R)−1 ;

11 x^t=x^t|t−1+Kt[Yt−Hx^t|t−1] ;

12 P^t=(I−KtH)P^t|lt−1 ;

13 end for

14 end

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

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 [Formula omitted. See PDF.] . Once the Po^(U) is unavailable, the proposed NN assisted Kalman filtering algorithm estimate Y_t using [Formula omitted. See PDF.] 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.

Figure 3: Test environment. DOI: 10.7717/peerj-cs.630/fig-3

Figure 4: The quadrotor used in this work. DOI: 10.7717/peerj-cs.630/fig-4

Figure 5: The reference path, UWB RNs, and the outage areas used in the test. DOI: 10.7717/peerj-cs.630/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:

[Formula omitted. See PDF.]

where [Formula omitted. See PDF.] means the MSE of the position at time index t, [Formula omitted. See PDF.] is the estimated position in x, y, and z directions at the time index t, [Formula omitted. See PDF.] 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.

Figure 6: The trajectories estimated by the LS-SVM and the NN in outage areas: (A) outage #1, (B) outage #2, (C) outage #3, and (D) outage #4. DOI: 10.7717/peerj-cs.630/fig-6

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.

Figure 7: The MSEs estimated by the LS-SVM and the NN in outage areas: (A) outage #1, (B) outage #2, (C) outage #3, and (D) outage #4. DOI: 10.7717/peerj-cs.630/fig-7

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 [Formula omitted. See PDF.] 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

Distance information measured by UWB.

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

Download

Location information measured by INS.

DOI: 10.7717/peerj-cs.630/supp-2

Download

Reference trajectory.

DOI: 10.7717/peerj-cs.630/supp-3

Download

Heading information.

DOI: 10.7717/peerj-cs.630/supp-4

Download

KF based estimation procedure.

DOI: 10.7717/peerj-cs.630/supp-5

Download

KF based prediction program.

DOI: 10.7717/peerj-cs.630/supp-6

Download

Main simulation program.

DOI: 10.7717/peerj-cs.630/supp-7

Download

Additional Information and Declarations

Competing Interests

The authors declare that they have no competing interests.

Author Contributions

Shuhui Bi conceived and designed the experiments, authored or reviewed drafts of the paper, and approved the final draft.

Liyao Ma analyzed the data, prepared figures and/or tables, and approved the final draft.

Tao Shen conceived and designed the experiments, prepared figures and/or tables, and approved the final draft.

Yuan Xu performed the computation work, authored or reviewed drafts of the paper, and approved the final draft.

Fukun Li performed the experiments, prepared figures and/or tables, and approved the final draft.

Data Availability

The following information was supplied regarding data availability:

The raw data and codes are available in the Supplemental Files.

Funding

This work was supported by National Natural Science Foundation of China No. 61803175, Shandong Provincial Key Research and Development Project No. 2019GGX104026, and Shandong Provincial Natural Science Foundation ZR2020KF027. The funders had no role in study design, data collection and analysis, decision to publish, or preparation of the manuscript.

References

Benzerrouk H, Nebylov A, Salhi H. 2016. Quadrotor UAV state estimation based on high-degree cubature Kalman filter. IFAC-PapersOnLine 49(17):349-354

Bonny T, Abdelsalam MB. 2019. Autonomous navigation of unmanned aerial vehicles based on android smartphone. International Journal of Advacnced Computer Science and Applications 10(11):589-598

Camci E, Kayacan E. 2019. Learning motion primitives for planning swift maneuvers of quadrotor. Autonomous Robots 43(7):1733-1745

Chiella ACB, Machado HN, Teixeira BOS, Pereira GAS. 2019. GNSS/LiDAR-based navigation of an aerial robot in sparse forests. Sensors 19(19):1-22

Cui B, Wei X, Chen X, Li J, Li L. 2019. On sigma-point update of cubature Kalman filter for GNSS/INS under GNSS-challenged environment. IEEE Transactions on Vehicular Technology 68(9):8671-8682

Do T, Carrillo-Arce LC, Roumeliotis SI. 2019. High-speed autonomous quadrotor navigation through visual and inertial paths. International Journal of Robotics Research 38(4):486-504

Gośliński J, Kasiński A, Giernacki W, Owczarek P, Gardecki S. 2019. A study on coaxial quadrotor model parameter estimation: an application of the improved square root unscented Kalman filter. Journal of Intelligent and Robotic Systems 95(2):491-510

Guo S, Chang L, Li Y, Sun Y. 2020. Robust fading cubature Kalman filter and its application in initial alignment of sins. OPTIK 202(2):163593

Kou L, Xiang J, Li Y, Bian J. 2018. Stability and nonlinear controllability analysis of a quadrotor-like autonomous underwater vehicle considering variety of cases. International Journal of Advanced Robotic Systems 15(6):1-10

Li T, Zhang H, Gao Z, Niu X, El-Sheimy N. 2019. Tight fusion of a monocular camera, MEMS-IMU, and single-frequency multi-GNSS RTK for precise navigation in GNSS-challenged environments. Remote Sensing 11(6):1-24

Liang X, Wang Q, Hu C, Dong C. 2019. Observer-based h-infinity fault-tolerant attitude control for satellite with actuator and sensor faults. Aerospace Science and Technology 95(4):105424

Liu D, Chen X, Xu Y, Liu X, Shi C. 2019. Maximum correntropy generalized high-degree cubature Kalman filter with application to the attitude determination system of missile. Aerospace Science and Technology 95(6):105441

Liu P, Huda MN, Sun L, Yu H. 2020. A survey on underactuated robotic systems: bio-inspiration, trajectory planning and control. Mechatronics 72:102443

Liu P, Yu H, Shuang C. 2019. Adaptive neural network tracking control for underactuated systems with matched and mismatched disturbances. Nonlinear Dynamics 98(2):1447-1464

Nguyen X-M, Hong SK. 2019. Robust adaptive formation control of quadcopters based on a leader-follower approach. International Journal of Advanced Robotic Systems 16(4):1-11

Qin H-D, Yu X, Zhu Z-B, Deng Z-C. 2020. An expectation-maximization based single-beacon underwater navigation method with unknown ESV. Neurocomputing 378:295-303

Schmid K, Lutz P, Tomić T, Mair E, Hirschmüller H. 2014. Autonomous vision-based micro air vehicle for indoor and outdoor navigation. Journal of Field Robotics 31(4):537-570

Wang R, Xiong Z, Liu J, Cao Y. 2018. Stepwise fusion algorithm with dual correction for multi-sensor navigation. International Journal of Advanced Robotic Systems 15(3):1-11

Xu Y, Shmaliy YS, Ahn CK, Shen T, Zhuang Y. 2020a. Tightly-coupled integration of INS and UWB using fixed-lag extended UFIR smoothing for quadrotor localization. IEEE Internet of Things Journal 8(3):1716-1727

Xu Y, Shmaliy YS, Chen X, Li Y, Ma W. 2020b. Robust inertial navigation system/ultra wide band integrated indoor quadrotor localization employing adaptive interacting multiple model-unbiased finite impulse response/Kalman filter estimator. Aerospace Science and Technology 98(3):UNSP 105683

Zahran S, Moussa AM, Sesay AB, El-Sheimy N. 2019. A new velocity meter based on hall effect sensors for UAV indoor navigation. IEEE Sensors Journal 19(8):3067-3076

Zhang Y, Dong Z, Wang SH, Yu X, Górriz JM. 2020. Advances in multimodal data fusion in neuroimaging: overview, challenges, and novel orientation. Information Fusion 64:149-187

Zhang Y, Satapathy SC, Guttery DS, Górriz JM, Wang S. 2021. Improved breast cancer classification through combining graph convolutional network and convolutional neural network—sciencedirect. Information Processing and Management 58(2):102439

Zhao S, Huang B. 2020. Trial-and-error or avoiding a guess? Initialization of the Kalman filter. Automatica 121(21):109184