Low-cost integrated INS/GNSS using adaptive H∞ Cubature Kalman Filter

Abstract

We proposed an adaptive H-infinity Cubature Kalman Filter (AH∞CKF) to improve the navigation accuracy of a highly manoeuvrable unmanned aerial vehicle (UAV). AH∞CKF fuses the Inertial Navigation System (INS) and Global Navigation Satellite System (GNSS) measurements. Traditional state estimation filters like extended Kalman filters (EKF) and cubature Kalman filters (CKF) assume Gaussian noises. However, their performance degrades for non-Gaussian noises and system uncertainties encountered in real-world applications. Thus, designing filters robust to noise and distribution is crucial. AH∞CKF combines H∞CKF design with an added adaptive factor to adjust the state estimation covariance matrix according to measurements by exploiting the square root method to yield more numerically stable results (SrAH∞CKF). We conducted multiple dynamically rich flight tests to validate our claims using a UAV equipped with a commercially well-known GNSS solution. Results show that the SrAH∞CKF state estimation outperforms EKF and CKF methods on average by 90% in various statistical measures.

1. Introduction

Using unmanned aerial vehicles (UAVs) for non-military purposes has become a reality. For this purpose, autonomous systems have been designed to perform photogrammetry, aerial surveys (Beard et al., 2005), surveillance (Statheros et al., 2008), environmental monitoring, mapping (Titterton et al., 2004) and emergency management tasks autonomously (Sun and Tang, 2012). The Inertial Navigation System (INS) can evaluate the UAV's position, velocity and attitude. Still, a slight error in sensor biases can cause a significant accumulated error over prolonged operations. To circumnavigate this state divergence problem, one can use multiple sources of measurement (Allerton and Jia, 2005) to fuse the sensor recordings and form integrated navigation (Alandry et al., 2011) that estimates UAV's states better than each individual sensor (Dissanayake et al., 2001). In recent years, multi-sensor data integration has received much more attention, with many theories and techniques proposed to target its challenges. One of the best integration techniques developed in this field is the Global Navigation Satellite System (GNSS) integration with the INS (Groves, 2013). An integrated INS/GNSS uses data integration algorithms, commonly known as filters or estimators (Mwenegoha et al., 2021). Considering the level of information needed to be integrated, there are three primary forms of integration structures: loosely coupled system (Xu et al., 2018), tightly coupled system (Miller et al., 2008) and ultra-tightly coupled (Qin et al., 2019). In the system shown in Figure 1 top, two separate filters function independently. The first filter in the GNSS block computes the position and velocity vectors using raw GNSS data.

Figure 1. Integration schemes of the INS/GNSS. Loosely coupled is shown at the top and tightly coupled at the bottom

In contrast, the second filter uses an inertial measurement unit (IMU) to estimate position, velocity and attitude, while the GNSS block compensates for position and velocity errors of the INS. Figure 1 bottom shows that INS navigation data and raw GNSS measurements are optimally processed in a specific central filter in the latter system. The most popular estimator for the INS/GNSS integration is the Kalman filter (KF), which is widely used in many designs. Although much better performing factor graph optimisation (FGO) methods are available, their computationally extensive nature restricts their usage in embedded systems (Taghizadeh et al., 2022).

The KF and the extended Kalman filter (EKF) have long been the primary tools of state estimation (Nezhadshahbodaghi et al., 2021). KF performance can be seriously degraded by a non-Gaussian distribution of the measured noise (Farrell and Barth, 1999). Considering this problem, a wide variety of improved filtering algorithms, including the unscented Kalman filter (UKF), particle filter (Gustafsson, 2010), robust filter (Simon, 2006), adaptive Kalman filter (Ding et al., 2007), adaptive high-degree cubature Kalman filter (Cui et al., 2015) and classified weighted adaptive filtering Kang et al., 2019) have been proposed. UKF and particle filters can eliminate nonlinearities. However, both become unstable for high-dimensional systems (Sun and Tang, 2012). The CKF is based on a spherical–radial cubature law that facilitates the computation of the multivariable numerical integrals emerging in nonlinear Bayesian filtering schemes (Arasaratnam et al. 2010). Recently, a nonlinear filter, so-called high-dimensional CKF, has been proposed for state estimation (Jia and Xin, 2015). CKF does not require state linearisation and brings all of the UKF algorithm's features to the table. CKF uses an asymmetric sampling strategy and has fewer sampling points than UKF. Compared to EKF, UKF and particle filters, CKF results in (1) better nonlinear performance, (2) higher numerical accuracy, (3) better stability and (4) more convenient implementation (Sun and Tang, 2012). Drifting sensor readings, environmental inconsistencies, and rapid UAV manoeuvres occurring in conjunction with the unknown previous noises that are unknown in many real-world applications required for CKF calculations are primary limiting factors of using CKF in every navigation system (Burkhart, 2019).

