Validation and Experimental Testing of Observers for Robust GNSS-Aided Inertial Navigation

Abstract

This chapter is the study of state estimators for robust navigation. Navigation of vehicles is a vast field with multiple decades of research. The main aim is to estimate position, linear velocity, and attitude (PVA) under all dynamics, motions, and conditions via data fusion. The state estimation problem will be considered from two different perspectives using the same kinematic model. First, the extended Kalman filter (EKF) will be reviewed, as an example of a stochastic approach; second, a recent nonlinear observer will be considered as a deterministic case. A comparative study of strapdown inertial navigation methods for estimating PVA of aerial vehicles fusing inertial sensors with global navigation satellite system (GNSS)-based positioning will be presented. The focus will be on the loosely coupled integration methods and performance analysis to compare these methods in terms of their stability, robustness to vibrations, and disturbances in measurements.

1. Introduction

Inertial navigation systems (INSs) are widely used with price being a crucial factor predetermining the application. In case of unmanned vehicles, “low-cost” or “cost-effective” systems are preferred in general applications. As long as low-cost inertial measurement units (IMUs) use micro-electro-mechanical system (MEMS)-based inertial sensors, they are small in dimension and light and are low power consuming, and thus their presence can be found for instance in mobile phones, terrestrial vehicles, robots, stabilized platforms as well as in unmanned aerial vehicles (UAVs), small aircraft, and satellites. Even if the applications are cost-effective, the performance commonly requires data fusion from various sources due to the inertial sensors’ imperfections, such as insufficient resolution for navigation purposes, bias instabilities, noise, etc. Therefore, special data treatment is required. In sense of aerial applications, the usage of UAVs has increased rapidly in recent years. UAVs can be used in many applications [1, 2] fulfilling a broad spectrum of assignments in fields of reconnaissance, surveillance, search and rescue, remote sensing for atmospheric measurements, traffic monitoring, natural disaster response, damage assessment, inspection of power lines, or for aerial photography [2, 3]. These applications generally require navigation to be carried out which includes the position, velocity, and attitude (PVA) estimation [4], and thus cost-effective solutions have been commonly studied and implemented with advantage.

Current research and development in the area of low-cost navigation systems are focused on small-scale and integrated solutions [5]. As mentioned, as long as MEMS-based IMUs are used, the evaluation process requires data fusion from other aiding sources available. These sources stabilize errors in navigation solutions and thus increase navigation accuracy. Over the last few years, a solution for vehicle navigation without absolute position measurements provided by global positioning system (GPS) or radio frequency beacons has become very popular. For indoor or low-altitude navigation, it can use for example cameras, laser scanners, or odometers in terrestrial navigation [6, 7]. However, the solutions fusing inertial and GPS measurements are still preferable for aerial vehicles operating outside in large areas simply because of unblocked GPS signals. The implementation of other aiding sensors, such as magnetometer or pressure sensors, can further enhance the overall accuracy, reliability, and robustness of a navigation system [8, 9]. Attention is also paid to data processing algorithms used for PVA estimation, so that many literature sources can be found dealing with filtering techniques used for instance complementary filters [10], particle filters [11], or Kalman filters (KFs) [12, 13]. In the last named case, the extended KF (EKF) is used most of the time since it provides an acceptable accuracy with a reasonable computational load. Therefore, KF represents one of the most used algorithms for UAV attitude estimation (see comprehensive survey of estimation techniques in [14]) and is often complemented by other algorithms and decision-based aiding [15]. Since the accuracy of navigation systems is always directly related with the choice of sensors, the chapter also includes a short introduction on sensors suitable for cost-effective navigation systems as well as topics concerning stochastic sensor parameter evaluation methods and data pre-processing.

The contribution of this chapter is dedicated to comparison of two approaches suitable for navigation solutions and thus provides a clear understanding of the differences in the studied approaches. These approaches are tuned to satisfy a certain level of accuracy and applied on real flight data. The results are compared to an accurate referential attitude obtained from a multi-antenna GPS receiver. Such comparison with an independent referential system provides a thorough evaluation of performances of the studied approaches and shows their capabilities to handle sensors’ imperfections and vibration impacts of harsh environment on the accuracy of attitude estimation in aerial applications.

2. Theoretical background

This section will include an introduction to sensors suitable for cost-efficient navigation systems, as well as topics concerning deterministic and stochastic sensor parameters. Moreover, a review of the current state of the art on state estimation will be included, while also the kinematic vehicle model and general assumptions are presented.

2.1. Inertial sensors

Navigation systems providing the tracking of an object’s attitude, position, and velocity play a key role in a wide range of applications, e.g., in aeronautics, astronautics, robotics, automotive industry, underwater vehicles, or human motion observation. A basic technique to do so is via dead reckoning. One technique for dead reckoning is using an initial position, velocity, and attitude and consecutively updating the estimates based on acceleration and angular rate measurements. These measurements are generally provided by three axial accelerometer (ACC) and three axial angular rate sensors (gyros) forming a so-called inertial measurement unit (IMU). The inertial sensors have to be chosen according to required accuracy and economical aspects. The sensors are a major source of errors in navigation systems. Therefore, the type of application should be considered as well. The required accuracy related to various applications is shown in Figures 1 and 2, see [16], for gyroscopes and accelerometers, respectively.

Figure 1.

Bias instability of gyroscopes related to specified applications [16].

Figure 2.

Bias instability of accelerometers related to specified applications [16].

Accuracy of performed navigation is related to inertial sensors’ characteristics such as resolution and sensitivity and their imperfections in terms of bias instability, scale factor nonlinearity, and dependency of sensitive element on other quantities than just gyros or ACCs. Unwanted deterministic behavior can be reduced by calibration, but stochastic parameters such as bias instability and initial offset can be described by statistical values.

An uncompensated accelerometer bias, b_ACC, contributes in position error based on $\Delta p=1/2b_{\text{ACC}}{t}^2$, where tis time. Thus, even small deviations in sensed acceleration will cause unbound error in position with time. For instance, when b_ACC = 0.1 mg is considered, it leads to position errors of 0.05 m after 10 s, and an error of 177 m after 600 s. Generally speaking, all navigation systems dedicated for aircraft need to fulfill the requirement of a maximum error of 1 nautical mile per hour. In navigation attitude, accuracy also plays a key role. When attitude is considered, the azimuth is the most difficult parameter to estimate, since ACCs can be used for pith and roll angle compensation. For azimuth compensation, other sources can be used, e.g., magnetometers and GNSS, but these are not inertial sensors and thus depend on environmental conditions.

