**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. More‐ over, 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**

stance inmobilephones,terrestrialvehicles,robots, stabilizedplatforms aswell as inunmanned aerial vehicles (UAVs), small aircraft, and satellites. Even if the applications are cost-effective, theperformancecommonlyrequiresdatafusionfromvarious sourcesduetotheinertial 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 applicationsgenerallyrequirenavigationtobe carriedoutwhichincludes theposition,velocity, andattitude(PVA)estimation[4],andthuscost-effectivesolutionshavebeencommonlystudied

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

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

and implemented with advantage.

108 Recent Advances in Robotic Systems

methods and data pre-processing.

the accuracy of attitude estimation in aerial applications.

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, automo‐ tive 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, respec‐ tively.

**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 *<sup>Δ</sup><sup>p</sup>* =1 / <sup>2</sup>*b*ACC*<sup>t</sup>* <sup>2</sup> , where *t* is 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 envi‐ ronmental 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 highaccuracy 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].

Validation and Experimental Testing of Observers for Robust GNSS-Aided Inertial Navigation http://dx.doi.org/10.5772/63575 111

**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−6 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 costeffective applications, such as navigation of small aircraft and unmanned vehicles both terrestrial and aerial. MEMS sensors offer reduced power consumption, weight, manufactur‐ ing 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 algo‐ rithms, 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*

Unwanted deterministic behavior can be reduced by calibration, but stochastic parameters

An uncompensated accelerometer bias, *b*ACC, contributes in position error based on

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 envi‐

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

, where *t* is time. Thus, even small deviations in sensed acceleration will cause

such as bias instability and initial offset can be described by statistical values.

*<sup>Δ</sup><sup>p</sup>* =1 / <sup>2</sup>*b*ACC*<sup>t</sup>* <sup>2</sup>

110 Recent Advances in Robotic Systems

ronmental conditions.

devices will be considered.

**Figure 3.** Gyro technology [17].

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 highperformance gyros. When looking at datasheets, the in-run bias stability provides the infor‐ mation 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 g2 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 g2 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**

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)

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

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

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

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

sensitivities are not specified because their design is not optimized for a vibration

analyses or via Allan variance analysis (AVAR).

112 Recent Advances in Robotic Systems

g and g2

and life cycle.

in [18, 20].

*2.1.2. Accelerometers*

calibration methods and procedures can be found in [18].

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\_Q^2 + \sigma\_{\text{ARW}}^2 + \sigma\_{\text{RIN}}^2 + \sigma\_{\text{RRW}}^2 + \sigma\_{\text{RR}}^2,\tag{1}
$$


where the abbreviations correspond to **Table 1**.

**Table 1.** Summary of error sources and their characterization [25].

**Figure 5.** Allan variance/deviation plot [25].


**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 interchangea‐ bly.

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

### *2.3.1. Kalman filter review*

**Figure 5.** Allan variance/deviation plot [25].

114 Recent Advances in Robotic Systems

**Parameter DMU10**

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

directly observable [28].

**(Silicon S.)**

**Table 2.** Stochastic parameters of inertial sensors according to AVAR.

**AHRS M3 (InnaLabs)**