To overcome these shortcomings of CKF, robust and adaptive filtering algorithms were proposed. These integrated algorithms usually are a jack of all trades and provide the required adaptability (Hamza et al., 2014) and robustness (Zhenbing et al., 2018). In designing filters, it is possible to mitigate the impact of dynamical model deviations or measurement noises by using adaptively robust filtering schemes (Yang et al., 2013). H-infinity (H∞) is one of the best robust filters designed to handle bounded uncertainties in noisy measurements (Yang et al., 2009). The H∞ filter design can be exploited to expand on the applicability of the navigation algorithms without a need for extra statistical information (Yang et al., 2009; Guo et al., 2022). However, the H∞ filter can improve performance only if the uncertainty values are bounded (Yang et al., 2009). The robust estimation method can be added to the CKF filter (H∞CKF) to improve its robustness. A classified weighted adaptive filter-based integrated strap-down INS/Bei Dou navigation satellite system (SINS/BDS) was put forward to remove the declining CKF accuracy problem caused by modelling uncertainties (Kang et al., 2019). Pakki et al. used Arasaratnam and Haykin (2010) to achieve a cubature rule-based third-degree spherical–radial filter (2011). Jia and Xin applied the fifth-degree spherical–radial cubature rule to fuse multiple sensors (2015). Wang et al. developed a high-degree robust filter to estimate ballistic missile orientation (2015). Chandra et al. directly embedded the H∞ filter (Yang et al., 2009) in CKF filters to improve CKF in the presence of non-Gaussian noises (2014). The federated derivative cubature Kalman filter (FDCKF) was proposed by combining the traditional Kalman filter with the cubature Kalman filter (He et al., 2020). Ge et al. also suggested an adaptive UKF scheme for time-varying noise covariance matrixes (2019), extending the UKF filter's applicability to more realistic real-world problems. Nourmohammadi and Keighobadi developed a novel INS/GNSS navigation system using a direct and indirect decentralised integration scheme based on the CKF structure (2018). Chang et al. presented a new fuzzy strong tracking cubature Kalman filter (FSTCKF) algorithm for data fusion (2021). Li et al. proposed Huber's M-estimation-based robust CKF (RCKF) filter for synchronous machines by combining Huber's M-estimation theory and classical CKF (2019). Gao et al. incorporated the maximum likelihood principle (MLP) and moving horizontal estimation (MHE) to develop a new adaptive UKF to remove its constraints (2018). Hu et al. proposed the orthogonality-based robust UKF filter (2019) to withstand the error disturbance in the navigation system measurements. Cao et al. used the Laplace distribution and the Huber-based robust design method to update the measurement covariance noise matrix to counteract different types of model inconsistencies and noisy measurements (2018). AH∞CKF is a robust adaptive design. Thus, it can operate outstandingly by adaptively satisfying the H∞ filter's boundedness requirement even when modelling uncertainties exist. To improve the capabilities of the AH∞CKF, we integrated the square root technique into the AH∞CKF design to form the SrAH∞CKF filter. Filters implemented by the square root method tend to have better numerical stability, avoiding round-off errors in the process noise (Zhang et al., 2016).

As its name suggests, the SrAH∞CKF consists of an adaptive factor, a compound H-infinity CKF filter with an added square root. To implement this design, one can start with an H∞CKF design by introducing a cost function given in the methodology section. One of the main parameters in H∞CKF filter calculations is the state estimation covariance matrix. In the SrAH∞CKF design, the state estimation covariance matrix is adaptively adjusted via an adaptive factor that exploits square root calculations to yield more numerically stable results and circumnavigate the time-varying behaviour of the characteristics of non-Gaussian noises. Extracting the most probable measurements from the H∞CKF cost function is accountable for the robustness of the filter.

The main contribution of this paper to the existing literature is to integrate the square root method into an adaptive design and implement the suggested filter for a highly manoeuvrable UAV, which is discussed in the results and testing section. Multiple flight tests are carried out to validate the superiority of the SrAH∞CKF design compared to the traditional methods, such as EKF and CKF, in the last section of the paper. Our testing method's reference ground truth measurements are recorded via the commercially well-known high-performance s3DM-GX1 and the Neo6 GNSS receiver sensors (available at: microstrain.com/inertial/3DM-GX1.). Results of statistical measures such as root mean square error (RMSE), mean absolute error (MAE), and standard deviation (StD.) show that the SrAH∞CKF design outperforms the EKF and CKF methods in estimating the velocity and position vectors especially. This outperforming of the SrAH∞CKF can be attributed to the fact that there are many uncertainties and non-Gaussian noises in real-world situations that are not accounted for in the traditional methods, such as EKF and CKF, in contradiction to the robust and adaptive design of the SrAH∞CKF.

2. Methodology

Most studies based on the CKF or H∞ filters do not perform well in facing non-Gaussian noises encountered in real-world scenarios (Arasaratnam and Haykin 2010). Still, our filter performs more robustly since the core idea is to minimise estimation error. H∞ design of the SrAH∞CKF filter allows for controlling uncertainties of a system with many states. We conducted various flight tests to validate the filter's superiority. Primary advantages of our proposed method include (1) adding an adaptive parameter to adjust the noise covariance matrix, (2) using H∞ design to dynamically calculate error distribution, hence improving filter convergence, and (3) formulating the square root method for AH∞CKF. These methods are all well studied individually; however, few studies integrate this wide variety of algorithms to attain better performance in real-world applications.

2.1 Theoretical implementation of SrAH∞CKF

A dynamical system can be described in discrete state-space form as given by Equations (1) and (2) (Simon, 2006):

(1)\begin{gather}{x_k} = f({{x_{k - 1}},{u_k}} )+ {w_k}\end{gather}

(2)\begin{gather}{z_k} = h({{x_k},{u_k}} )+ {v_k}\end{gather}