For aircraft navigation it is, according to Figures 1 and 2, required to use gyros with the precision better than 1 deg/h and ACC not more than 10μg, see [17]. The higher precision, the more expensive the device is. The other aspect, which has to be taken into account, is to check if a particular device is in solid state or is using moving parts. Figures 3 and 4 depict the current state of gyroscope and accelerometer technology. Recently, there has been a progress on MEMS-based sensors increasing the sensitivity of the gyroscopes to compete with the fiber optic gyros (FOGs). However, FOGs still provide better stability than the MEMS-based gyros. For ACCs, it has become very popular to use quartz resonator in applications with high-accuracy requirements due to its costs. If higher accuracy is still required, only mechanical pendulous rebalance (servo) ACCs have to be utilized. According to Figures 3 and 4, one can see that mechanical gyros and ACCs still satisfy the precision requirements best; nevertheless, there is a trend to replace them with solid-state devices for their better reliability, stability, and mean-time-before-failure parameter. Therefore, in the following paragraphs, solid-state devices will be considered.

Figure 3.

Gyro technology [17].

Figure 4.

Accelerometer types and their performances [17].

The most precise device for angular rate measurements is a ring laser gyroscope (RLG), which has stability better than 0.1 deg/h and resolution better than 10⁻⁶ deg/s. In the case of ACC, the most precise existing device is a servo ACC with a resolution about 1 μg. These devices would have been ideal for all applications, if they were not so expensive. Due to this reason, other systems, such as micro-electro-mechanical system (MEMS)-based ones, have been used in cost-effective applications, such as navigation of small aircraft and unmanned vehicles both terrestrial and aerial. MEMS sensors offer reduced power consumption, weight, manufacturing and assembly costs, and increased system design flexibility. Reducing the size and weight of a device allows multiple MEMS components to be used to increase functionality, device capability, and reliability. In contrast, MEMS-based systems might suffer from low resolution, noisy output, bias instability, temperature dependence, etc. Nevertheless, their applicability in navigation is wide due to fast technology improvements, applied data processing algorithms, and aiding systems. In navigation, aiding systems are commonly used to provide corrections for position, velocity, or attitude. Those systems might be based for instance on GNSS, electrolytic tilt sensors, pressure-based altimeter, odometer, laser scan, or vision-based odometry usage.

In cost-effective applications, MEMS-based devices are preferred. Therefore, their usage has to be accompanied by modern methods of signal and data processing, algorithms for their calibration, parameters identification, and fusion.

2.1.1. Gyroscopes

The basic parameters generally provided in product datasheets include dynamic range, initial sensitivity, nonlinearity, alignment error, initial bias error, in-run bias instability, angular random walk, linear acceleration effect on bias, and rate noise density. According to these parameters, the following types of gyros can be defined: low-cost, moderate-cost, and high-performance gyros. When looking at datasheets, the in-run bias stability provides the information about the best sensor performance corresponding to the gyro resolution floor. Unfortunately, there are other exhibiting error factors which affect a gyro performance. In all cases, the gyro noise and its frequency dependency have to be taken into account and handled. Stochastic sensor parameters can be generally estimated via power spectral density (PSD) analyses or via Allan variance analysis (AVAR).

In the case of low- and moderate-cost gyros, scale factor, alignment error, and null bias errors accompanied by parameters variation over a temperature range highly decrease the gyro performance. To minimize their impacts, it is required to perform the calibration within which a correction table or polynomial correction function is acquired. More details about the calibration methods and procedures can be found in [18].

The other perspectives of the gyro performance are produced by the fact that it does not measure just a rotational rate, but also its sensitive element has linear acceleration and g² sensitivities. It is caused by the asymmetry of a mechanical design and/or micromachining inaccuracies, and it can vary from design to design. Due to Earth’s 1 g field of gravity, according to [19], it can suffer from large errors when uncompensated. In the case of low-cost gyros, the g and g² sensitivities are not specified because their design is not optimized for a vibration rejection. They can have g sensitivity about 0.3°/s/g. Therefore, looking at a bias instability in these cases is almost pointless due to the high effect of this vibration behavior. Higher performance gyros improve the vibration rectification by design so the g sensitivity can go down to about 0.01°/s/g. To further decrease this sensitivity, anti-vibration mounts might be applied. Nevertheless, these anti-vibration mounts are very difficult to design, because they do not have a flat response over a wide frequency range, and they work particularly poorly at low frequencies. Moreover, their vibration reduction characteristics change with temperature and life cycle.

2.1.2. Accelerometers

MEMS technology-based accelerometers (ACCs) in the navigation area measure specific force mainly on a capacitive principle and/or on a vibrating differential structure. The basic parameters include measurement range, nonlinearity, sensitivity, initial bias error, in-run bias instability, noise density, bandwidth of frequency response, alignment error, and cross-axis sensitivity. In the case of multi-axial ACCs, the z-axis often has a different noise and bias performance. Vibrations will affect ACCs; however, if the frequency spectrum is adequate for the application, no problem arises from this point of view. The current MEMS technology cannot compete with high-performance types and cannot be implemented to stand-alone inertial navigation systems due to their low resolution, bias instability, and insufficient noise level reduction. Generally, this type of ACC is used in navigation systems in which a GNSS receiver is also implemented to compensate position errors, or in attitude and heading reference systems in which the position is not required, and thus ACCs are used just for an attitude measurement done according to Earth’s field 1 g sensing. ACC stochastic parameters can be estimated the same way as in the case of gyros via PSD or AVAR. More details about the calibration and estimation of deterministic errors and consecutive analyses can be found in [18, 20].

2.2. Inertial sensors’ stochastic parameters

Various methods for stochastic error estimation and sensor modeling exist, for example PSD and auto correlation function (ACF) which are straightforward; however, these methods cannot clearly distinguish different characters of noise error sources inside the data without understanding the sensor model and its state-space representation [21]. On the contrary, AVAR is a time-domain approach to analyze time series of data from the noise terms’ point of view. The AVAR was introduced by Allan in 1966 [22]. Originally, it was oriented at the study of oscillator stability; however, after its first publication, this kind of analysis was adopted for general noisy data characterization. Because of the close analogies to inertial sensors, the AVAR has been also included in the IEEE standards, e.g. [23–25], and that is why AVAR has become a standard tool for inertial sensors’ noise analysis. As described in [26], the AVAR technique provides several significant advantages over the others. Traditional approaches, such as computing the sampled mean and variance from a measured data set, do not reveal the underlying error sources. Although the combined PSD/ACF approach provides a complete description of error sources, the results are difficult to interpret.

