A FIR filter assisted with the predictive model and ELM integrated for UWB-based quadrotor aircraft localization

Abstract

To improve the accuracy of the Ultra-Wide Band (UWB) based quadrotor aircraft localization, a Finite Impulse Response (FIR) filter aided with an integration of the predictive model and Extreme Learning Machine (ELM) is proposed in this work. The FIR filter estimates the quad-rotor aircraft ’s position by fusing the positions measured with the UWB and Inertial Navigation System respectively. When the UWB dada are unavailable, both the ELM and the predictive model are used to provide the measurements, replacing those unavailable UWB data, for the FIR filter. The ELM estimates the measurement via the mapping between the one step prediction of state vector and the measurement built when the UWB data are available. For the predictive model, we mathematically describe the missing UWB data. Then, both the measurements estimated with the ELM and predictive model are employed to estimate the observations via Mahalanobis distance. The test results show that the FIR filter aided by the predictive model/ELM integrated can reduce the Cumulative Distribution Function and position Root Mean Square Error effectively when the UWB is unavailable. Compared with the ELM assisted FIR filter, the proposed FIR filter can reduce the localization error by about 48.59 %, meanwhile, the integrated method is close to the method with a better solution.

Graphical Abstract

Introduction

Quadrotor aircraft is an indispensable instruments in our daily life and industrial applications (Gadekar et al., 2022; Xu et al., 2021; Liu et al., 2021). The navigation and positioning accuracy is one of the most important—performance indicators.

For improving the location accuracy, a variety of sensor technologies have been put forward and applied. For instance, the Radio Frequency Identification (RFID) with single passive tag was used in the intelligent mobile robot navigation (Wu et al., 2021; Gueaieb and Miah 2008), and the indoor mobile localization method based on Wireless Fidelity (WiFi) fingerprint was proposed by Bai et al. (2021). These localization technologies can provide the navigation information with meter level positioning accuracy. However, they cannot meet the needs of high-precision tasks. For improving the positioning accuracy, the Ultra-Wide Band (UWB) technology was proposed, which can provide decimeter-level positioning accuracy. For example, a UWB localization system was deigned for the underground coal mine application (Li et al., 2020) and the indoor quadrotor localization and so on (Xu et al., 2020a).

However, many problems are prone to occur in the use of sensors for quadrotors. For instance, it is not able to get the high precision navigation information when the Global Navigation Satellite System (GNSS) is not available in outdoor navigation. Therefore, many ways of integrating multiple sensors have been proposed to enhance the positioning accuracy. Ma et al. (2019) used an Inertial Measurement Unit (IMU) and UWB to realize the precise hovering at a certain position for a Mini-quadrotor. IMU and a sonar sensor were combined together in a real-time monocular visual navigation system for ensuring the robust and accurate navigation results for a quadrotor (Zheng et al., 2015). The Inertial Navigation System (INS) and Global Positioning System (GPS) was integrated by Dang et al. (2022). An integration of multiple sensors takes full advantages of each sendor involved, improving the positioning accuracy. UWB and INS will be used together in a quadrotor aircraft.

In addition, an advanced filtering method is also an effective tool to improve positioning accuracy. The Kalman Filter (KF) and its variants based on the RFID, WiFi, and UWB localization technologies are the most used methods. For instance, the Kalman Filter based on a High-Frequency RFID (HF RFID) positioning measurement system was employed to locate a mobile object has been considered in Shirehjini and Shirmohammadi (2020). A complementary Kalman Filter was proposed for the UWB-based indoor localization (Liu et al., 2019). Meanwhile, a federated derivative Cubature Kalman Filtering (CKF) methodology was proposed for the UWB based an indoor localization system by the combination of traditional KF and CKF (He et al., 2020). Wang et al. (2021) presented a relative position estimation algorithm based on an Extended Kalman Filtering (EKF) for the robot in the local reference frame. Unscented Kalman Filtering (UKF) was modified in vehicular integration system (Hu et al., 2020).

Since the Kalman Filter depends greatly on the model noise (Zhao and Huang 2020; Zhao et al., 2016), an Interacting Multiple Model (IMM) was used to fit the current noise statistics (Liao and Chen 2006; Li and Bar-Shalom 1994), and the Expectation-Maximization (EM) based KF approaches were designed (Huang et al., 2018; Cui et al., 2019). Moreover, a Finite Impulse Response (FIR) filter was employed to robustly provide the robot’s accurate position by fusing the RFID based distance (Pomarico-Franquiz and Shmaliy 2014). The Extended FIR Filter and Composite Particle/FIR Filter were designed for improving the localization accuracy (Xu et al., 2018). The robustness of FIR were fully verified (Pak et al., 2017; Chandra et al., 2021; Paka and Ahn 2021; Zhao et al., 2020). Although the FIR-based approach can improve the robustness, the accurate state and noise covariance matrices are still important factors affecting the positioning accuracy due to the truncation error in the nonlinear system.