where ${x_k}$, ${z_k}$ and ${u_k}$ are the state vector, the measurement vector and the system input at step k; f and h, in a general case, are nonlinear functions describing the system and measurement dynamics; and ${w_k}$ and ${v_k}$ are system and measurement noises, respectively.

CKF is a newly developed Bayesian filter first introduced by Arasaratnam and Haykin (2010) to tackle the long-known high-dimensional nonlinear filter problem. This design used nonlinear techniques assuming a Gaussian distribution. The main Bayesian filter design steps are given by Equations (3)–(13) (Särkkä, 2013):

(3)\begin{equation}p({x_k}|{z_{1:k - 1}}) = N({{x_k}|{x_{k|k - 1}},{P_{k|k - 1}}} )\end{equation}

where probability density function $\; p({x_k}|{z_{1:k - 1}})$ is assumed Gaussian ($N$) with mean ${x_k}|{x_{k|k - 1}}$, and covariance matrix $\; {P_{k|k - 1}}$.

(4)\begin{equation}p({{z_k}\textrm{|}{z_{1:k - 1}}} )= N({{z_k}|{z_{k|k - 1}},P_{k|k - 1}^{zz}} )\end{equation}

In Equation (4) ${z_k}|{z_{k|k - 1}}$ and $P_{k|k - 1}^{zz}$ represent the mean and variance. The joint one-step predicted probability density function (PDF) of the state and measurement $p({x_k},{z_k}|{z_{1:k - 1}})$ is also Gaussian, i.e.:

(5)\begin{equation}p({{x_k},{z_k}\textrm{|}{z_{1:k - 1}}} )= N\left( {\left[ {\begin{array}{@{}c@{}} {{x_k}}\\ {{z_k}} \end{array}} \right]|\left[ {\begin{array}{@{}c@{}} {{{\hat{x}}_{k|k - 1}}}\\ {{{\hat{z}}_{k|k - 1}}} \end{array}} \right],\left[ {\begin{array}{@{}cc@{}} {{P_{k|k - 1}}}& {P_{k|k - 1}^{xz}}\\ {{{({P_{k|k - 1}^{xz}} )}^T}}& {P_{k|k - 1}^{zz}} \end{array}} \right]} \right)\end{equation}

where $P_{k|k - 1}^{xz}$ is the cross-covariance of ${x_k}$ and ${z_k}$ based on Equations (4) and (5), while ${\hat{x}_{k|k - 1}}$ and ${\hat{z}_{k|k - 1}}$ denotes the estimated values of the state and measurement vectors. Also, by the Bayesian theorem, the current PDF of ${x_k}\;$ is considered Gaussian, for example:

(6)\begin{equation}p({{x_k}\textrm{|}{z_{1:k}}} )= \frac{{p({{x_k},{z_k}\textrm{|}{z_{1:k - 1}}} )}}{{p({{z_k}\textrm{|}{z_{1:k - 1}}} )}} = N({{x_k}|{{\hat{x}}_{k|k}},{P_{k|k}}} )\end{equation}

${x_{k|k}}$ and ${P_{k|k}}$ values are derived by Arasaratnam and Haykin (2010). For a system with zero input (${u_k} = 0$), other parameters also can be calculated as follows (Särkkä, 2013):

(7)\begin{align}{x_{k|k - 1}} & = E[{f({{x_{k - 1}}} )\textrm{|}{z_{1:k - 1}}} ]= \smallint f({{x_{k - 1}}} )N({{x_{k - 1}}|{x_{k - 1|k - 1}},{P_{k - 1|k - 1}}} )d{x_{k - 1}}\end{align}

(8)\begin{align}{P_{k|k - 1}} & = E[{({{x_k} - {{\hat{x}}_{k|k - 1}}} ){{({{x_k} - {{\hat{x}}_{k|k - 1}}} )}^T}\textrm{|}{z_{1:k - 1}}} ]\nonumber\\ & = \smallint ({f({{x_{k - 1}}} )- {{\hat{x}}_{k|k - 1}}} )\; {({f({{x_{k - 1}}} )- {{\hat{x}}_{k|k - 1}}} )^T}N({{x_{k - 1}}|{{\hat{x}}_{k - 1|k - 1}},{P_{k - 1|k - 1}}} )d{x_{k - 1}} + {Q_{k - 1}} \end{align}

(9)\begin{align}{z_{k|k - 1}} & = E[h({{x_k}} )|{z_{1:k - 1}}] = \smallint h({{x_k}} )N({{x_k}|{x_{k|k - 1}},{P_{k|k - 1}}} )d{x_k}\end{align}

(10)\begin{align}P_{k|k - 1}^{zz} & = E[{({{z_k} - {{\hat{z}}_{k|k - 1}}} ){{({{z_k} - {{\hat{z}}_{k|k - 1}}} )}^T}\textrm{|}{z_{1:k - 1}}} ]\nonumber\\ & = \smallint ({h({{x_k}} )- {{\hat{z}}_{k|k - 1}}} ){({h({{x_k}} )- {{\hat{z}}_{k|k - 1}}} )^T}N({{x_k}|{{\hat{x}}_{k|k - 1}},{P_{k|k - 1}}} )d{x_k} + {R_k} \end{align}