The AVAR and its results are related to five noise terms, defined in Table 1, whose typical performance can be seen in Figure 5. This kind of error sources can be identified in inertial sensor output, and whose estimation can lead to error suppression in the data [25, 27]. The five basic noise terms correspond to the following random processes: angle/velocity random walk, rate/acceleration random walk, bias instability, quantization noise, and drift rate ramp. Values of particular coefficients denoted in Table 2 in the last column can be observed in AVAR deviation plots in a time instance corresponding to the one indicated in particular brackets. Furthermore, this basic set of random processes is extended by the sinusoidal noise and exponentially correlated (Markov) noise [27]. Generally, a total noise error can be classified as a sum of individual independent noise errors [25], and the total variance can be expressed as

${\sigma }_{\text{total}}^2={\sigma }_{\text{Q}}^2+{\sigma }_{\text{ARW}}^2+{\sigma }_{\text{BIN}}^2+{\sigma }_{\text{RRW}}^2+{\sigma }_{\text{RR}}^2$E1

where the abbreviations correspond to Table 1.

Type of noise	Abbreviation	Curve slope	Value of coefficients
Quantization noise	Q	−1	Q= σ($\sqrt{3}$)
Angular/velocity random walk	ARW	−1/2	N= σ(1)
Flicker noise/bias instability	BIN	0	B= σmin / 0.664
Rate/acceleration random walk	RRW	+1/2	K= σ(3)
Rate ramp noise	RR	+1	$R=\sigma (\sqrt{2})$

Table 1.

Summary of error sources and their characterization [25].

Figure 5.

Allan variance/deviation plot [25].

	Parameter	DMU10; (Silicon S.)	AHRS M3; (InnaLabs)	MPU9150; (InvenSense)	DSP-3100; (KVH)	INN-204; (InnaLabs)
$\text{GYR}$	$\text{ARW}(°/\sqrt{h})$	0.35	2.50	0.25	0.03	NA
	BIN (°/h)	7.53	55.72	15.06	0.60	NA
$\text{ACC}$	$\text{VRW}(m/s/\sqrt{h})$	0.05	0.06	0.08	NA	0.01
	BIN (mg)	0.04	0.06	0.06	NA	0.01

Table 2.

Stochastic parameters of inertial sensors according to AVAR.

2.3. State-of-the-art of state estimators

Many physical systems are considered partly closed systems with no means of measuring internal signals, where only the inputs and outputs are available. However, it is often of interest to know the current value of the internal states, e.g., such that appropriate action can be taken using a control element. There might be multiple internal states and only a few measured outputs due to lack of appropriate sensors, cost, or insufficient data rate. The state estimation problem describes the need to estimate variables of interest in a model that is not otherwise directly observable [28].

The model describes the dominating dynamics of the system, while less important dynamics might be removed for simplicity. The states for navigation systems often include position, linear velocity, and attitude of the vehicle, while the inclusion of auxiliary states is possible. These auxiliary states might describe specific force of the vehicle or inertial sensor errors [29].

State estimators consist of two categories: “filters” and “observers.” Filters take the stochastic approach to find the current state values and consider the measurement and state noise as well as the covariance estimate of the states. Observers use a deterministic approach based on control theory focusing on the stability of the proposed equations. In both cases, a model of the physical system is duplicated to propagate the states while comparing with the system outputs. In the literature, the terms “filter” and “observer” are used somewhat interchangeably.

The following sections will include a review of previous work on Kalman filters and nonlinear observers for navigation.

2.3.1. Kalman filter review

Modern filtering theory began around 1959–1960 with publications by Swerling [30] and Kalman [31], presenting error propagation methods using a minimum variance estimation algorithm for linear systems. The discrete method presented by Kalman has received large attention and is now a coined term in multiple fields [32].

The Kalman filter (KF) introduced a recursive algorithm for state estimation, which is optimal in the sense of minimum variance or least square error. Changing from analytical solutions to a recursive algorithm had the advantage of being easily implementable in digital computers. Another advantage was that the previous non-recursive estimation methods used the entire measurement set, whereas the recursive estimation of the KF uses current measurements as well as prior estimates to propagate the states from an initial estimate. The KF is therefore more computational efficient as it can discard previous measurements and update the state estimates with only the present measurements [29]. The KF theory was expanded upon in 1961 by Kalman and Bucy [33], introducing a continuous time variant.

The Kalman filter’s stochastic approach to the state estimation problem assumes noise on the measurements as well as the state equations of the filter. It is a well-established state estimation approach [34] which excels in working with normal-distributed inputs characterized by their mean and covariance values and a linear time-varying state space model in its basic form. The KF is an estimator, which provides estimates of the state as well as its uncertainty [35]. The measurements have to be functions of the states, as the residual measurement (the difference between measured and estimated measurements) is used to update the states and keep them from diverging. The process and measurement noise is assumed to be Gaussian white noise. In some cases where the noise of the physical system cannot be confirmed to be white, the KF might be augmented, by so-called “shaping filters, with additional linear state equations to let the colored noise be driven by Gaussian white noise [28]. In addition to the recursive estimation of the model states, the Kalman filter also propagates a covariance matrix describing the uncertainties of the state estimates as well as the correlation between the various states [29].

Even though the Kalman filter was designed for linear systems, it can be applied to nonlinear systems without changing the structure or the operational principles. However, the optimality of minimal variance of the errors is lost, and the filter is no longer an optimal estimator. The kinematic equations are inherently nonlinear and thus must be addressed by nonlinear techniques or approximations to maintain the performance and stability of the modeled system. Nonlinear problems are commonly handled by the linearized KF (LKF), extended KF (EKF), or sample-based methods such as unscented KF (UKF) [36, 37]. Probably the most popular of the mentioned methods is the EKF, which has been applied in an enormous number of applications where it achieved excellent performance [38]. The EKF linearizes the model around an estimate of the current mean using multivariate Taylor expansions to adapt to the nonlinear model; however, this makes the EKF more susceptible to errors in the initial estimates and modeling errors compared to the KF.