Accuracy, robustness, and efficiency are the most used requirements for quadrotor aircraft localization. Although many filtering methods can obtain the innovative results in terms of accuracy and robustness, they still suffer from sensor unavailability and are prone to errors. For this case, there are many approaches for seamless localization. For example, the EKF and LS-SVM were proposed (Chen et al., 2016)to achieve the seamless navigation, The decision tree method (Xu et al., 2020b) was used to compensate the outage of the UWB data, and the LS-SVM assisted UFIR filter was used (Xu et al., 2020c) in seamless indoor pedestrian tracking. Moreover, the Extreme Learning Machine (ELM) has its advantage and can be factored as a complement. For example, for handling the drift originated in severe occlusion, an online classifier based on ELM for reinitializing the object on the condition of tracking failure was used by Qu et al. (2019). Bi et al. (2022) proposed the ELM assisted particle filter for INS/UWB integrated quadrotor positioning.

In this paper, for enhancing the accuracy of the UWB based quadrotor aircraft localization, a FIR filter aided with the integration of the predictive model and ELM is derived. The FIR filter estimates the quadrotor aircraft’s position by fusing the positions measured by the UWB and Inertial Navigation System (INS) respectively. When the UWB dada is unavailable, the ELM and the predictive model will together provide the measurements, replacing those missing UWB data, for the FIR filter. ELM estimates the measurements via the mapping between the one step prediction from the state vector and the built measurements under the condition that UWB data is available. The predictive model will mathematically describe the missing UWB data. Then, both the measurements provided by the ELM and predictive model are employed to estimate the observation via distance Mahalanobis.

The remainder of the paper is organized as follows. Firstly, we introduce the FIR filter assisted with the predictive model/ELM integrated. Then we compare the localization error of the proposed filter with the ELM assisted FIR filter. Finally, the results and conclusions are given.

The FIR filter assisted with the predictive model/ELM integrated

In this section, we will design a FIR filter assisted with the predictive model/ELM integrated. Then scheme of the FIR filter assisted with the predictive model/ELM integrated for UWB-based quad-rotor aircraft localization is first introduced. The FIR filter and the ELM method are then briefly introduced.

The scheme of the FIR filter assisted with the predictive model/ELM integrated for UWB-based quad-rotor aircraft localization

In this subsection, we will discuss the scheme of the FIR filter assisted with the predictive model/ELM integrated. The technical scheme of the proposed filtering method for UWB is shown in Fig. 1.

Fig. 1

The scheme of the predictive model/ELM integrated assisted FIR filter for UWB-based quad-rotor aircraft localization

In this work, we employ the loosely integrated model to fuse the UWB and the INS data. Firstly, the UWB localization system measure the position ${\varvec{P}}_{{o}}^{\mathrm U}$ of a target quad-rotor aircraft by fusing the distances between the UWB Reference Nodes (RNs) and the UWB Blind Node (BN). Meanwhile, the INS measures the position ${\varvec{P}}_{{o}}^{\mathrm I}$ of the target quad-rotor aircraft. With the difference between the ${\varvec{P}}_{{o}}^{\mathrm U}$ and ${\varvec{P}}_{{o}}^{\mathrm I}$, the proposed FIR filter assisted with predictive model/ELM integrated estimates the INS’s position error $\mathrm{{\delta }} {\varvec{P}}_{{o}}^{\mathrm{{I}}}$, which is designed as the main filter and is applied to correct the position error from INS. Finally, we can obtain ${{\varvec{P}}_{{o}}} = {\varvec{P}}_{{o}}^{\mathrm{{I}}} - \mathrm{{\delta }} {\varvec{P}}_{{o}}^{\mathrm{{I}}}$.

Data fusion model

In this subsection, the used FIR filter is introduced firstly, and the loosely-coupled integrated model is employed. The state equation used for the FIR filter is listed in Eq. (1), which is the same as the state equation proposed in Xu et al. (2019a).