(11)\begin{align}P_{k|k - 1}^{xz} & = E[{({{x_k} - {{\hat{x}}_{k|k - 1}}} ){{({{z_k} - {{\hat{z}}_{k|k - 1}}} )}^T}\textrm{|}{z_{1:k - 1}}} ]\nonumber\\ & = \smallint ({{x_k} - {{\hat{x}}_{k|k - 1}}} ){({h({{x_k}} )- {{\hat{z}}_{k|k - 1}}} )^T}N({{x_k}|{{\hat{x}}_{k|k - 1}},{P_{k|k - 1}}} )d{x_k} \end{align}

where $E[{\cdot} ]$ is the expectation operator, and Q and R are process and measurement noise covariance matrixes. Equations (7)–(11) are the essence of the Bayesian filters that calculate Gaussian weighted integrals, where f and h are arbitrary nonlinear functions. As already anticipated, it is challenging to obtain accurate numerical or closed-form solutions for Equations (7)–(11); hence, approximate solutions are inescapable. The heart of CKF is to use the third-degree cubature rule to approximate the integrals of the for (nonlinear function × Gaussian density) instead of approximating the nonlinear functions. Based on the third-degree cubature rule, 2n cubature points with identical weights are selected to propagate the state and covariance matrixes within each update cycle. The cubature rule approximating the general Gaussian weighted integral is given by Equation (12) (Burkhart, 2019):

(12)\begin{equation}I(f )= \int_{{\mathrm{\mathbb{R}}^n}} {f(x )N({x|\bar{x},P} )dx} = \int_{{\mathrm{\mathbb{R}}^n}} {f({\textrm{S}x + \bar{x}} )N({x|0,I} )dx \approx \sum_{i = 1}^m {\omega _i}f({\textrm{S}{\xi_i} + \bar{x}} )}\end{equation}

where $P = S\; {S^T}$, S can be calculated by the Cholesky or QR decomposition. f is an arbitrary nonlinear function, ${\mathrm{\mathbb{R}}^n}$ the integral domain, $\bar{x}$ the mean value of variable x, and m is the total number of cubature points. $\{{{\mathrm{\xi }_\textrm{i}},{\mathrm{\omega }_\textrm{i}}} \}$ represent the $\textrm{ith}$ cubature point and its corresponding weight, respectively, given by Equation (13) (Burkhart, 2019).

(13)\begin{equation}{\xi _i} = \sqrt {\frac{m}{2}} {[1 ]_{i}},\;{\omega _i} = \frac{1}{m},\;i = 1,2, \ldots m = 2n\end{equation}

The symbol ${[1 ]_\textrm{i}}$ denotes the $\textrm{i}th$ column vector of the points set [1].

KF is a beneficial tool for estimating system parameters. The H∞ filter is a particular form of KF that uses an optimal H∞ estimation guaranteeing the least energy estimation error for a disturbance with an arbitrary shape yet bounded (Simon, 2006). The cost function of this nonlinear filter is defined by Equation (14) (Cao et al., 2018):

(14)\begin{equation}{J_\infty } = \frac{{\mathop \sum \nolimits_{k = 0}^{N - 1} ||{{z_k}} - {{{\hat{z}}_k}} ||_{{S_k}}^2}}{{||{{x_0}} - {{{\hat{x}}_0}} ||_{P_0^{ - 1}}^2 + \mathop \sum \nolimits_{k = 0}^{N - 1} ({ {{{||w }_k}} ||_{Q_k^{ - 1}}^2 + ||{ {{v_k}} ||}_{R_k^{ - 1}}^2} )}}\end{equation}

where $|{|. |} |\;$denotes the Huber norm, ${x_0}$ and ${z_0}$ are initial conditions predicted as ${\hat{x}_0}$ and ${\hat{z}_0}$, and N is number of filtering epochs (Cao et al., 2018). The goal is to estimate a ${z_k}$ that minimises ${J_\infty }.$ The behaviour of nature as an opposing force is to find perturbations ${v_k}$ and ${w_{k - 1}}$ as well as the initial parameter ${x_0}$ such that ${J_\infty }$ has the maximum value. The ultimate goal of this opposing force is to maximise the estimation error of $({{z_k} - {{\hat{z}}_k}} )$ by choosing ${v_k}$, ${w_{k - 1}}$ and ${x_0}$. To justify this contradiction $({{x_0} - {{\hat{x}}_0}} )$, ${v_k}$ and ${w_{k - 1}}$ are placed in the denominator of the cost function. Hence, the machinery of nature cannot simply maximise the defined cost function by selecting large values for these parameters. This argument illustrates the fundamental difference between the KF philosophy and the H∞ filter since the KF is considered indifferent to nature. It is difficult to obtain statistical noise properties in real-world applications in a general case, leading to filter divergence; thus, the robust filter is an excellent solution to improve accuracy (Wang et al., 2015). If any of the Gaussian integral components ($f$) is integrated with the nonlinear H∞ filter using the following cubature rule, this filter is called the H∞ cubature Kalman filter (H∞CKF). Using Equations (12)–(14), the recursive H∞CKF can be achieved by going through the following eight steps (Wang et al., 2015):

• Step1: Covariance factorisation and cubature points evaluation.

• Step2: Evaluation of cubature points propagated through the process model.

• Step3: Predicted state and relevant covariance error.

• Step4: Covariance factorisation and cubature points evaluation calculations.

• Step5: Evaluation of cubature points propagated through the observation model stage.

• Step6: Predicted measurement estimation evaluation.