The KF and EKF are seen as the standard theory and are therefore used as baseline for comparison when developing new methods. The KF and its variants are widely used in the navigation-related literature where a few examples are mentioned. An introduction to choice of states and sensor alignment consideration can be found in [39], while [40] considers alternative attitude error representations. For extensive details on Kalman filtering, see [28, 29, 38, 9, 41–43]. Among the extensions to nonlinear systems, other examples can be found, e.g., [44] where a method for evaluating the linearization quality is presented alongside a Kalman filter extension for nonlinear systems. The unscented Kalman filter (UKF) is an extension to nonlinear systems that does not involve an explicit Jacobian matrix, see e.g., [45]. Studies on time-correlated noise, as opposed to the white noise assumption, without state augmentation have been carried out in [46, 47]. The adaptive Kalman filter might be used in applications where tuning of the Kalman filter is uncertain at initialization, see [48–50]. If the application is not real-time critical, such as surveying, the estimate can be enhanced by use of a smoother. In [51], a forward smoother was proposed, while in [52] a backward smoother was introduced. When nonlinear systems are considered, another alternative to the EKF is the particle filter. Particle filters are based on sequential Monte Carlo estimation algorithms, which compared to the Kalman filter are more computationally demanding; however, they are noise distribution independent, see e.g. [37, 53–56]. The advantage of the particle filter is its use in nonlinear non-Gaussian systems. However, since this approach is computationally heavy in current navigation systems, it is not often used. Therefore, the particle filter is considered outside the scope of this chapter.

2.3.2. Nonlinear observer review

In comparison to the Kalman filter, the nonlinear observers have a shorter history, motivated by drawbacks of the KF when applied to nonlinear systems. These drawbacks include unclear convergence properties for nonlinear systems, difficulty of tuning, and large computational load.

Nonlinear observers are contrary to the Kalman filters based on a deterministic approach. The noise is not assumed to have specific properties, except that the difference between the measured and estimated signal is smallest when the estimate reflects the true signal. Like the Kalman filter, nonlinear observers commonly utilize an injection term consisting of the difference between measured and estimated system output to drive the observer states toward the true values.

The field of nonlinear observers has expanded within groups dealing with specific problems. Nonlinear attitude estimation has been the focus of extensive research [57–61]; see in particular [13] for an extensive survey including EKF methods. One method used has centered on the comparison of two attitude measurement vectors in the BODY frame with two corresponding vectors in an Earth-fixed or inertial frame. One such attitude observer was proposed by [62] and was later expanded upon by [63] to include a gyro bias estimate. A vector-based attitude observer was proposed by [64] which depended on inertial measurements, magnetometer readings, and GNSS velocity measurements. Expanding on this framework [65] introduced an attitude observer that utilized the derivative of the GNSS velocity as the vehicle acceleration allowing for comparison with accelerometer measurements.

Where the Kalman filter computes new gains for each iteration, some nonlinear observers have proven convergence with fixed or slowly time-varying gains, e.g. [66]. This is a computational improvement as the gain determination is the dominating computational burden, see [28; Section 5.6.1].

One of the design challenges of nonlinear observers is the requirement for proven stability. The optimality of the Kalman filter may give the user confidence in the performance and stability of the filter. However, for nonlinear observers, the stability should be explicitly stated, as the gain usually comes without any optimality guarantee. The aim is to be robust toward disturbances and poor initial estimates.

The field of nonlinear observers is recent and rapidly expanding. A few publications within navigation are mentioned here. Considerations of a nonlinear attitude estimator for use on a small aircraft was presented in [67], while a globally exponentially stable observer for long baseline navigation was presented in [68] with clock bias estimation in a tightly coupled system.

2.4. Models and preliminaries

Estimating the position, linear velocity, and attitude (PVA) of a vehicle is commonly achieved through INS/GNSS integration, where the inertial navigation system (INS) consists of an inertial measurement unit (IMU) providing inertial navigation between updates from a GNSS receiver. The GNSS receiver usually has a lower sample rate than the IMU and is used to update the PVA estimates by correcting for the drift of the inertial sensors.

2.4.1. Notation

A column vector $x\in {ℝ}^3$is denoted x= [x₁; x₂; x₃] with its transpose x^T and Euclidean vector norm ‖x‖₂. The same notation is used for matrices where the induced norm is used. The skew symmetric matrix of a vector xis given as

$S(x)=\left[\begin{array}{ccc}0& -x_3& x_2\\ x_3& 0& -x_1\\ -x_2& x_1& 0\end{array}\right]$

A unit quaternion, q= [r_q; s_q], consisting of a real part, $r_q\in ℝ$, and a vector part, $s_q\in {ℝ}^3$, has ‖q‖₂ = 1. A vector $x\in {ℝ}^3$can be represented as a quaternion with zero real part; $\overline{x}=[0;x]$. The product of two quaternions q₁ and q₂ is the Hamiltonian product denoted by $q_1\otimes q_2$. The cross-product of two vectors is then represented by ×.

Superscripts are used to signify which coordinate frame a vector is decomposed in. Rotation between two frames may be represented by a quaternion, $q_{\text{a}}^c$, describing the rotation from coordinate frame ato c, with the corresponding rotation matrix $R_{\text{a}}^c=R(q_{\text{a}}^c)\in S{O}_3$, where $R(q_{\text{a}}^c):=I+2s_{q_a^c}S\left(r_{q_a^c}\right)+2S{\left(r_{q_a^c}\right)}^2$. The attitude can also be expressed as Euler angles; ${\Theta }_{\text{ca}}={[\varphi ,\theta ,\psi ]}^T$with the associated rotation matrix $R_{\text{a}}^c=R({\Theta }_{\text{ca}})$:

$R({\Theta }_{\text{ca}})=\left[\begin{array}{ccc}c\psi c\theta & -s\psi c\varphi +c\psi s\theta s\varphi & s\psi s\varphi +c\psi c\varphi s\theta \\ s\psi c\theta & c\psi c\varphi +s\varphi s\theta s\psi & -c\psi s\varphi +s\theta s\psi c\varphi \\ -s\theta & c\theta s\varphi & c\theta c\varphi \end{array}\right]$