$$\begin{aligned}{} & {} {\underbrace{\left[ {\begin{array}{*{20}{c}} {{\varvec{\phi }_{{{q|q - 1}}}}}\\ {{{\delta }}{\varvec{V}}_{{{q|q - 1}}}^{\mathrm{{n}}}}\\ {{{\delta }}{\varvec{P}}_{{{q|q - 1}}}^{\mathrm{{n}}}}\\ {\varvec{\chi }_{{{q|q - 1}}}^{\mathrm{{b}}}}\\ {\varvec{\varepsilon }_{{{q|q - 1}}}^{\mathrm{{b}}}} \end{array}} \right] }_{{{\varvec{X}}_{{{q|q - 1}}}}}} = \underbrace{\left[ {\begin{array}{*{20}{l}} {{{{\textbf {I}}}_{3 \times 3}}}&{}{{{{\textbf {O}}}_{3 \times 3}}}&{} {{{{\textbf {O}}}_{3 \times 3}}}&{}{{{{\textbf {O}}}_{3 \times 3}}}&{}{ - {{{\textbf {I}}}_{3 \times 3}} {{\textbf {C}}}_{{\textrm{b}}}^{{\textrm{n}}}{\Delta } q}\\ {S\left( {{{\textbf {f}}}_{{q}}^{{\textrm{n}}}} \right) \Delta q}&{} {{{{\textbf {I}}}_{3 \times 3}}}&{}{{{{\textbf {O}}}_{3 \times 3}}}&{}{{{{\textbf {I}}}_{3 \times 3}} {{\textbf {C}}}_{{\textrm{b}}}^{{\textrm{n}}}{\Delta } q}&{}{{{{\textbf {O}}}_{3 \times 3}}}\\ {{{{\textbf {O}}}_{3 \times 3}}}&{}{{{{\textbf {I}}}_{3 \times 3}} \Delta q}&{}{{{{\textbf {I}}}_{3 \times 3}}}&{}{{{{\textbf {O}}}_{3 \times 3}}}&{}{{{{\textbf {O}}}_{3 \times 3}}}\\ {{{{\textbf {O}}}_{3 \times 3}}}&{}{{{{\textbf {O}}}_{3 \times 3}}}&{} {{{{\textbf {O}}}_{3 \times 3}}}&{}{{{{\textbf {I}}}_{3 \times 3}}}&{}{{{{\textbf {O}}}_{3 \times 3}}}\\ {{{{\textbf {O}}}_{3 \times 3}}}&{}{{{\textbf {{O}}}_{3 \times 3}}}&{}{{{{\textbf {O}}}_{3 \times 3}}}&{}{{{{\textbf {O}}}_{3 \times 3}}}&{}{{{{\textbf {I}}}_{3 \times 3}}} \end{array}} \right] } _{{{{\textbf {F}}}_{{{q - 1}}}}} \underbrace{\left[ {\begin{array}{*{20}{c}} {{\varvec{\phi }_{{{q - 1}}}}}\\ {\mathrm{{\delta }}{\varvec{V}}_{{{q - 1}}}^{\mathrm{{n}}}}\\ {\mathrm{{\delta }}{\varvec{P}}_{{{q - 1}}}^{\mathrm{{n}}}}\\ {\varvec{\chi }_{{{q - 1}}}^{\mathrm{{n}}}}\\ {\varvec{\varepsilon }_{{{q - 1}}}^{\mathrm{{n}}}} \end{array}} \right] }_{{{\varvec{X}}_{{{q - 1}}}}} + {{\varvec{w}}_{{{q - 1}}}} \end{aligned}$$

(1)

$$\begin{aligned}{} & {} {\varvec{S}}\left( {{\varvec{f}}_{{q}}^{\textrm{n}} } \right) = \left[ {\begin{array}{*{20}c} {{0}} &{} {f_{\mathrm U}^{\mathrm n} } &{} { - f_{\mathrm N}^{\mathrm n} } \\ { - f_{\mathrm U}^{\mathrm n} } &{} {{0}} &{} {f_{\mathrm E}^{\mathrm n}} \\ {f_{\mathrm N}^{\mathrm n} } &{} { - f_{\mathrm E}^{\mathrm n} } &{} {{0}} \\ \end{array}} \right] \, , \end{aligned}$$

(2)

$$\begin{aligned}{} & {} {\varvec{C}}_{\mathrm{\mathrm b}}^{\mathrm{\mathrm n}} = \left[ {\begin{array}{*{20}c} {\cos \gamma } &{} 0 &{} { - \sin \gamma } \\ 0 &{} 1 &{} 0 \\ {\sin \gamma } &{} 0 &{} {\cos \gamma } \\ \end{array}} \right] \left[ {\begin{array}{*{20}c} 1 &{} 0 &{} 0 \\ 0 &{} {\cos \theta } &{} {\sin \theta } \\ 0 &{} { - \sin \theta } &{} {\cos \theta } \\ \end{array}} \right] \left[ {\begin{array}{*{20}c} {\cos \psi } &{} { - \sin \psi } &{} 0 \\ {\sin \psi } &{} {\cos \psi } &{} 0 \\ 0 &{} 0 &{} 1 \\ \end{array}} \right] \end{aligned}$$