• Step7: Kalman gain (${K_k}$) and covariance estimation is calculated by Equations (15)–(17). Capitalised notation denotes variables evaluated at $2n$ cubature points:

  (15)\begin{gather}P_{k|k - 1}^{zz} = \frac{1}{{2n}}\sum_{i = 1}^{2n} (Z_{k|k-1})_i(Z_{k|k-1})_i^T - {\hat{z}_{k|k - 1}}\hat{z}_{k|k - 1}^T + {R_k}\end{gather}
  (16)\begin{gather}P_{k|k - 1}^{xz} = \frac{1}{{2n}}\sum_{i = 1}^{2n} (X_{k|k-1})_i(Z_{k|k - 1})_i^T - {\hat{x}_{k|k - 1}}\hat{z}_{k|k - 1}^T\end{gather}
  (17)\begin{gather}{K_k} = \frac{{P_{k|k - 1}^{xz}}}{{P_{k|k - 1}^{zz}}}\end{gather}

• Step8. Updated state and relevant covariance error estimation as Equations (18)–(20) (Wang et al., 2015):

  (18)\begin{gather}{\hat{x}_{k|k}} = {\hat{x}_{k|k - 1}} + {K_k}({{z_k} - {{\hat{z}}_{k|k - 1}}} )\end{gather}
  (19)\begin{gather}{P_{k|k}} = {P_{k|k - 1}} - [{P_{k|k - 1}^{xz}\; \; \; \; \; {P_{k|k - 1}}} ]\; \cdot R_{e.k}^{^{\prime} - 1}\left[ {\begin{array}{@{}cc@{}} {{{(P_{k|k - 1}^{xz})}^T}}\\ {P_{k|k - 1}^T} \end{array}} \right]\end{gather}
  where
  (20)\begin{gather} R_{e.k}^{\prime} = \left[{\begin{array}{@{}cc@{}} {P_{k|k - 1}^{zz}}& {{{({P_{k|k - 1}^{xz}} )}^T}}\\ {P_{k|k - 1}^{xz}}& {{P_{k|k - 1}} - {\gamma^2}I} \end{array}} \right]\end{gather}
  (21)\begin{gather}{\gamma ^2} = {\alpha _\gamma }max\{ eig{[P_{k|k - 1}^{ - 1} + P_{k|k - 1}^{ - 1}P_{k|k - 1}^{xz}R_k^{ - 1}{({P_{k|k - 1}^{xz}} )^T}P_{k|k - 1}^{ - 1}]^{ - 1}}\; \} \end{gather}

${\mathrm{\alpha }_\mathrm{\gamma }} > 1$ is a scalar and $max\{{eig{{[A ]}^{ - 1}}} \}$ denotes the maximum eigenvalue of the matrix ${[A ]^{ - 1}}$.

Our approach for integrating an adaptive factor into the square root method for the H∞CKF is given through Equations (22)–(30). The H∞CKF algorithm helps to eliminate the effects of abnormal system measurement noises and disturbances. The adaptive factor allows the CKF algorithm to adapt to the measurement noise covariance. It should be noted that modelling errors induce inaccuracies in the statistical properties of the system's measured noise. In the adaptive approach, once the system undergoes a sizeable dynamical disturbance, the AH∞CKF algorithm tunes itself adaptively and the adaptive factor ${\theta _k}$ modifying the ${P_{k|k - 1}}$ matrix is given by Equation (22):

(22)\begin{equation}{\hat{P}_{k|k - 1}} = \theta _k^{ - 1}{P_{k|k - 1}}\end{equation}

Theorem 1: If ${\hat{P}^{zz}}_{k|k - 1}$ is the post-measurement residual covariance matrix, ${\tilde{P}^{zz}}_{k|k - 1}$ is the theoretical residual covariance matrix obtained by the adaptive factor, and $P_{k|k - 1}^{zz}$ is the residual covariance matrix obtained by the covariance propagation law, then:

(23)\begin{equation}{\tilde{P}^{zz}}_{k|k - 1} = {\hat{P}^{zz}}_{k|k - 1}\end{equation}

Thus, the optimal adaptive factor can be calculated as Equation (24):

(24)\begin{equation}{\theta _k} = \frac{{tr({P_{k|k - 1}^{zz} - {R_k}} )}}{{tr({{{\hat{P}}^{zz}}_{k|k - 1} - {R_k}} )}}\end{equation}

Proof: Theorem proving is provided in Appendix A1.

AH∞CKF is modified by the square root method to yield a more accurate and numerically stable INS/GNSS integration (Kang et al., 2019). The main advantages of square root AH∞CKF include fast nonlinear estimation and non-Gaussian noise removal (Kang et al., 2019). The square root AH∞CKF algorithm steps are elaborated in the following steps:

• Step1: Adjusting initial conditions (Arasaratnam and Haykin 2010).

  (25)\begin{equation}{\hat{x}_0} = E({{x_0}} ),{P_0} = E{[{({{x_0} - {{\hat{x}}_0}} )({{x_0} - {{\hat{x}}_0}} )} ]^T}\end{equation}

• Step2: Using equations given by Arasaratnam and Haykin (2010), the state vector and covariance matrix are predicted.

• Step3: The square root factor estimation of the predicted error covariance: The decomposition of the state prediction covariance matrix is modified by the adaptive factor according to Equations (26) and (27):

  (26)\begin{gather}\sqrt {\theta _k^{ - 1}} {S_{k|k - 1}} = Tria[{({{P_{k|k - 1}}} )} ]\end{gather}
  (27)\begin{gather}{({{X_{k|k - 1}}} )_i} = \sqrt {\theta _k^{ - 1}} {S_{k|k - 1}}{\xi _i} + {\hat{x}_{k|k - 1}}\end{gather}