where the sine and cosine functions have been abbreviated, e.g. $\sin (\varphi )=s\varphi$. There is no difference between using $R(q_{\text{a}}^c)$and $R({\Theta }_{\text{ca}})$in terms of transformation. The Euler angles are often preferred as they are more intuitively interpreted; however, they suffer from singularities (e.g., at pitch of 90°) which the quaternion representation avoids [40].

Various reference frames will be used where the Earth-centered-Earth-fixed (ECEF) frame will use notation e, while bwill be used for BODY frame, nfor North East down (NED), and ifor Earth-centered inertial (ECI) frame.

2.4.2. Kinematic vehicle model

The vehicle model describes position, p^e, and linear velocity, v^e, as well as the attitude described either as Euler angles or quaternions. The gyroscope bias, b^b, is also included in the model and is assumed to be slowly time varying.

The kinematic equations describing the vehicle motion are [22]

${\dot{p}}^e=v^e$E2

${\dot{v}}^e=-2S({\omega }_{\text{ie}}^e)v^e+f^e+g^e(p^e)$E3

${\dot{q}}_{\text{b}}^e=\frac{1}{2}{q}_{\text{b}}^e\otimes {\overline{\omega }}_{\text{ib}}^b-\frac{1}{2}{\overline{\omega }}_{\text{ie}}^e\otimes q_{\text{b}}^e$E4

${\dot{R}}_{\text{b}}^n=R_{\text{b}}^{n}S({\omega }_{\text{ib}}^b)$E5

${\dot{b}}^b=0$E6

where Earth rotation rate around the ECEF z-axis, ${\omega }_{i\text{e}}^e$, is known and $g^e(p^e)$is the plumb bob gravity vector at the vehicle position. The specific force acting on the vehicle is described by f^e. The rotational matrix from body to NED frame is denoted as $R_{\text{b}}^n$. The kinematic equations for the Euler angle and quaternion propagations have been included; however, at implementation, only one of Eqs. (4) or (5) should be used.

The vehicle is described with 6 degrees of freedom (DOF) where the BODY-frame vectors are defined as shown in Figure 6. An UAV is used as an example with a position vector p^b= (x^b, y^b, z^b) and Euler vector ${\Theta }_{\text{n}}^b={[\varphi ,\theta ,\psi ]}^{\text{T}}$.

Figure 6.

6 DOF UAV in the BODY frame.

The UAV can be seen as an example of a high dynamic vehicle and can be considered a challenging navigation environment, as it allows for rapid changes in attitude and heading.

2.4.3. Measurement assumptions

It is assumed that the vehicle is equipped with an IMU and a GNSS receiver, as well as a magnetometer. The following measurements are assumed available:

• Position measurement, $p_{\text{GNSS}}^e=p^e$,

• Specific force measurement, $f_{\text{IMU}}^b=f^b$, acting on the vehicle,

• Biased angular velocity measurement, ${\omega }_{\text{ib},\text{IMU}}^b={\omega }_{\text{ib}}^b+b^b$,

• Magnetic field measurement, $m_{\text{IMU}}^b=m^b$, of the Earth’s magnetic field at vehicle position.

Furthermore, knowledge of bounds on the magnitude of specific force and gyro bias, denoted as M_f and M_b respectively, is assumed. The natural magnetic field at any position is assumed known in NED and ECEF frame, as mⁿand m^e, respectively.

3. Practical approaches to the state estimation problem

In the following, the two state estimators will be introduced. First, the EKF will be presented where the Allan variance is applied to tune the covariance matrices. Second, the nonlinear observer will be introduced consisting of two parts: a nonlinear attitude estimator and a translational motion observer.

3.1. Extended Kalman filter

One solution for estimating position, linear velocity, and attitude is to utilize an IMU/GPS loosely coupled integration scheme, shown in Figure 7, which can be done by an EKF (for details about the EKF algorithm see [69]). The 12-dimensional state vector contains position in NED frame, velocity in the BODY frame, attitude, and gyro biases. The estimation is done with respect to a control vector uconsisting of measured specific forces and angular rates, and to a measurement vector ydefined in Eq. (8). The measurement vector in Eq. (8) is three dimensional and includes a GNSS position in NED frame. The state and measurement vectors are given as

$x={[p_{\text{N}},p_{\text{E}},p_{\text{D}},v_x,v_y,v_z,\varphi ,\theta ,\psi ,b_{\omega x},b_{\omega y},b_{\omega z}]}^{\text{T}}$E7

$y={[p_{\text{N}},p_{\text{E}},p_{\text{D}}]}^{\text{T}}$E8

where $p^n=(p_{\text{N}},p_{\text{E}},p_{\text{D}})$are components of position vector in NED frame; $v^b=(v_{\text{x}},v_{\text{y}},v_{\text{z}})$are the BODY-frame components of velocity vector; the gyroscope bias is decomposed into $b^b=(b_{\omega x},b_{\omega y},b_{\omega z})$; and yis the measurement vector.

Figure 7.

IMU/GNSS loosely coupled integration scheme.

(SF stands for specific force and LP is low pass)

The system function f(x, u) propagates the state xand input u(i.e., accelerations and angular rates) and the measurement function h(x) is used to update the EKF state with measurements (i.e., GNSS-based position). They are defined as

$f(x,u)=\left[\begin{array}{c}{R}_{\text{b}}^n{v}^b\\ f^b+v^b\times ({\omega }_{\text{ib,IMU}}^b-b^b)-{(R_b^n)}^{\text{T}}{g}^n\\ \left[\begin{array}{ccc}1& \sin (\varphi )\tan (\theta )& \cos (\varphi )\tan (\theta )\\ 0& \cos (\varphi )& -\sin (\varphi )\\ 0& \sin (\varphi )\sec (\theta )& \cos (\varphi )\sec (\theta )\end{array}\right]({\omega }_{\text{ib,IMU}}^b-b^b)\\ b^b\end{array}\right]$E9

$h(x)=p^n$E10

where $g^n={[0;0;g]}^{\text{T}}$is the gravity vector and $\sec (\cdot )$is the secant function. The process and measurement noise covariance matrices Qand Rfor the model used in Eqs. (9) and (10) are defined as follows:

$Q=\text{diag}\left(0_3,{\sigma }_{\text{v}}^2,{\sigma }_{\text{ω}}^2,{\sigma }_{{\text{b}}_{\text{ω}}}^2\right),R=\text{diag}\left({\sigma }_{\text{p}}^2\right)$E11