(3)

In Eq. (1), the time index is denoted by q, $\varvec{\phi }_q$, $\mathrm{{\delta }} {\varvec{V}}_q^{\mathrm n}$, and $\mathrm{{\delta }} {\varvec{P}}_q^{\mathrm n}$ represent the attitude, velocity, and position error vector of INS, respectively. The $\left( {{\varvec{\chi } }_{{q}}^{\mathrm{{b}}},{\varvec{\varepsilon }} _{{q}}^{\mathrm{{b}}}} \right)$ means the bias of accelerometers and drift vectors deduced from the gyroscope, ${\Delta } q$ is the sample time, ${\varvec{\omega }}_q \sim \mathrm{{N}}\left( {{\varvec{O}},{{\varvec{Q}}}_q} \right)$ is the system noise. (${f_{Uq}^{\mathrm n}}$,${f_{Eq}^{\mathrm n}}$,${f_{Nq}^{\mathrm n}}$) is the acceleration in up, east and north directions. ${\varvec{C}}_{\mathrm b}^{\mathrm n}$ is rotation matrix, and the $\theta$, $\gamma$, $\psi$ denote rotation angle respectively.

The observation equation of the FIR filter is as follows:

$$\begin{aligned} \underbrace{\left[ \begin{array}{c} x_q^{(\textrm{I})}-x_q^{(\textrm{U})} \\ y_q^{(\textrm{I})}-y_q^{(\textrm{U})} \\ z_q^{(\textrm{I})}-z_q^{(\textrm{U})} \end{array}\right] }_{\varvec{Z}_q}=\underbrace{\left[ \begin{array}{lll} \varvec{O}_{3 \times 6}&\varvec{I}_{3 \times 3}&\varvec{O}_{3 \times 6} \end{array}\right] }_{\varvec{H}} \varvec{x}_{q \mid q-1}+\varvec{v}_{q-1} \end{aligned}$$

(4)

where $\left( {x_q^{\left( \mathrm I \right) }, y_q^{\left( \mathrm I \right) }, z_q^{\left( \mathrm I \right) } } \right)$ is the position in east, north, and up of ${\varvec{P}}_{{o}}^{\mathrm{{I}}}$ , respectively. $\left( {x_q^{\left( \mathrm U \right) }, y_q^{\left( \mathrm U \right) }, z_q^{\left( \mathrm U \right) } } \right)$ is the position of the ${\varvec{P}}_{{o}}^{\mathrm{{U}}}$ in east, north, and up. ${\varvec{\upsilon }}_{q} \sim \mathrm{{N}}\left( {{\varvec{O}},{{\varvec{B}}}_q} \right)$ is the measurement noise.

FIR filter

Based on the system described in Eqs. (1)–(4), when the time index $q \ge N-1$ (the N means the FIR filter’s horizon length), the FIR filter compute two parameters: $m = q - N + 1$ and $s = m + M - 1$ (the M is the dimension of ${\varvec{x}}_q$) from the time index $m + M$ to q and complete one-step estimation by using the Eq. (5).

$$\begin{aligned} {{\varvec{{\tilde{x}}}}_{p|p-1}} = {{\varvec{F}}_{p-1}}{{\varvec{{\tilde{x}}}}_{p-1}} \end{aligned}$$

(5)

where p is the time index in the FIR filter’s horizon. Then, the Generalized Noise Power Gain (GNPG) is computed by using the Eq. (6).

$$\begin{aligned} {{\varvec{G}}_{p}} = {\left[ {{\varvec{H}}^{\mathrm T}{{\varvec{H}}} + {{\left( {{{\varvec{F}}_{p-1}}{{\varvec{G}}_{p-1}}{\varvec{F}}_{p-1}^{\mathrm T}} \right) }^{ - 1}}} \right] ^{ - 1}} \end{aligned}$$

(6)

Thus, we can compute the bias correction gain by using the Eq. (7).

$$\begin{aligned} {{\varvec{K}}_{p}} = {{\varvec{G}}_p}{\varvec{H}}^{\mathrm T} \end{aligned}$$

(7)

