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