where diagdenotes a diagonal matrix, and ${\sigma }_{\text{*}}^2$is a vector of element-wise squared standard deviations for velocity, angular rate, gyroscope biases, and GNSS-based position.

The system dynamic and measurement models are

$x_k={\Phi }_{k-1}{x}_{k-1}+{\Gamma }_{k-1}{u}_{k-1}+w_{k-1}$E12

$z_k=H_k{x}_k+v_k$E13

where the state transition matrix, ${\Phi }_k$, and control matrix, ${\Gamma }_k$, are linearized from Eq. (9) with respect to the state and input vector, respectively. The initial conditions are $E⟨x_0⟩={\widehat{x}}_0$and $E⟨x_0,x_0^{\text{T}}⟩=P_0$. The process noise and measurement noise are assumed to satisfy $w_k~N(0,Q_k)$and $v_k~N(0,R_k)$.

Figure 8.

Enhanced IMU/GNSS integration scheme.

The state vector and covariance matrices are described by a priori and posteriori part denoted with superscripts − and +, respectively. A discrete form of the time and correction update of the state vector and covariance matrix are given as [69]:

${\widehat{x}}_k^{-}={\Phi }_{k-1}{\widehat{x}}_{k-1}^{+}+{\Gamma }_k{u}_k$E14

$P_k^{-}={\Phi }_{k-1}{P}_{k-1}^{+}{\Phi }_{k-1}^{\text{T}}+Q_{k-1}$E15

$K_k=P_k^{-}{H}_k^{\text{T}}{\left(H_k{P}_k^{-}{H}_k^{\text{T}}+R_k\right)}^{-1}$E16

${\widehat{x}}_k^{+}={\widehat{x}}_k^{-}+K_k\left(z_k-H_k{\widehat{x}}_k^{-}\right)$E17

$P_k^{+}=(I-K_k{H}_k)P_k^{-}$E18

where the Kalman gain matrix is denoted by K_k, while the observation matrix, H_k, is the linearization of Eq. (10) with respect to the state vector.

The advantage of this approach is a straightforward implementation and satisfactory navigation performance. The motion model is corrected for the centrifugal force; therefore, it is highly preferable for applications where this force occurs frequently, e.g., during a turn. However, even when properly tuned, the estimates strongly rely on the GNSS signal availability. In the case of blocked or lost GNSS signal, the estimates begin to diverge quickly and results may become unstable as long as the filter parameters are not adjusted.

To enable enhanced positioning function of the solution within GNSS outages, it is recommended to integrate accelerometer biases into a state vector and add attitude corrections obtained from accelerometer readings. This extended solution might use the integration scheme depicted in Figure 8.

3.1.1. AVAR applied in Kalman filter modeling

Having a state transition matrix and observation matrix defined is one issue, but it is also very important to set driving noise in accordance to expected situation. The AVAR analysis can help to do so in terms for inertial sensors. A comparison of different grades inertial sensors from their stochastic parameters point of view is shown in Figures 9 and 10 and further summarized in Table 2 where two basic parameters, i.e., angular random walk (ARW) or velocity random walk (VRW), and bias instability (BIN) are picked up.

Figure 9.

AVAR analysis—Allan deviation plot of several MEMS based gyros (DMU10, DSP3100—tactical grade gyros, AHRS M3, MPU9150—commercial grade gyros).

Figure 10.

AVAR analysis—Allan deviation plot of several MEMS-based accelerometers (DMU10, INN204— tactical grade, AHRS M3, MPU9150—commercial grade).

According to Table 1, one can see that analyzed sensors differ, but the parameters still correspond to its sensor grade. However, it needs to be highlighted that the cheapest IMU MPU-9150 has parameters close to the boundary between commercial and tactical grade. So it leads to considering this unit as suitable for navigation solutions in robotics for its price and performance. From other perspectives, it is hard to say anything about the gyro sensitivity to “g” in the form of vibrations which might degrade the overall performance. Parameters ARW/VRW and BIN are generally used in covariance matrix of process noise, of course according to the particular model utilized.

Figure 11.

Block diagram of nonlinear observer.

3.2. Nonlinear observer

Numerous nonlinear observers have been proposed for integration of IMU and GNSS data; however, in the following, the observer proposed in [66] will be considered, estimating position and velocity in the ECEF frame and describing the attitude as a unit quaternion.

The nonlinear observer presented here has a modular structure consisting of an attitude estimator and a translational motion observer. The two subsystems are interconnected with feedback of the specific force estimate from the motion observer to the attitude estimator. An advantage of the modular design is that the stability properties of the subsystems can be investigated individually leading to the stability result of the entire observer system using nonlinear stability theory, see [66] for further details. The observer structure is depicted in Figure 11.

The subsystems will be explained in detail in the following sections.

3.2.1. Attitude estimation

The attitude of the vehicle is denoted by a unit quaternion, ${\widehat{q}}_b^e$, describing the rotation between the BODY and ECEF frame. The attitude observer is a complementary filter fusing data from an accelerometer, magnetometer, and gyroscope to estimate the vehicle attitude. The nonlinear observer estimating the attitude and gyro bias, ${\widehat{b}}^b$, is given as [58, 61]:

${\dot{\widehat{q}}}_b^e=\frac{1}{2}{\widehat{q}}_{\text{b}}^e\otimes \left({\overline{\omega }}_{\text{ib,IMU}}^b-{\overline{\widehat{b}}}^b+\overline{\widehat{\sigma }}\right)-\frac{1}{2}{\overline{\omega }}_{\text{ie}}^e\otimes {\widehat{q}}_{\text{b}}^e$E19

${\dot{\widehat{b}}}^b=\text{Proj}({\widehat{b}}^b,-k_I\widehat{\sigma })$E20

$\widehat{\sigma }=k_1{v}_1^b\times R{({\widehat{q}}_{\text{b}}^e)}^{\text{T}}{v}_1^e+k_2{v}_2^b\times R{({\widehat{q}}_{\text{b}}^e)}^{\text{T}}{v}_2^e$E21

where k₁, k₂, and k_I are positive and sufficiently large tuning constants. The Proj(·,·) operator limits the gyro bias estimate to a sphere with radius $M_{\widehat{b}}$where $M_{\widehat{b}}>M_b$. The injection term, $\widehat{\sigma }$, consists of two vectors in BODY frame and their corresponding vectors in ECEF frame. There are various ways of choosing these vectors, but here they will be considered as