Finally, the ${{\varvec{{\hat{x}}}}_q} = {{\varvec{{\tilde{x}}}}_q}$, and the ${{\varvec{{\tilde{x}}}}_q}$ is computed via the Eq. (8).

$$\begin{aligned} {{\varvec{{\tilde{x}}}}_{p}} = {{\varvec{{\tilde{x}}}}_{p|p-1}} + {{\varvec{K}}_{p}}\left[ {{\varvec{Z}}_{p}} - {\varvec{H}}{\varvec{{\tilde{x}}}}_{p|p-1} \right] \end{aligned}$$

(8)

It should be noted that the Eqs. (5–8) work when $q \ge N-1$, which means that the FIR filter has a dead zone. In this work, we employ the traditional Kalman Filter (KF) in the dead zone of the FIR filter. Moreover, the KF employs the error matrix ${\varvec{P}}_{q}$, but the FIR filter do not need this error matrix. One can see that the FIR filer is able to estimate the ${{\varvec{{\hat{x}}}}_q}$ without the accurate description of $\varvec{B}$ and $\varvec{Q}$, which indicates its robustness. The FIR filter for model (1)(4) is shown in Algorithm 1.

ELM

Base on the model (1)(4) and the scheme proposed in Section , the ELM method used in this work is introduced in this subsection, which was introduced briefly in Zou et al. (2017). With the $q-1$ arbitrary distinct samples, the ELM’s activation function ${\varvec{G}}(x)$ can be computed as Eq. (9).

$$\begin{aligned} \sum \limits _{c = 1}^L {{\beta _{c}}{{\varvec{G}}}\left( {{a_{c}}{\varvec{I}}_j + {b_{c}}} \right) = } {{{o}}_j},j \in \left[ {1,q - 1} \right] \end{aligned}$$

(9)

where ${\varvec{I}}_j = {\left[ {I_1},I_2,...,I_{q-1} \right] ^{\mathrm T}}$, ${y_{c}} \in {{{\mathbb {R}}}}$, L is the additive hidden nodes, ${{a_{c}}}$, ${{b_{c}}}$ are the connection weights for input and hidden layer and output and hidden layer, respectively. When $L \ge q - 1$, the ELM can achieve the $\sum \limits _{c = 1}^{q - 1} {\left\| {{o_j} - {y_j}} \right\| } = 0$ with the following equation:

$$\begin{aligned} \sum \limits _{c = 1}^L {{\beta _{c}}{{\varvec{G}}}\left( {{a_{c}}{\varvec{I}}_j + {b_{c}}} \right) = } {y_j},j \in \left[ {1,q - 1} \right] \end{aligned}$$

(10)

The Eq. (10) can be written as

$$\begin{aligned} \underbrace{\left[ {\begin{array}{*{20}{c}} {h_1^{\mathrm E}\left( {{\varvec{I}}_1} \right) }\\ \vdots \\ {h_{q - 1}^{\mathrm E}\left( {{\varvec{I}}_{q - 1}} \right) } \end{array}} \right] }_{{{\varvec{H}}^{\mathrm E}}}\underbrace{\left[ {\begin{array}{*{20}{c}} {{\beta _1}}\\ \vdots \\ {{\beta _{q - 1}}} \end{array}} \right] }_{{{\varvec{\beta }}^{\mathrm E}}} = \underbrace{\left[ {\begin{array}{*{20}{c}} {{y_1}}\\ \vdots \\ {{y_{q - 1}}} \end{array}} \right] }_{{{\varvec{Y}}^{\mathrm E}}} \end{aligned}$$

(11)

then, its least square solution ${{\varvec{\beta }}^{\mathrm E}}$ can be computed as the following equation.

$$\begin{aligned} {{{\varvec{{\hat{\beta }} }}}^{\mathrm E}} = {{\varvec{H}}^{{\mathrm E} + }}{{\varvec{Y}}^{\mathrm E}} \end{aligned}$$

(12)

where the ${{\varvec{H}}^{\mathrm E + }}$ denotes the Moore–Penrose generalized inverse of matrix ${\varvec{H}}$ (Zou et al., 2017).

Thus,

$$\begin{aligned} \left\| {{{\varvec{H}}^{\mathrm E}}{{{\varvec{{\hat{\beta }} }}}^{\mathrm E}} - {{\varvec{Y}}^{\mathrm E}}} \right\| = \min \left\| {{{\varvec{H}}^{\mathrm E}}{{\varvec{\beta }}^{\mathrm E}} - {{\varvec{Y}}^{\mathrm E}}} \right\| \end{aligned}$$

