**4. Experimental verification**

*3.2.2. Translational motion observer*

126 Recent Advances in Robotic Systems

motion observer is described by

stability.

with

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

> ˆ ˆ ( ), ˆ *ee e e pp GNSS pv Kp p* =+ q

ˆ <sup>2</sup> ˆˆˆ ˆ 2 ( ) ( ) ( ), *e ee e e e e e ie <sup>r</sup> vp GNSS v S v f gp K p p*=- + + + -

> ˆ () . ˆ *e eb <sup>b</sup> f Rq f* = +

 sq

<sup>3</sup> ( ) ( ) ( ), ˆ ˆ ˆ *e b ee Rq S f K p p <sup>b</sup>* x*p GNSS*

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

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

*I K*

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

é ù é ù ê ú ê ú ê ú ê ú ê ú ê ú ë û ë û

 q

x

w

3

0 0

0 0

0

suppression and gives faster convergence.

3 3

0 0 , [ 0 0],

*A I CI K K*

x

& (23)

& (24)

= - +- & (25)

(26)

.

*pp vp p*

*K*x

= == (27)

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 consid‐ ered 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*3, with *Q*EKF = blkdiag (03, 0.0962*I*3, 0.0761 · 10−4 *I*2, 0.3047 · 10−4, 3.0462 · 10−10 *I*3). 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*<sup>2</sup> *b* and *v*<sup>2</sup> *e* in Eq. (22) with *v*<sup>2</sup> *<sup>b</sup>* <sup>=</sup> 1;0;0 and *v*<sup>2</sup> *<sup>e</sup>* =*v* ^*e* / *v* ^*<sup>e</sup>* 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*<sup>b</sup> = 0.0087, while the remaining parameters are *k*1 = 0.2, *k*2 = 0.05, *θ* = 1, *kI* = 0.00005. The fixed gains are *K*pp = 0.38*I*3, *K*vp = 0.44 *I*3 and *K*ξp = 0.14 *I*3. 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.

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

trajectories, ensuring uniform semi-global exponential stability through [72]. For flights including numerous turns, a magnetometer might be preferred as loitering, and cross-winds

The nonlinear observers include bound parameters which should be chosen sufficiently large *M*<sup>b</sup> = 0.0087, while the remaining parameters are *k*1 = 0.2, *k*2 = 0.05, *θ* = 1, *kI* = 0.00005. The fixed gains are *K*pp = 0.38*I*3, *K*vp = 0.44 *I*3 and *K*ξp = 0.14 *I*3. For the NLO-Vel, the attitude injection gain

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

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

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

three state estimators equally and thoroughly to keep the comparison fair.

could affect the heading assumption.

is substituted for *k*2v = 0.01.

128 Recent Advances in Robotic Systems

**4.2. Results**

to 5 Hz.

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

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

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

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

130 Recent Advances in Robotic Systems

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

The trajectory of flight 1 is shown in **Figure 13**, covering an area of approximately 0.7 km2 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

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


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