$v_1^b=\frac{f^b}{{\Vert f^b\Vert }_2},v_2^b=\frac{m^b}{{\Vert m^b\Vert }_2}\times v_1^b,v_1^e=\frac{{\widehat{f}}^e}{{\Vert {\widehat{f}}^b\Vert }_2},v_2^e=\frac{m^e}{{\Vert m^e\Vert }_2}\times v_1^e$E22

where the specific force estimate, ${\widehat{f}}^e$, will be supplied by the translational motion observer, while the magnetic field vector, m_e, is assumed known and depends on the vehicle position.

3.2.2. Translational motion observer

The translational motion observer estimates the position and velocity of the vehicle by using injection terms based on the difference between measured and estimated position. The measurements are traditionally provided by a GNSS receiver. Additionally, the observer also estimates the specific force of the vehicle by introducing an auxiliary state, ξ. The translational motion observer is described by

${\dot{\widehat{p}}}^e={\widehat{v}}^e+\theta K_{\text{pp}}\left(p_{\text{GNSS}}^e-{\widehat{p}}^e\right)$E23

${\dot{\widehat{v}}}^e=-2S({\omega }_{\text{ie}}^e){\widehat{v}}^e+{\widehat{f}}^e+g^e({\widehat{p}}_{\text{r}}^e)+{\theta }^2{K}_{\text{vp}}\left(p_{\text{GNSS}}^e-{\widehat{p}}^e\right)$E24

$\dot{\xi }=-R({\widehat{q}}_{\text{b}}^e)S(\widehat{\sigma })f^b+{\theta }^3{K}_{\xi \text{p}}\left(p_{\text{GNSS}}^e-{\widehat{p}}^e\right)$E25

${\widehat{f}}^e=R({\widehat{q}}_{\text{b}}^e)f^b+\xi$E26

The observer can also be stated with additional injection terms using GNSS velocity; however, it was shown in [70] that the velocity part of the injection term is not required to achieve stability.

The constant θ≥ 1 serves as a tuning parameter that should be sufficiently large to guarantee global stability of the interconnection of the translational motion observer and attitude observer. The gain matrices, K_pp, K_vp, and K_ξp can be chosen to satisfy A−KCbeing Hurwitz with

$A=\left[\begin{array}{ccc}0& I_3& 0\\ 0& 0& I_3\\ 0& 0& 0\end{array}\right],C=[\begin{array}{ccc}{I}_3& 0& 0\end{array}],K=\left[\begin{array}{c}{K}_{\text{pp}}\\ K_{\text{vp}}\\ K_{\xi \text{p}}\end{array}\right]$E27

The translational motion observer is similar to the EKF, and the gain matrix Kcan therefore be determined similarly to the EKF gain, by solving a Riccati equation. However, an advantage of this nonlinear observer is that the gain matrix is not required to be determined in each iteration, but rather on a slower time scale, see [66]. This time scale can be slower than the GNSS update rate, decreasing the computational load substantially. The load can be further reduced by considering the implementation as a fixed gain observer only determining the gains at the initialization phase. It has been shown in [71] that time-varying gains aid in sensor noise suppression and gives faster convergence.

4. Experimental verification

Experimental measurements from flights with a fixed-wing Bellanca Super Decathlon XXL unmanned aerial vehicle (UAV) are used to verify and compare the performance of the EKF and nonlinear observer. The UAV (shown in Figure 6) is equipped with an ADIS 16375 IMU, supplying acceleration and angular rate measurements, a HMR2300 magnetometer, and a GARMIN 18X GPS-receiver. The inertial data are sampled at 100 Hz, while the position measurements are sampled at 5 Hz. Furthermore, the UAV is equipped with a Polar X2@e (Septentrio) GPS system consisting of three antennas, placed at the wing tips and tale, providing attitude and position estimates. The estimates of the Septentrio system are considered highly accurate and therefore used as a reference for comparison with the estimates of the EKF and nonlinear observer.

The accuracy of the reference represented by the three-antenna Septentrio GPS receiver is evaluated based on the distances among three antennas and manufacturer documentation. The resulting attitude accuracy of 1σ is 0.2° in roll angle, 0.6° in pitch angle, and 0.3° in yaw angle. Accuracy in horizontal position in standalone application is 1.1m, with SBAS corrections about 0.7m.

The goal of the following experimental verification is to compare the performance of the proposed EKF and nonlinear observer. Two datasets were used in the verification where it was desired to use the same tuning for both datasets to ensure that the state estimators were not tuned specifically for a single dataset. The performance has been evaluated by comparison with the reference position, speed, and attitude. For each of the datasets, figures showing the estimation errors are depicted comparing the state estimators.

4.1. Parameters and tuning variables

The state estimators have several parameters and tuning variables to be determined, which will be presented and explained here. In the case of coinciding, naming subscripts “EKF” and “NO” will be used.

Tuning the EKF consists of choosing reasonable Q_EKF and R_EKF matrices. While the R_EKF matrix relies on the accuracy of the GNSS receiver, the Q_EKF matrix describes the expected process noise due to accelerometer and gyro noise and instabilities and can be tuned for the application. Here they are initialized as R_EKF = 14.40 I₃, with Q_EKF = blkdiag (0₃, 0.0962I₃, 0.0761 · 10⁻⁴ I₂, 0.3047 · 10⁻⁴, 3.0462 · 10⁻¹⁰ I₃). The state vector is driven by measured angular rates and specific force by inertial sensors having particular noise parameters. These parameters should be involved in the Q_EKF matrix.

Two versions of the fixed gain nonlinear observer are presented for comparison with the difference being the vectors used for attitude estimation: a magnetometer implementation (denoted as NLO-Mag) and a version with velocity vectors (denoted as NLO-Vel). The NLO-Vel version substitutes $v_2^b$and $v_2^e$in Eq. (22) with $v_2^b=[1;0;0]$and $v_2^e={\widehat{v}}^e/{\Vert {\widehat{v}}^e\Vert }_2$. This approach assumes the heading and course to be coinciding, which is mostly true for straight flight trajectories, ensuring uniform semi-global exponential stability through [72]. For flights including numerous turns, a magnetometer might be preferred as loitering, and cross-winds could affect the heading assumption.