(13)

Then, we can compute the output function of ELM using Eq. (14)

$$\begin{aligned} {\varvec{f}}\left( {{\varvec{I}}} \right) = \underbrace{\left[ {{\varvec{G}}\left( {{a_1}{{I}}_1 + {b_1}} \right) ,...,{\varvec{G}}\left( {{a_L}{{I}}_L + {b_L}} \right) } \right] }_{{{\textbf {h}}}\left( {{\varvec{I}}} \right) }{{\varvec{\beta }}^{\mathrm E}} \end{aligned}$$

(14)

The predictive model

The model (1)(4) do not consider the outage of the UWB. In order to reduce the influence of the data outage on the filter, the predictive model based on the model (1)(4) will be designed in this subsection. The core of the prediction model is the estimation when the UWB data is unavailable. In this work, we modify the Eq. (4) as the following. In the prediction model, we use one-step prediction ${{\varvec{H}}{{\varvec{x}}_{q|q - 1}}}$ to directly replace the filter’s observation when there exists UWB data outage.

When the UWB data is available,

$$\begin{aligned} {{\varvec{Z}}_q} = \left[ {\begin{array}{*{20}{c}} {x_q^{\left( \mathrm{{I}} \right) } - x_q^{\left( \mathrm{{U}} \right) }}\\ {y_q^{\left( \mathrm{{I}} \right) } - y_q^{\left( \mathrm{{U}} \right) }}\\ {z_q^{\left( \mathrm{{I}} \right) } - z_q^{\left( \mathrm{{U}} \right) }} \end{array}} \right] \end{aligned}$$

(15)

When the UWB data is unavailable,

$$\begin{aligned} {{\varvec{Z}}_q} = {\varvec{H}}{{\varvec{x}}_{q|q - 1}} \end{aligned}$$

(16)

The predictive model/ELM integrated scheme

From the above sections, we can find that both the predictive model and ELM model are able to achieve the seamless navigation. However, the shortcoming of the ELM method is that outliers may exist in the estimated values, leading to the accumulation of the estimation error of the predictive model. In this work, we propose a model fusing the outputs of the predictive model and ELM method.

In this part, the predictive model/ELM integrated design scheme will be investigated, which is used to provide the measurement in the case of the UWB data outage. With the method proposed in Sections and , the FIR filter assisted with the predictive model/ELM integrated for models (1) and (4) can be computed as follows:

Firstly, the FIR filter completes one-step estimation by using the Eq. (17).

$$\begin{aligned} {{\varvec{{\tilde{x}}}}_{p|p-1}} = {{\varvec{F}}_{p-1}}{{\varvec{{\tilde{x}}}}_{p-1}} \end{aligned}$$

(17)

Then, the bias correction gain by using the Eqs. (18) (19).

$$\begin{aligned}{} & {} {{\varvec{G}}_{p}} = {\left[ {{\varvec{H}}^{\mathrm T}{{\varvec{H}}} + {{\left( {{{\varvec{F}}_{p-1}}{{\varvec{G}}_{p-1}}{\varvec{F}}_{p-1}^{\mathrm T}} \right) }^{ - 1}}} \right] ^{ - 1}} \end{aligned}$$

(18)

$$\begin{aligned}{} & {} {{\varvec{K}}_{p}} = {{\varvec{G}}_p}{\varvec{H}}^{\mathrm T} \end{aligned}$$

(19)

When the UWB data is available, the ELM is in the training stage. In this stage, the difference $\mathrm{\delta } {\varvec{P}}_{{o}}$ between the ${\varvec{P}}_{{o}}^{\mathrm U}$ and ${\varvec{P}}_{{o}}^{\mathrm I}$ is applied by the FIR filter for estimating the INS’s position error, and the target quadrotor aircraft’s position is computed by the ${\varvec{P}}_{{o}} = {\varvec{P}}_{{o}}^{\mathrm I} - \mathrm{\delta } {\varvec{P}}_{{o}}$ . Meanwhile, the $\mathrm{\delta } {{\varvec{P}}_o}\left( t \right) ,t = 1,2,...,q - 1$ are selected as the input and the target of the ELM. Here, the ELM is applied to build the mapping between the $\mathrm{\delta } {{\varvec{P}}_o}\left( t \right) ,t = 1,2,...,q - 1$ and ${\varvec{{\hat{x}}}}_{t|t-1,t=1,2,...,q-1}$. Moreover, we compute the measurement vector as the Eq. (20).