In Equations (26) and (27), $Tria\; (. )$ represents a triangular operation, ${({{X_{k|k - 1}}} )_i}$ denotes system state evaluated at the cubature point i, and ${\hat{x}_{k|k - 1}}$ represents post-measurement state estimation. ${\textrm{B} = Tria(\textrm{A})}$ suggests that matrix B is calculated by applying the triangular process to the matrix ${A^T}$, which is derived from the QR decomposition (Kang et al., 2019).

• Step4: Estimation of the square root of the residual covariance matrix $P_{k|k - 1}^{zz}$, denoted as $S_{k|k - 1}^{zz}$, using the adaptive factor governed by Equation (28):

  (28)\begin{equation}S_{k|k - 1}^{zz} = Tria\left[ {\left( {\frac{1}{{2n}}\sum_{i = 1}^{2n} {{({Z_{k|k - 1}})}_i}(Z_{k|k - 1})_i^T - {{\hat{z}}_{k|k - 1}}\hat{z}_{k|k - 1}^T + {R_k}} \right)} \right]\end{equation}

• Step5: Estimating the cross-covariance matrix using Equation (16).

• Step6: Calculation of the adaptive factor using Equation (24).

• Step7: Prediction of Kalman gain (${K_k}$) using Equation (29).

  (29)\begin{equation}{K_k} = ({P^{xz}}_{k|k - 1}/{S_{k|k - 1}^{zz}}^T)/\; S_{k|k - 1}^{zz}\end{equation}

• Step8: Finally, the adaptive factor's measurement updating is performed according to Equation (18), and the square root estimation of relevant error covariance is evaluated by Equation (30):

  (30)\begin{equation}{S_{k|k}} = Tria\left[ {\left( {{P_{k|k - 1}} - [{P_{k|k - 1}^{xz}\; \; \cdot \; \; {P_{k|k - 1}}} ]\cdot R_{e.k}^{^{\prime} - 1}\left[{\begin{array}{@{}c@{}} {{{(P_{k|k - 1}^{xz})}^T}\; }\\ {P_{k|k - 1}^T} \end{array}} \right]} \right)} \right]\end{equation}

As the final step, $P_{k|k}$ is calculated from $S_{k|k}$. and the algorithm loops back to step 2 to carry out the next round of calculations.

2.2 Loosely coupled INS/GNSS navigation system using square root AH∞CKF

Possessing accurate position and velocity is vital in carrying out UAV control and navigation. Figure 2 illustrates a typical navigation scheme used by many UAVs.

Figure 2. Typical navigation and control block diagram of the UAV. The figure shows that the mission control block transforms mission specifications into trajectorial commands, and flight control transforms generated trajectory history into the actuator space. It is crucial to acquire accurate measurements to adopt valid control commands to ensure stable UAV states

INS and GNSS share complementary specifications that can be integrated to eliminate measurement defects of each other. However, a precisely engineered integration scheme is required to achieve acceptable results in a low-cost INS. The SrAH∞CKF algorithm combines multiple ideas to be capable of carrying out acceptable estimates in the proposed integration scheme while using low-cost IMUs. The INS algorithm includes various devices and their frame transformations. Navigation measurements are expressed in the navigation frame, while inertial sensors measure angular velocity and acceleration in the UAV's body frame. The frames used in the paper are i-frame (inertial device), e-frame (ground device), n-frame (navigation device) and b-frame (body device), depicted in Figure 3.

Figure 3. Different frames are required to implement navigation systems. The e, i, n (north-east-down) and b subscripts denote earth fixed, inertial, navigation, and body coordinate systems accordingly

In classic INS/GNSS algorithms, state estimation is performed by the primary filter. Any divergent state estimation propagation may lead to a failing position/velocity updating algorithm. Therefore, an approximate cascade integration method avoids a chain of failures leading to a vicious cycle. The angle estimation accuracy can be improved by integrating angle data from the gravity vector adaptation between the body and the reference frames. An angle filter (rotation) accordingly updates the position/velocity filter based on the cascade connection. Figure 4 illustrates the mechanism between the orientation and position/velocity filters. Initially, the orientation filter state is estimated, and afterward, the position/velocity vectors are updated in a secondary filter. Since gravitational acceleration is indistinguishable from motion acceleration, it is essential to remove gravitational acceleration from the acceleration measured by accelerometers to estimate roll and pitch angles correctly. The non-gravitational acceleration block subtracts motion acceleration from measured acceleration by utilising the vehicle's velocity data so that the orientation filter can evaluate roll and pitch angles correctly.

Figure 4. Integrated INS/GNSS system overview and SrAH∞CKF: pseudo-code. The heading angle is evaluated from GNSS true course

Several formulations can describe a vehicle's orientation, such as the direction cosine matrix (DCM), Euler, and quaternion angles. The advantage of a quaternion is that once the pitch angle closes to 90°, it has no singularity while having a higher computational efficiency than Euler angles. The quaternion is a four-dimensional vector subjected to normalisation. Quaternion formulation of a rotating body can be found in many references, such as Alandry et al. (2011). The error state vector $\delta {x_o}$ and the ${z_o}$ the measurement model for the angle filter is as follows (Titterton et al., 2004):