The nonlinear observers include bound parameters which should be chosen sufficiently large M_b = 0.0087, while the remaining parameters are k₁ = 0.2, k₂ = 0.05, θ= 1, k_I= 0.00005. The fixed gains are K_pp = 0.38I₃, K_vp = 0.44 I₃ and K_ξp = 0.14 I₃. For the NLO-Vel, the attitude injection gain is substituted for k_2v = 0.01.

The initial values of the state vectors are chosen from the first available measurements and are similar for the three estimators (EKF, NLO-Mag, and NLO-Vel). It is important to tune the three state estimators equally and thoroughly to keep the comparison fair.

4.2. Results

Two datasets are available using the same UAV and sensor suite. The proposed state estimators are tested on both datasets to verify that they are not tuned exclusively for one dataset. The inertial measurements are preprocessed with a low-pass filter whose bandwidth is set according to vibration spectrum. Based on measured real-flight data obtained by the IMU unit and FFT analyses shown in Figure 12, the bandwidth of the fifth order low-pass filter was set to 5 Hz.

Figure 12.

Amplitude spectrum of the IMU measurements from the UAV flight. Top—accelerations, bottom—angular rates.

Figure 13.

Vehicle trajectory (Dataset 1): Septentrio (black), EKF (red), NLO-Mag (blue dashed), NLO-Vel (green).

Results of dataset 1 can be seen in Figures 13–16, while the results of dataset 2 are shown in Figures 17–20. The occasional gap in the attitude error is due to temporary loss of reference. The findings are evaluated and summarized in Table 2 which compares the two estimators during the two flights.

Figure 14.

Speed estimation error (Dataset 1): EKF (red), NLO-Mag (blue dashed), NLO-Vel (green).

Figure 15.

Attitude error (Dataset 1): EKF (red), NLO-Mag (blue dashed), NLO-Vel (green).

The trajectory of flight 1 is shown in Figure 13, covering an area of approximately 0.7 km² with a maximum altitude of 170 m. The estimation errors of speed, attitude, and position are shown in Figures 14–16, where the speed estimation error is centered around zero and includes a zoomed view for clarification. The attitude errors shown in Figure 15 have similar behavior for roll and pitch for the state estimators, whereas the nonlinear yaw estimate has some systematic offset. The position errors of Figure 16 are very similar for the state estimators attesting that the nonlinear observers have comparable results to the EKF.

Figure 16.

Position estimation error (Dataset 1): EKF (red), NLO-Mag (blue dashed), NLO-Vel (green).

Figure 17.

Speed estimation error (Dataset 2): EKF (red), NLO-Mag (blue dashed), NLO-Vel (green).

Figure 18.

Attitude error (Dataset 2): EKF (red), NLO-Mag (blue dashed), NLO-Vel (green).

The second dataset consisted of approximately a third of the amount of measurements compared to Dataset 1. The speed and attitude estimation errors are shown in Figures 17 and 18, with comparable performance between the EKF and nonlinear observers. The position errors depicted in Figure 19 show that an offset is present between the estimates, although the estimates follow the same pattern. Finally, the gyro bias estimates are shown in Figure 20. As there are no reference for the gyro biases, these are included to show the similarities across the state estimators.

Figure 19.

Position estimation error (Dataset 2): EKF (red), NLO-Mag (blue dashed), NLO-Vel (green).

Figure 20.

Gyro bias estimation (Dataset 2): EKF (red), NLO-Mag (blue dashed), NLO-Vel (green).

In summary, according to Table 3 and previous figures, the EKF and nonlinear observers are seen to have similar performance during both compared flights. The differences can be assumed negligible, and real flight conditions are considered. The attitude estimates shown in Figures 15 and 18 are very alike and correspond well to the reference, although the nonlinear yaw estimation is seen to have a systematic difference.

		EKF			NLO-Mag			NLO-Vel
Dataset 1	POS RMS:	3.43	2.71	2.62	3.37	2.62	2.50	3.37	2.63	2.50
	POS STD:	2.60	2.44	2.46	2.48	2.41	2.44	2.48	2.42	2.44
	ATT RMS:	1.83	2.59	5.50	2.02	2.69	5.46	1.93	3.01	6.88
	ATT STD:	1.67	1.81	5.42	1.84	1.90	5.28	1.90	1.92	6.15
	SPE RMS:		0.60			0.67			0.69
	SPE STD:		0.59			0.66			0.69
Dataset 2	POS RMS:	4.43	4.40	3.43	3.38	5.00	3.63	3.37	5.00	3.63
	POS STD:	2.53	4.35	2.86	2.46	4.20	2.82	2.46	4.20	2.82
	ATT RMS:1.76	1.87	6.39	2.09	1.87	7.85	1.56	1.94	6.36
	ATT STD:	1.70	1.66	6.34	1.73	1.67	6.28	1.46	1.59	6.36
	SPE RMS:		0.86			1.06			1.05
	SPE STD:		0.83			1.02			1.02

Table 3.

Observer performance comparison (NED position in m, attitude in degree, and speed in m/s).

The position estimation errors depicted in Figures 16 and 19 are within the expected bounds. From Table 3, it can be concluded that the three state estimators have good performances with little variation between the estimators. It can further be concluded that the tuning used gave good results for both datasets.

5. Conclusive remarks

Two methods for INS/GNSS integration have been investigated and compared: an extended Kalman filter using a 12-state vector and a nonlinear observer. The advantages and drawbacks of the methods have been presented and experimentally verified on flight data from a fixed-wing UAV. A reference system consisting of three-antenna GNSS receiver with the antennas placed at the tail and each wing tip was use for performance comparison of the presented state estimators.

The inertial sensors used in the integration schemes are considered low-cost variants with respect to the reference system utilized. As the performance of the presented methods estimates the position, linear velocity, and attitude reasonably close to the reference, it is concluded that the methods are able to overcome the vibrations, disturbances, and bias drift connected to low-cost sensors in reasonable manner and thus provide sufficiently stable and accurate navigation solution.

Acknowledgments

This work was partially supported by the EEA/Norway grant No. NF-CZ07-ICP-3-2082015 supported by the Ministry of Education, Youth and Sports of the Czech Republic and named Enhanced Navigation Algorithms in Joint Research and Education, and partially by Norwegian Research Council (projects no. 221666 and 223254) through the NTNU Centre of Autonomous Marine Operations and Systems (NTNU AMOS) at the Norwegian University of Science and Technology.