BIN (°/*h*) 7.53 55.72 15.06 0.60 NA

BIN (*mg*) 0.04 0.06 0.06 NA 0.01

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

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

GYR ARW(° / *h* ) 0.35 2.50 0.25 0.03 NA

ACC VRW (*m* /*s* / *h* ) 0.05 0.06 0.08 NA 0.01

**MPU9150 (InvenSense)** **DSP-3100 (KVH)**

**INN-204 (InnaLabs)** 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 distribu‐ tion 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*

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

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 distribu‐ tion 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

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

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

estimates and modeling errors compared to the KF.

116 Recent Advances in Robotic Systems

scope of this chapter.

load.

the true values.

*2.3.2. Nonlinear observer review*

A column vector *x* ∈ℝ<sup>3</sup> is denoted *x* = [*x*1; *x*2; *x*3] with its transpose *x*<sup>T</sup> and Euclidean vector norm ‖*x*‖2. The same notation is used for matrices where the induced norm is used. The skew symmetric matrix of a vector *x* is given as

$$S(\mathbf{x}) = \begin{bmatrix} \mathbf{0} & -\mathbf{x}\_3 & \mathbf{x}\_2 \\ \mathbf{x}\_3 & \mathbf{0} & -\mathbf{x}\_1 \\ -\mathbf{x}\_2 & \mathbf{x}\_1 & \mathbf{0} \end{bmatrix}.$$

A unit quaternion, *q* = [*r*q; *s*q], consisting of a real part, *rq* ∈ℝ, and a vector part, *sq* ∈ℝ<sup>3</sup> , has ‖*q*‖<sup>2</sup> = 1. A vector *x* ∈ℝ<sup>3</sup> can be represented as a quaternion with zero real part; *x*¯ = 0; *x* . The product of two quaternions *q*1 and *q*<sup>2</sup> is the Hamiltonian product denoted by *q*<sup>1</sup> ⊗ *q*2. The crossproduct 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*<sup>a</sup> *c* , describing the rotation from coordinate frame *a* to *c*, with the corresponding rotation matrix *R*<sup>a</sup> *<sup>c</sup>* <sup>=</sup>*R*(*q*<sup>a</sup> *c* )∈*SO*3, where *R*(*q*<sup>a</sup> *c* ): = *I* + 2*s qa cS*(*r qa <sup>c</sup>*) + 2*S*(*r qa <sup>c</sup>*)2. The attitude can also be expressed as Euler angles; *<sup>Θ</sup>*ca <sup>=</sup> *<sup>ϕ</sup>*, *<sup>θ</sup>*, *<sup>ψ</sup> <sup>T</sup>* with the associated rotation matrix *R*<sup>a</sup> *<sup>c</sup>* <sup>=</sup>*R*(*Θ*ca):

$$R(\Theta\_{ca}) = \begin{bmatrix} c\boldsymbol{\psi}c\boldsymbol{\theta} & -s\boldsymbol{\psi}c\boldsymbol{\phi} + c\boldsymbol{\psi}s\boldsymbol{\theta}s\boldsymbol{\phi} & s\boldsymbol{\psi}s\boldsymbol{\phi} + c\boldsymbol{\psi}c\boldsymbol{\phi}s\boldsymbol{\theta} \\ s\boldsymbol{\psi}c\boldsymbol{\theta} & c\boldsymbol{\psi}c\boldsymbol{\phi} + s\boldsymbol{\phi}s\boldsymbol{\theta}s\boldsymbol{\psi} & -c\boldsymbol{\psi}s\boldsymbol{\phi} + s\boldsymbol{\theta}s\boldsymbol{\psi}c\boldsymbol{\phi} \\ -s\boldsymbol{\theta} & c\boldsymbol{\theta}s\boldsymbol{\phi} & c\boldsymbol{\theta}c\boldsymbol{\phi} \end{bmatrix},$$

where the sine and cosine functions have been abbreviated, e.g. sin(*ϕ*)=*sϕ*. There is no difference between using *R*(*q*<sup>a</sup> *c* ) and *R*(*Θ*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 *b* will be used for BODY frame, *n* for North East down (NED), and *i* for Earth-centered inertial (ECI) frame.

### *2.4.2. Kinematic vehicle model*

The vehicle model describes position, *pe* , and linear velocity, *ve* , as well as the attitude described either as Euler angles or quaternions. The gyroscope bias, *bb* , is also included in the model and is assumed to be slowly time varying.

The kinematic equations describing the vehicle motion are [22]

$$
\dot{\mathbf{p}}^e = \mathbf{v}^e,\tag{2}
$$

$$\dot{\mathbf{v}}^{\epsilon} = -\mathfrak{D}S(o\_{\mu}^{\epsilon})\mathbf{v}^{\epsilon} + f^{\epsilon} + \mathbf{g}^{\epsilon}(p^{\epsilon}),\tag{3}$$

$$
\dot{q}\_b^e = \frac{1}{2} q\_b^e \otimes \overline{\partial^b} - \frac{1}{2} \overline{\partial^a} \otimes q\_b^e,\tag{4}
$$

$$
\dot{R}\_b'' = R\_b'' S(o\_{lb}^b),
\tag{5}
$$

Validation and Experimental Testing of Observers for Robust GNSS-Aided Inertial Navigation http://dx.doi.org/10.5772/63575 119

$$
\dot{b}^b = 0,\tag{6}
$$

where Earth rotation rate around the ECEF *z*-axis, *ωi*<sup>e</sup> *e* , is known and *g <sup>e</sup>* (*p <sup>e</sup>* ) 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*<sup>b</sup> *<sup>n</sup>*. The kinematic equations for the Euler angle and quaternion propagations have been included; however, at implemen‐ tation, 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 *pb* = (*xb* , *yb* , *zb* ) and Euler vector *Θ*<sup>n</sup> *<sup>b</sup>* = *ϕ*, *θ*, *ψ* <sup>T</sup> .

**Figure 6.** 6 DOF UAV in the BODY frame.

Superscripts are used to signify which coordinate frame a vector is decomposed in. Rotation

( ) , é ù -+ + ê ú Q = + -+

where the sine and cosine functions have been abbreviated, e.g. sin(*ϕ*)=*sϕ*. There is no

often preferred as they are more intuitively interpreted; however, they suffer from singularities

Various reference frames will be used where the Earth-centered-Earth-fixed (ECEF) frame will use notation *e*, while *b* will be used for BODY frame, *n* for North East down (NED), and *i* for

, and linear velocity, *ve*

2 ( ) ( ), *e ee e e e ie v S v f gp* & =- + + w

> w

1 1 2 2 , *b e e e <sup>e</sup> ib ie b b <sup>b</sup> qq q* & = Ä- Ä w

( ), *n nb R RS*

*b b ib* = w

&

*R s c c c sss c s ss c s cs c c*

 qf

 y f yqf

ë û -

*c c s c c ss s s c cs*

*c*

*<sup>c</sup>*)2. The attitude can also be expressed as Euler angles;

*<sup>c</sup>* <sup>=</sup>*R*(*Θ*ca):

 y f y fq

 y f qy f

> q f

) and *R*(*Θ*ca) in terms of transformation. The Euler angles are

, describing the rotation from

, as well as the attitude described

, is also included in the model and

, *e e p v* & = (2)

(3)

(4)

(5)

)∈*SO*3, where

*<sup>c</sup>* <sup>=</sup>*R*(*q*<sup>a</sup> *c*

between two frames may be represented by a quaternion, *q*<sup>a</sup>

y q

y q y f fqy

*c*

either as Euler angles or quaternions. The gyroscope bias, *bb*

The kinematic equations describing the vehicle motion are [22]

q

(e.g., at pitch of 90°) which the quaternion representation avoids [40].

*<sup>Θ</sup>*ca <sup>=</sup> *<sup>ϕ</sup>*, *<sup>θ</sup>*, *<sup>ψ</sup> <sup>T</sup>* with the associated rotation matrix *R*<sup>a</sup>

*ca*

*R*(*q*<sup>a</sup> *c* ): = *I* + 2*s*

*qa cS*(*r qa <sup>c</sup>*) + 2*S*(*r qa*

118 Recent Advances in Robotic Systems

difference between using *R*(*q*<sup>a</sup>

Earth-centered inertial (ECI) frame.

The vehicle model describes position, *pe*

is assumed to be slowly time varying.

*2.4.2. Kinematic vehicle model*

coordinate frame *a* to *c*, with the corresponding rotation matrix *R*<sup>a</sup>

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:


Furthermore, knowledge of bounds on the magnitude of specific force and gyro bias, denoted as *M*<sup>f</sup> and *M*b respectively, is assumed. The natural magnetic field at any position is assumed known in NED and ECEF frame, as *mn* and *me* , 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 *u* consisting of measured specific forces and angular rates, and to a measurement vector *y* defined in Eq. (8). The measurement vector in Eq. (8) is three dimen‐ sional and includes a GNSS position in NED frame. The state and measurement vectors are given as

$$\mathbf{x} = \begin{bmatrix} p\_N, p\_E, p\_D, \mathbf{v}\_\chi, \mathbf{v}\_\chi, \mathbf{v}\_\varepsilon, \phi, \theta, \psi, \mathbf{b}\_{ax}, \mathbf{b}\_{ay}, \mathbf{b}\_{az} \end{bmatrix}^\mathsf{T},\tag{7}$$

$$\mathbf{y} = \begin{bmatrix} p\_N, p\_E, p\_D \end{bmatrix}^\mathrm{r},\tag{8}$$

where *<sup>p</sup> <sup>n</sup>* =(*p*N, *<sup>p</sup>*E, *<sup>p</sup>*D) are components of position vector in NED frame; *<sup>v</sup> <sup>b</sup>* =(*v*x, *<sup>v</sup>*y, *<sup>v</sup>*z) are the BODY-frame components of velocity vector; the gyroscope bias is decomposed into *<sup>b</sup> <sup>b</sup>* =(*bωx*, *<sup>b</sup>ωy*, *<sup>b</sup>ωz*); and *y* is the measurement vector.

**Figure 7.** IMU/GNSS loosely coupled integration scheme.

**•** Position measurement, *p*GNSS

120 Recent Advances in Robotic Systems

**•** Specific force measurement, *<sup>f</sup>* IMU

**•** Magnetic field measurement, *m*IMU

translational motion observer.

**3.1. Extended Kalman filter**

as *M*<sup>f</sup>

given as

**•** Biased angular velocity measurement, *ω*ib,IMU

known in NED and ECEF frame, as *mn* and *me*

*<sup>b</sup> <sup>b</sup>* =(*bωx*, *<sup>b</sup>ωy*, *<sup>b</sup>ωz*); and *y* is the measurement vector.

*<sup>e</sup>* = *p <sup>e</sup>* ,

*<sup>b</sup>* = *f <sup>b</sup>*

*<sup>b</sup>* =*m<sup>b</sup>*

**3. Practical approaches to the state estimation problem**

, acting on the vehicle,

*<sup>b</sup>* + *b <sup>b</sup>* ,

, of the Earth's magnetic field at vehicle position.

*<sup>b</sup>* <sup>=</sup>*ω*ib

Furthermore, knowledge of bounds on the magnitude of specific force and gyro bias, denoted

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

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 *u* consisting of measured specific forces and angular rates, and to a measurement vector *y* defined in Eq. (8). The measurement vector in Eq. (8) is three dimen‐ sional and includes a GNSS position in NED frame. The state and measurement vectors are

> [ , , , , , , , , , , , ], *N E Dxyz xyz x p p p vvv b b b* fqy

where *<sup>p</sup> <sup>n</sup>* =(*p*N, *<sup>p</sup>*E, *<sup>p</sup>*D) are components of position vector in NED frame; *<sup>v</sup> <sup>b</sup>* =(*v*x, *<sup>v</sup>*y, *<sup>v</sup>*z) are the BODY-frame components of velocity vector; the gyroscope bias is decomposed into

www

<sup>T</sup> = (7)

<sup>T</sup> , , ] ,[ *NED y ppp* = (8)

and *M*b respectively, is assumed. The natural magnetic field at any position is assumed

, respectively.

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

The system function *f*(*x*, *u*) propagates the state *x* and 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(\mathbf{x},\boldsymbol{\mu}) = \begin{bmatrix} R\_{\boldsymbol{\theta}}^{\boldsymbol{\mu}}\boldsymbol{\nu}^{\boldsymbol{\theta}} \\ \boldsymbol{f}^{\boldsymbol{\theta}} + \boldsymbol{\nu}^{\boldsymbol{\theta}} \times (\boldsymbol{\alpha}\_{\boldsymbol{\theta},\boldsymbol{\mu}\boldsymbol{\mu}\boldsymbol{\theta}}^{\boldsymbol{\theta}} - \boldsymbol{b}^{\boldsymbol{\theta}}) - (R\_{\boldsymbol{\theta}}^{\boldsymbol{\theta}})^{\top}\boldsymbol{g}^{\boldsymbol{\theta}} \\ \begin{bmatrix} 1 & \sin(\boldsymbol{\theta})\tan(\boldsymbol{\theta}) & \cos(\boldsymbol{\phi})\tan(\boldsymbol{\theta}) \\ 0 & \cos(\boldsymbol{\phi}) & -\sin(\boldsymbol{\phi}) \\ 0 & \sin(\boldsymbol{\phi})\sec(\boldsymbol{\theta}) & \cos(\boldsymbol{\phi})\sec(\boldsymbol{\theta}) \end{bmatrix} \begin{bmatrix} \boldsymbol{\alpha}^{\boldsymbol{\theta}} \\ (\boldsymbol{\alpha}\_{\boldsymbol{\theta},\boldsymbol{\mu}\boldsymbol{\theta}}^{\boldsymbol{\theta}} - \boldsymbol{b}^{\boldsymbol{\theta}}) \\ \boldsymbol{0} \\ \boldsymbol{b}^{\boldsymbol{\theta}} \end{bmatrix},\tag{9}$$

$$h(\mathbf{x}) = p^\*,$$

where *g <sup>n</sup>* = 0;0; *g* <sup>T</sup> is the gravity vector and sec( ⋅ ) is the secant function. The process and measurement noise covariance matrices *Q* and *R* for the model used in Eqs. (9) and (10) are defined as follows:

$$Q = \text{diag}\left(0\_{\text{>}}, \sigma\_{\text{>}}^2, \sigma\_{\text{as}}^2, \sigma\_{b\_{\text{>}}}^2\right), \qquad R = \text{diag}\left(\sigma\_{\text{~}}^2\right) \tag{11}$$

where *diag* denotes a diagonal matrix, and *σ*\* 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

$$\mathbf{x}\_{k} = \mathbf{0}\_{k-1}\mathbf{x}\_{k-1} + \Gamma\_{k-1}\mu\_{k-1} + \mathbf{w}\_{k-1} \tag{12}$$

$$
\Delta \varpi\_k = H\_k \mathbf{x}\_k + \mathbf{v}\_k \tag{13}
$$

where the state transition matrix, Φ*<sup>k</sup>* , and control matrix, *Γ<sup>k</sup>* , are linearized from Eq. (9) with respect to the state and input vector, respectively. The initial conditions are *E x*<sup>0</sup> = *x* ^ <sup>0</sup> and *E x*0, *x*<sup>0</sup> <sup>T</sup> <sup>=</sup>*P*0. The process noise and measurement noise are assumed to satisfy *wk* <sup>~</sup> *<sup>N</sup>* (0, *Qk* ) and *vk* ~ *N* (0, *Rk* ).

**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]:

$$
\hat{\mathbf{x}}\_k^- = \boldsymbol{\Phi}\_{k-1} \hat{\mathbf{x}}\_{k-1}^\* + \boldsymbol{\Gamma}\_k \boldsymbol{\mu}\_k \tag{14}
$$

$$P\_k^\* = \Phi\_{k-1} P\_{k-1}^\* \Phi\_{k-1}^T + \underline{Q}\_{k-1} \tag{15}$$

$$K\_k = P\_k^- H\_k^T (H\_k P\_k^- H\_k^T + R\_k)^{-1} \tag{16}$$

$$
\hat{\mathbf{x}}\_k^\* = \hat{\mathbf{x}}\_k^- + K\_k(\boldsymbol{\varpi}\_k - H\_k \hat{\mathbf{x}}\_k^-) \tag{17}
$$

$$P\_k^\* = (I - K\_k H\_k) P\_k^\* \tag{18}$$

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

The advantage of this approach is a straightforward implementation and satisfactory naviga‐ tion 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 recom‐ mended 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*

*k kk kk k* 11 11 1 *x x uw* =F +G + -- -- - (12)

where the state transition matrix, Φ*<sup>k</sup>* , and control matrix, *Γ<sup>k</sup>* , are linearized from Eq. (9) with

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

11 1 1

*<sup>T</sup> PP Q k kk k k*

<sup>1</sup> ( ) *T T K PH HPH R k k k kk k k*

( ) *P I KH P <sup>k</sup> kk k*

<sup>T</sup> <sup>=</sup>*P*0. The process noise and measurement noise are assumed to satisfy *wk* <sup>~</sup> *<sup>N</sup>* (0, *Qk* )

respect to the state and input vector, respectively. The initial conditions are *E x*<sup>0</sup> = *x*

*E x*0, *x*<sup>0</sup>

and *vk* ~ *N* (0, *Rk* ).

122 Recent Advances in Robotic Systems

**Figure 8.** Enhanced IMU/GNSS integration scheme.

the state vector and covariance matrix are given as [69]:

*z Hx v k kk k* = + (13)

1 1 ˆ ˆ *k k k kk x xu* - + =F +G - - (14)



ˆˆ ˆ ( ) *k k k k kk x x K z Hx* + - - =+ - (17)

+ - = - (18)

^ <sup>0</sup> and

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

**Figure 10.** AVAR analysis—Allan deviation plot of several MEMS-based accelerometers (DMU10, INN204— tactical

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

grade, AHRS M3, MPU9150—commercial grade).

124 Recent Advances in Robotic Systems

to the particular model utilized.

**Figure 11.** Block diagram of nonlinear observer.

The attitude of the vehicle is denoted by a unit quaternion, *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, *b* ^*b* , is given as [58, 61]:

$$
\dot{\hat{q}}\_b^e = \frac{1}{2} \hat{q}\_b^e \otimes \left( \overline{\hat{o}\_{lb, l\text{fl}U}^b} - \overline{\hat{b}}^b + \overline{\hat{\sigma}} \right) - \frac{1}{2} \overline{o}\_{b\iota}^e \otimes \hat{q}\_b^e,\tag{19}
$$

$$
\dot{\hat{b}}^b = \text{Proj}(\hat{b}^b, -k\_\text{I}\hat{\sigma}),
\tag{20}
$$

$$
\hat{\sigma} = k\_1 \mathbf{v}\_1^b \times R(\hat{q}\_b^e)^\mathrm{T} \mathbf{v}\_1^e + k\_2 \mathbf{v}\_2^b \times R(\hat{q}\_b^e)^\mathrm{T} \mathbf{v}\_2^e,\tag{21}
$$

where *k*1, *k*2, and *k*I are positive and sufficiently large tuning constants. The Proj(·,·) operator limits the gyro bias estimate to a sphere with radius *Mb* ^ where *Mb* ^ >*Mb*. The injection term, *σ* ^, 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

$$\nu\_1 \nu\_1^b = \frac{f^b}{\|\|f^b\|\|\_2}, \quad \nu\_2^b = \frac{m^b}{\|\|m^b\|\|\_2} \times \nu\_1^b, \quad \nu\_1^s = \frac{\hat{f}^s}{\|\|\hat{f}''\|\|\_2}, \quad \nu\_2^s = \frac{m^s}{\|\|m^s\|\|\_2} \times \nu\_1^s,\tag{22}$$

where the specific force estimate, *f* ^ *e* , will be supplied by the translational motion observer, while the magnetic field vector, *me*, 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{\hat{p}}^{\epsilon} = \hat{\nu}^{\epsilon} + \theta K\_{pp} (p\_{G\text{NN}}^{\epsilon} - \hat{p}^{\epsilon}),
\tag{23}
$$

$$\dot{\hat{\mathbf{v}}}^{\varepsilon} = -2S(o\_{\mathrm{i}\prime}^{\varepsilon})\hat{\mathbf{v}}^{\varepsilon} + \hat{f}^{\varepsilon} + \mathbf{g}^{\varepsilon}(\hat{p}\_{r}^{\varepsilon}) + \theta^{2}K\_{\mathrm{vp}}(p\_{\mathrm{GNN3}}^{\varepsilon} - \hat{p}^{\varepsilon}),\tag{24}$$

$$\dot{\tilde{\varphi}} = -R(\hat{q}\_b^{\epsilon})S(\hat{\sigma})f^{\flat} + \theta^{\flat}K\_{\xi\rho}(p\_{G\text{NN}}^{\epsilon} - \hat{p}^{\epsilon}),\tag{25}$$

$$
\hat{f}^{\epsilon} = R(\hat{q}\_b^{\epsilon})f^{b} + \xi. \tag{26}
$$

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−KC* being Hurwitz with

$$A = \begin{bmatrix} 0 & I\_3 & 0 \\ 0 & 0 & I\_3 \\ 0 & 0 & 0 \end{bmatrix}, \quad C = \begin{bmatrix} I\_3 & 0 & 0 \end{bmatrix}, \quad K = \begin{bmatrix} K\_{pp} \\ K\_{vp} \\ K\_{\varphi\_P} \end{bmatrix}. \tag{27}$$

The translational motion observer is similar to the EKF, and the gain matrix *K* can 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.