(31)\begin{equation}\delta {x_o} = {[{\delta q\; \; \; \; \; \delta {b_\omega }\; \; \; \delta {b_f}} ]^T},\; \; {z_o} = \left[ {\begin{array}{@{}c@{}} {{\phi^{\textrm{acc}}} - {\phi^{\textrm{INS}}}}\\ {{\theta^{\textrm{acc}}} - {\theta^{\textrm{INS}}}}\\ {{\psi^{\textrm{GPS}}} - {\psi^{\textrm{INS}}}} \end{array}} \right]\end{equation}

where ${\phi ^{\textrm{acc}}}$ and ${\theta ^{\textrm{acc}}}$ are the roll and pitch angles calculated by the gravity adaptation, respectively, and ${\psi ^{\textrm{GPS}}}$ is the GNSS-based heading angle. $\delta {b_\omega }$ and $\delta {b_f}$ are gyroscope and accelerometer biases modelled by a first-order Gauss-Markov process, given by Equation (32). The o subscript denotes orientation filter states:

(32)\begin{equation}\dot{x}(t )={-} \beta x(t )+ \sqrt {2{\sigma ^2}\beta } w(t )\end{equation}

where $\sigma$ is the standard deviation, $\beta$ time constant of the autocorrelation function of $x(t )$, and $w(t )$ is the zero-mean Gaussian noise. Error state vector $\delta {x_p}$ and measurement vector ${z_p}$ for the position/velocity filter are as follows:

(33)\begin{equation}\delta {x_p} = {[{\delta {\lambda}\; \; \; \delta {l}\; \; \; \delta {h}\; \; \delta V_N\; \; \delta V_E\; \; \delta V_D} ]^T},\; \; {z_p} = \left[ {\begin{array}{@{}c@{}} {{r_{\textrm{GNSS}}} - {r_{\textrm{INS}}}}\\ {{v_{\textrm{GNSS}}} - {v_{\textrm{INS}}}} \end{array}} \right]\end{equation}

$\delta {V_N}$,$\delta {V_E}$, $\delta {V_D}$, $\delta \lambda$, $\delta l$, $\delta h$ denote the difference between INS and GNSS estimations of the north, east, and down velocities and longitude, latitude, and height, accordingly, while r and v represent position and velocity vectors, accordingly. GNSS position and velocity measurements are integrated with INS at 10 Hz in outdoor environments where GNSS signals are available.

3. Flight test results and discussion

A flight data set from the Amirkabir University of Technology Development Centre evaluates the proposed navigation system's performance. This dataset contains 1200 s of flight information. An open-source autopilot (Hattenberger et al., 2014) controls and records the UAV's flight. Such a path is depicted in Figure 5.

Figure 5. Flight test ground station view. Top view of the path taken by UAV during the flight test recorded by the ground station. The data set is gathered by DM-GX1 sensor, MEMS IMU and GNSS Ublox Neo6 receiver extracts special forces, angular rotation rate, and integrated navigation system measurement data with a sampling rate of 50 and 10 Hz

Figure 6 illustrates the 3DM-GX1 and the Neo6 GNSS receiver. The reference data is assumed to be the output of the commercially well-known 3DM-GX1 sensor, and the proposed filtering algorithm is fed by typical low-cost INS/GNSS. To evaluate the EKF, CKF and SrAH∞CKF performance, we compared statistical measures of each algorithm output with respect to the gathered reference data.

Figure 6. Sensors and equipment for managing the flight and recording data include 3DM-GX1 IMU, Ublox Neo6 GNSS receiver, and Datalogger

Once the vehicle is switched on, it operates for 45 s in the initial state alignment mode, where the heading, roll and pitch angles are initialised using GNSS and accelerometer sensors. The key performance characteristics of the 3DM-GX1 IMU and the Ublox Neo6 GNSS receiver are given in Tables 1 and 2, respectively.

Table 1. Frequency domain specification of the 3DM-GX1 IMU

Table 2. Specifications of the Ublox Neo6 GNSS receiver

The DM-GX1 IMU is used to measure attitude, and the Neo6 GNSS receiver is used to measure the reference position/ velocity vectors. 2D projection of the reference flight path is illustrated in Figure 7. As seen, the proposed algorithm is assessed from P_s to P_f. Flight test includes ascent, rotation, cruise and descent manoeuvres which helps with observability and enriches the collected data. The results are compared to two other methods for a more confident evaluation of the SrAH∞CKF algorithm, including a typical INS/GNSS system integrated via an EKF or a CKF estimator. Figure 7 illustrates the diagrams comparing longitudinal and latitudinal estimations using the SrAH∞CKF, EKF, and CKF relative to the reference data. According to this figure, CKF and EKF latitude and longitude are distant from the reference orientation; however, the SrAH∞CKF method estimates the longitudinal and latitudinal orientation more accurately. Figures 8–10 illustrate results from estimating velocities in the north (v_n), east (v_e) and down (v_d).

Figure 7. Comparison of SrAH∞CKF (blue), EKF (green), and CKF (black) positions to the reference data (red). From starting position (P_s) to the final destination (P_f), the figure shows that the SrAH∞CKF filter exhibits better overall performance than other traditional filters