$$\begin{aligned} {{\varvec{Z}}_q} = \left[ {\begin{array}{*{20}{c}} {x_q^{\left( \mathrm I \right) } - x_q^{\left( \mathrm U \right) }}\\ {y_q^{\left( \mathrm I \right) } - y_q^{\left( \mathrm U \right) }}\\ {z_q^{\left( \mathrm I \right) } - z_q^{\left( \mathrm U \right) }} \end{array}} \right] \end{aligned}$$

(20)

Under the condition that the UWB data is unavailable, the ELM is in the prediction stage. In this stage, we employ two methods to provide the measurement of the FIR filter. One is the ELM, with the mapping between the $\mathrm{\delta } {{\varvec{P}}_o}\left( t \right) ,t = 1,2,...,q - 1$ and ${\varvec{{\hat{x}}}}_{t|t-1,t=1,2,...,q-1}$, the ELM is able to provide the $\mathrm{\delta } {{\varvec{P}}_{{o}}^{\mathrm E}}\left( q \right)$ by using the ${\varvec{{\hat{x}}}}_{q|q-1}$. The other is we employ the ${\varvec{{\hat{x}}}}_{q|q-1}$ to compute the measurement of the FIR filter $\mathrm{\delta } {{\varvec{P}}_{{o}}^{\mathrm P}}\left( q \right)$ at time index q by using $\mathrm{\delta } {{\varvec{P}}_{{o}}^{\mathrm P}}\left( q \right) = {\varvec{H}}{\varvec{{\hat{x}}}}_{q|q-1}$. Then, the Mahalanobis distance is employed to evaluate the performance of the $\mathrm{\delta } {{\varvec{P}}_{{o}}^{\mathrm E}}\left( q \right)$ by using the Eq. (21). If the $\mathrm{\delta }{{\varvec{P}}_{{o}}^{\mathrm E}}\left( q \right) < door$, which is the preset threshold, we set ${{\varvec{Z}}_q} = \mathrm{\delta } {{\varvec{P}}_{{o}}^{\mathrm E}}\left( q \right)$. If the $\mathrm{\delta }{{\varvec{P}}_{{o}}^{\mathrm E}}\left( q \right) > d$, we set ${{\varvec{Z}}_q} = \mathrm{\delta } {{\varvec{P}}_{{o}}^{\mathrm P}}\left( q \right)$.

$$\begin{aligned} {\varvec{D}}_q^{\mathrm E}\mathrm{{ = }}{\left( {\mathrm{\delta } {{\varvec{P}}_{{o}}^{\mathrm E}}\left( q \right) - {\varvec{H}}{{{\varvec{{\hat{x}}}}}_{q|q - 1}}} \right) ^{\mathrm E}}{\varvec{R}}\left( {\mathrm{\delta } {{\varvec{P}}_{{o}}^{\mathrm E}}\left( q \right) - {\varvec{H}}{{{\varvec{{\hat{x}}}}}_{q|q - 1}}} \right) \end{aligned}$$

(21)

With the ${\varvec{Z}}_q$, the ${{\varvec{{\tilde{x}}}}_q}$ is computed via the Eq. (22).

$$\begin{aligned} {{\varvec{{\tilde{x}}}}_{p}} = {{\varvec{{\tilde{x}}}}_{p|p-1}} + {{\varvec{K}}_{p}}\left[ {{\varvec{Z}}_{p}} - {\varvec{H}}{\varvec{{\tilde{x}}}}_{p|p-1} \right] \end{aligned}$$

(22)

The FIR filter assisted with the predictive model/ELM integrated for model (1)(4) is listed in Algorithm 2.

Fig. 2

The predictive model/ELM integrated scheme when the UWB data is available

Fig. 3

The predictive model/ELM integrated scheme when the UWB data is outage

Fig. 4

The test environment

Experimental verification

In this section, the performance of the raised algorithm will be evaluated by the experiment. The test setting and the structure of the filter will be introduced. Moreover, the performance of the proposed FIR filter assisted with the predictive model/ELM integrated will be assessed (Figs. 2, 3).

The test parameters

In this work, the test was done in No.2 teaching building of University of Jinan, China, which is shown in Fig. 4. And the frame of the testbed is given in Fig. 5. In the test, the six UWB RNs were mounted at the known positions, and one UWB BN was installed on the target quadrotor aircraft. The UWB localization system was used to measure the ${{\varvec{P}}_{{o}}^{\mathrm U}}$ of the target quad-rotor aircraft at the time index q. Meanwhile, the INS localization system, which employs the MPU9250, was used to measure the ${{\varvec{P}}_{{o}}^{\mathrm I}}$ of the target quad-rotor aircraft at the time index q.