Figure 8. Comparison of SrAH∞CKF (blue), EKF (green), and CKF (black) velocities (v_n) to the reference (red). Acquired velocities in the northern direction show the algorithm's accuracy. We divided velocity data into 10-second intervals to depict reference data variations. As can be seen, EKF and CKF estimates are alike. However, SrAH∞CKF follows the reference curve better (H∞ and adaptive design). This following characteristic is due to the faster filter convergence in sharp changes resulting in no following lag (square root design)

Figure 9. Comparison of SrAH∞CKF (blue), EKF (green), and CKF (black) velocities (v_e) to the reference (red). As expected, in the east direction, SrAH∞CKF performs better. Evaluating filters by empirical data rather than a computer-generated dataset allows the SrAH∞CKF filter to show its superiority more straightforwardly

Figure 10. Comparison of SrAH∞CKF (blue), EKF (green), and CKF (black) velocities (v_d) to the reference (red). Acquired velocities in the northern and vertical orientations show the algorithm's accuracy during the flight. Fluctuating vertical velocity results from distinct gravity models and vertical channel instability. Also, by referring to the zoomed portions of the v_d, it is clear that the SrAH∞CKF output is more numerically stable and has fewer fluctuations around the reference curve

To compare SrAH∞CKF with other integrated systems quantitatively, the RMSE, MAE and StD. are used as defined by Equations (34)–(36), respectively:

(34)\begin{gather}\textrm{RMSE} = \frac{1}{n}\sqrt {\sum_{i = 1}^n {{({{D_{\textrm{ref}}} - {D_{\textrm{estimate}}}} )}^2}} \end{gather}

(35)\begin{gather}\textrm{MAE} = \frac{1}{n}\sum\limits_{i = 1}^n {|{{D_{\textrm{ref}}}} - {{D_{\textrm{estimate}}}} |} \end{gather}

(36)\begin{gather}StD. = \frac{1}{n}\sqrt {\sum_{i = 1}^n {{({\mu - ({{D_{\textrm{ref}}} - {D_{\textrm{estimate}}}} )} )}^2}} \end{gather}

where n is the number of samples; ${D_{\textrm{ref}}}$ and ${D_{\textrm{estimate}}}$ are reference data and estimated position, velocity, and attitude values, respectively; and $\mu$ is the average error. RMSE, MAE and Std. of all nine navigation parameters are calculated and listed in Table 3, while Table 4 discusses the relative percentage performance improvement of implementing the SrAH∞CKF method rather than the EKF or CKF method.

Table 3. Statistical measures. Improved results from the mean estimation error values for EKF, CKF and SrAH∞CKF were computed with respect to the closed-source algorithm gathered by Ublox Neo6 receiver

Table 4. Relative performance uplift of using SrAH∞CKF instead of CKF or EKF

We also compared the execution time and temporal performance hit of adding each of the robust and adaptive-square root mechanisms to the base CKF filter in Table 5, by running their associated codes on an Intel core i5 machine clocked at 3 · 4 GHz with 16GBs of random access memory (RAM). This table details the feasibility of implementing the suggested SrAH∞CKF during the path taken in Figure 5. The effects of the robust and adaptive-square root mechanism are insignificant compared to the base CKF calculations execution time.

Table 5. Execution time of the proposed SrAH∞CKF compared to the other methods

CKF calculations are faster than EKF due to the introduced cubature points. SrAH∞CKF, at its core, is a CKF filter algorithm utilising a robust (H∞) design procedure and adaptive factor. Thus, it should result in a higher execution time than the traditional sole-CKF filter. However, by implementing the square root method to achieve faster convergence in conjunction with QR decomposition, the difference between execution times of the CKF and SrAH∞CKF can be reduced, as detailed in Table 5.

According to Tables 3–5, spatial results have been improved in most cases, while the execution time of the SrAH∞CKF is not far from reach. The integrated INS/GNSS navigation errors of the square root AH∞CKF system are smaller than CKF and EKF, indicating the privilege of square root AH∞CK. EKF and CKF methods assume Gaussian noises. However, this assumption is not satisfied in many real-world applications, such as navigation scenarios. The encountered noises can exhibit time-varying behaviour resulting in degraded performance.

On the other hand, the SrAH∞CKF design adaptively adjusts the noise covariance while keeping the estimation error to its minimum due to the H∞ design. Considering the significant improvement of roll and pitch angles, the results suggest that the proposed algorithm is more robust in encountering accelerated manoeuvres. Also, the substantial improvement of the heading angle dramatically contributes to the UAV's navigation task.

4. Conclusion

We sought to improve the accuracy of the well-known integrated INS/GNSS systems by introducing the SrAH∞CKF filtering scheme that improves upon the newly developed nonlinear filtering CKF algorithm via (1) forming a robust H∞ estimator based on a spherical–radial cubature rule that solved the effect of anomalous measurement errors and uncertainties in modelling; (2) an adaptive tuning factor was used to modify the algorithm, eliminating the dynamic disturbance errors and adjusting the noise covariance matrix of the process noise accordingly; and (3) the square root method is utilised to implement the proposed algorithm, improving the numerical stability of the filter. The proposed claims are evaluated in a field test conducted by a UAV. Considering the statistical characteristics in Tables 3–5, the proposed SrAH∞CKF reduced the mean attitude and position/velocity estimation errors. These values are obtained by using the average test results. The proposed algorithm can produce more reliable performance, especially for low-cost INS/GNSS navigation systems, given its navigation accuracy and theoretical superiority while having similar temporal performance as CKF.