Fig. 5

The frame of the testbed

For the filter, we set $\Delta q = 0.02\;\textrm{s}$, ${\varvec{{\hat{x}}}}_0 = {\varvec{O}}_{15 \times 15}$, ${\varvec{{\hat{P}}}}_0 = {\varvec{I}}_{15 \times 15}$, $M= 15$, $N = 16$, $d = 0.2$, ${{\varvec{Q}}} = {\varvec{I}}_{15 \times 15}$, ${{\varvec{R}}} = {\varvec{I}}_{3 \times 3}$. In this work, we simulated three UWB data missing regions with the length of 150, 300, 200 sampling points respectively.

Localization error

This subsection investigates the performances of the proposed method. In order to display the performance, we employ the FIR+predictive mode which is proposed in , and FIR+ELM as the comparison method. For the UWB data missing regions, we set the ${\varvec{Z}}_p = {\varvec{Z}}_{p-1}$ directly. Figure 6 displays the reference paths and the paths measured by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM. The figure obviously shows that when the UWB data is available, all the methods can estimate the target quadrotor aircraft’s position. When the UWB data is unavailable, the results of the FIR+ELM has large error. The localization errors measured by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM in #1, #2, and #3 UWB data missing regions are shown in Figs. 7, 8 and 9, respectively. From the figures, one can easily see that in #1 UWB data missing region, the localization errors measured by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM are quite similar. However, in #2 and #3 UWB data missing regions, there is one big localization error when compared with the FIR filter and the FIR+predictive mode method. The FIR+predictive mode methods have error accumulations. The proposed predictive model/ELM can improve the estimation accuracy of observations when comparing with the methods mentioned above. And one can infer that the result of the proposed method will be close to that of the methods with better performance.

The Cumulative Distribution Function (CDF) of the localization errors measured by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM in UWB data missing regions are shown in Fig. 10. From the figure, the solutions with the proposed FIR+predictive model/ELM method are close to the better solutions of the ELM and predictive model. The position Root Mean Square Error (RMSEs) estimated by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM in the UWB data missing regions are given in Table 1. It shows that the localization errors of the proposed method in three UWB data missing regions are 0.35 m, 2.21 m, and 2.53 m, respectively. Compared with the FIR+ELM method, the localization errors are reduced by −4.25%, 73.77%, and 76.26% , respectively, which show the effectiveness of the proposed method when the outliers appear in the predicted value of FIR+ELM method.

Fig. 6

The reference paths and the paths measured by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM

Fig. 7

The localization errors measured by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM in #1 UWB data missing region: a east direction, b north direction, c up direction

Fig. 8

The localization errors measured by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM in #2 UWB data missing region: a east direction, b north direction, c up direction

Fig. 9

The localization errors measured by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM in #3 UWB data missing region: a east direction, b north direction, c up direction

Fig. 10

The CDF of the localization errors measured by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM in UWB data missing regions: a #1 UWB data missing region, b #2 UWB data missing region, c #3 UWB data missing region

Table 1 Position RMSEs estimated by the FIR+ELM, FIR+predictive mode, and the FIR+predictive model/ELM in the UWB data missing regions

The CDF of the localization errors measured by the KF and FIR+predictive model/ELM are listed in Fig. 11. From this figure, the KF’s localization error is larger than that of the FIR’s solution in the test, which indicates that the proposed FIR filter has the robustness when compared with the KF.

Fig. 11

The CDF of the localization errors measured by the KF and FIR+predictive model/ELM

Conclusion

Based on the UWB-based quad-rotor aircraft localization system, we proposed a FIR filter assisted with the predictive model/ELM integrated. In this method, the FIR filter estimates the quad-rotor aircraft’s position by fusing the UWB’s and INS’s positions. When the UWB data is unavailable, both the ELM and the predictive model are used to provide the measurements, replacing the UWB missing data, for the FIR filter. The ELM estimates these measurement via the mapping between the one-step prediction of the state vector and the measurement built when the UWB data is available. While the predictive model mathematically describes the missing UWB data. Then, both the measurements estimated by the ELM and predictive model are employed to estimate the observation via Mahalanobis distance. The test results show that the FIR filter assisted with the predictive model/ELM integrated has better performances than the FIR filter with ELM when the UWB is unavailable. Compared with the ELM assisted FIR filter, the proposed FIR filter assisted with the predictive model/ELM integrated can reduce the localization error by about 48.59 %, meanwhile, the integrated method can be close to the method with better solution.