Model-Based Autonomous Navigation with Moment of Inertia Estimation for Unmanned Aerial Vehicles

Abstract

The dominant navigation system for low-cost, mass-market Unmanned Aerial Vehicles (UAVs) is based on an Inertial Navigation System (INS) coupled with a Global Navigation Satellite System (GNSS). However, problems tend to arise during periods of GNSS outage where the navigation solution degrades rapidly. Therefore, this paper details a model-based integration approach for fixed wing UAVs, using the Vehicle Dynamics Model (VDM) as the main process model aided by low-cost Micro-Electro-Mechanical Systems (MEMS) inertial sensors and GNSS measurements with moment of inertia calibration using an Unscented Kalman Filter (UKF). Results show that the position error does not exceed 14.5 m in all directions after 140 s of GNSS outage. Roll and pitch errors are bounded to 0.06 degrees and the error in yaw grows slowly to 0.65 degrees after 140 s of GNSS outage. The filter is able to estimate model parameters and even the moment of inertia terms even with significant coupling between them. Pitch and yaw moment coefficient terms present significant cross coupling while roll moment terms seem to be decorrelated from all of the other terms, whilst more dynamic manoeuvres could help to improve the overall observability of the parameters.

1. Introduction

The dominant navigation system for low-cost, mass-market Unmanned Aerial Vehicles (UAVs) is based on Inertial Navigation System (INS)/Global Navigation Satellite System (GNSS) integration [1,2,3,4,5]. However, problems tend to arise during GNSS outages where the navigation solution error grows unboundedly [6,7,8,9]. This can happen due to intentional or unintentional corruption, even against cryptographically secured GNSS signals [10], rapid dynamics [11], loss of line of sight, and interference [12].

Rapid drift of the navigation solution during GNSS outages has been investigated extensively. Some authors have proposed using additional aiding exteroceptive sensors such as cameras [7,13,14] and range finders [1,7,15], which other than adding extra weight and cost, suffer from inherent limitations due to dependency on external sensing. Others have explored advanced integration schemes [2,3,11], and others have investigated advanced error modelling schemes [9,16], saving on weight but introducing additional software complexities.

More recently, research has been conducted on the inclusion of the Vehicle Dynamics Model (VDM) in providing a bounded navigation solution during periods of extended GNSS outages [15,17,18,19,20]. This approach preserves system autonomy while avoiding the addition of extra weight, cost, and power, essential for low-cost applications.

The use of both GNSS and Inertial Measurement Unit (IMU) measurements in aiding a VDM during extended GNSS outages with direct estimation of moment of inertia terms using an Unscented Kalman Filter (UKF) has not been explored before. Therefore, a model-based integration approach is proposed in this paper with the aforementioned architecture. The approach offers improved consistency and efficiency in the estimation of the navigation solution and model parameter terms, especially during periods of extended GNSS outage. The performance of the architecture is evaluated by a Monte Carlo simulation study with a predefined trajectory and a variable wind profile, assuming the use of commercial off-the shelf low-quality Micro-Electro-Mechanical Systems (MEMS) inertial sensors. The assessment is made in terms of navigation accuracy and filter consistency, especially during periods of extended GNSS outage.

Section 2 details current available solutions in using the VDM for navigation and the proposed architecture, focusing on similarities and differences with the available solutions. Section 3 details the performance assessment of the proposed architecture for a fixed wing UAV, highlighting the coordinate frame, equations of motion and filtering methodology. Section 4 details simulation results and the discussion of the results, and Section 5 concludes the report and highlights future work.

2. Solutions

A detailed description of the available solutions follows, and the proposed concept will be explained in the ensuing section, focusing on the differences and similarities with the presented solutions.

2.1. Available Solutions

Research explores two main concepts in using the VDM for navigation, namely, model-aided and model-based navigation. Model-aided navigation employs an INS as the main process model and a VDM as an aiding tool [17,18,19,21]. Model-based is the less common, more recent approach that uses a VDM as the main process model and an INS as the aiding system [20,22,23]. Authors have reported a similar level of accuracy with different computational efficiencies in the two approaches and some have considered multi-process model architectures, but the final solution is still derived from an INS.

Koifman and Bar-Itzhack [17] presented one of the early works in using the aircraft dynamics model to aid an INS using an Extended Kalman Filter (EKF). With perfectly known dynamics, the study showed that position error for the aided INS was relatively low during the entire flight as opposed to the pure INS case. In the presence of parameter uncertainties, state vector augmentation to include model parameters was found to improve navigation performance. Further, it was indicated that, slalom manoeuvres improved observability of different modes. The integration approach considered both the aircraft dynamics model and the INS at the same level in a multi-process model as shown in Figure 1.

A multi-process model approach is inherently computationally intensive as it introduces duplicate states. Additionally, the authors indicated using low-grade inertial sensors, but the presented error stochastics suggest high-end sensors.

Julier and Hugh [24] investigated the role of vehicle process models in sensor-based navigation systems for autonomous land vehicles using an EKF. Using a high-fidelity model of a high-speed automated ground vehicle implemented in the multibody dynamics simulation ADAMS software [24], the study showed that higher-order models suffer from observability problems in VDM parameters but imposing weak constraints helps to mitigate the problem. The authors show that the error between the true vehicle and the process model manifests itself in terms of a penalty which must be applied to the process noise covariance and nature of this penalty is time varying. It was shown that modest changes to the process model reduced orientation errors by 90% and position errors by 40%. The authors focused on land vehicles using constraints in their process model which are not directly applicable to a fixed wing aircraft. However, the general principles and deductions highlight the importance of a VDM in improved navigation performance.

Bryson and Sukkarieh [18] investigated the use of the VDM in aiding position, velocity, and orientation estimates provided by an INS with low-cost inertial sensors for a fixed wing UAV using an EKF. Two approaches were considered, as seen in Figure 2. The first approach compared and corrected the velocity and attitude, as predicted by both the INS and VDM. The second approach used the VDM predicted acceleration and rotation rates to provide direct calibration of the IMU. In both configurations, the INS formed the main process model and VDM aiding was activated during a GNSS outage.

With a 5% parameter uncertainty, the east position error was below 100 m for configuration 1 and above 800m for configuration 2 after 50 s of GNSS outage, indicating the superior performance of the first configuration. The good performance in configuration 1 was attributed to the marginal error growth in velocity and attitude that can be estimated and rejected with greater ease than the rapid error dynamics in acceleration and rotation rates. Both configurations did not include the direct estimation of wind and online parameter calibration and the final navigation solution was still dependent on an INS, which would be disabled in case of IMU failure.

Vissière et al. [25] reported the successful hovering flight of a model helicopter with low-cost inertial sensors by the addition of an accurate dynamics model, which improved the prediction of the EKF. Dassault Systèmes’s CATIA software was used to model 688 different parts in order to obtain the inertia matrix and centre of gravity position [25]. An autonomous outdoor flight under a 20km/h wind showed bounded position errors to within 1m vertically and 3 m horizontally. Attitude errors remained bounded to within 3 degrees in roll and pitch and 15 degrees in yaw. However, the architecture did not include a mechanism for online inertia calibration, instead CATIA was used for this purpose which can be time consuming.

Dadkhah et al. [26] investigated the use of model-aiding from knowledge of the dynamics of a helicopter to aid an Attitude Heading Reference System (AHRS) using low-cost rate gyros using an EKF. The helicopter dynamics model was developed using frequency domain system identification using attitude and position information gathered from 6 high-speed MX-40 cameras [26]. The authors argued that parametric errors in the EKF measurement stream resulting from the VDM were the main cause of suboptimal performance in gyro bias estimates. They also argued that state vector augmentation to account for correlation would improve the solution. The online calibration of model parameters was not considered even though the authors mention the potential benefits of such capability. Wind is neither estimated directly nor modelled as an external unknown in the system design in which the final navigation solution is still dependent on an INS.

Vasconcelos et al. [15] implements an embedded INS with VDM for a model helicopter using low-cost inertial sensors. The approach used both error states and total states, as can be seen in Figure 3. The execution time of the embedded VDM was 400 s, 26.3% lower with both angular and linear velocity aiding, and 310 s, 42.9% lower with only linear velocity aiding, as opposed to the external VDM aiding. However, both external and embedded VDM aiding where computationally intensive as opposed to the classical INS/GNSS with the external VDM aiding execution time being 85.32% greater than the classical INS/GNSS. Both the embedded and external VDM presented similar levels of accuracy. However, the use of both total and error states presents a complex integration approach.

Crocoll et al. [19] investigates a unified INS and VDM using a modified EKF by incorporating two valid state predictions achieving a reduced state vector size and computational load over the classical VDM aiding. It was shown analytically that inclusion of position states as pseudo-measurements led to filter divergence due to zero process noise. Making the velocity and orientation states relatively equal and with a higher update rate improved the accuracy of the navigation solution. It was shown that the accuracy of the unified approach is similar to that presented in Koifman and Bar-Itzhack [17]. The ability for online parameter calibration [27] and wind estimation [28] investigated at a later stage, revealed significant navigation performance improvements. Their model, however, was for a quadrotor and ignored rotational dynamics.

Sendobry [22] completely avoids the use of duplicate states by propagating the state vector using the VDM. The state vector is augmented to include vehicle accelerations in the EKF and applied to a quadrotor. The quadrotor propulsion model was parameterised through wind tunnel experiments. An experimental investigation showed the robustness of the proposed solution using a ground vehicle. The position solution showed a drift-free navigation performance near buildings where the raw GNSS solution presented erroneous measurements and sometimes even total outage. The architecture, however, was not investigated during periods of extended GNSS outage and simulation studies were based on a quadrotor and did not consider a fixed wing aircraft.

Khaghani and Skaloud [20,29] present an extension to the model-based approach presented by Sendobry [22,30] with specific application to fixed wing UAVs. Simulation results indicated 2 orders of magnitude improvement in navigation performance from the standard INS/GNSS integration using an EKF. The approach included the estimation of wind and model parameters but did not include the online calibration of moment of inertia terms and an account of the influence of their perturbation on navigation performance was not made. In further developments, experimental results indicated attitude errors of a VDM/GNSS integration being 1 to 2 orders of magnitude larger than the conventional INS/GNSS integration [31]. This was attributed mostly to unresolved errors in the moment terms that resulted to poor attitude determination in the absence of IMU data [29].

Zahran et al. [32] investigates the use of hybrid machine learning to train a VDM and enhance inertial navigation accuracy during periods of GNSS outage using low-cost inertial sensors in a quadcopter. The machine learning module (regression and classification) acts as a substitute, providing a position and velocity solution during periods of GNSS outage. An EKF with 21 states is used as the fusion filter with regression and classification schemes resulting in lower position errors during GNSS outages, as opposed to using a regression only scheme. The model, however, did not include a mechanism for online inertia tensor estimation during a GNSS outage. Further, training of the VDM happens only during periods of GNSS availability.

Mohammadkarimi and Nobahari [33], investigated a model-aided inertial navigation approach during landing of a UAV. The algorithm estimates and removes the Ground Effect uncertainties during the last phase of landing in close proximity to the ground. The architecture utilises both a Kalman filter (KF) and an UKF for INS state propagation and VDM state propagation, respectively. With perturbations in model parameters the integration architecture is seen not to handle estimation of a variable wind profile without an air data system. Moreover, the model included duplicate states and did not include a mechanism for online inertia estimation.

2.2. Proposed Concept

As mentioned earlier, a model-based integration approach is proposed that includes the estimation of wind, IMU errors, model parameters and moment of inertia terms. This is achieved using direct IMU and GNSS measurements. The key concept with a model-based approach is to use available control inputs from the autopilot system to drive the navigation solution using the dynamics model of the aircraft. A feature unique to the proposed architecture is the online estimation of moment of inertia terms without additional sensors and hardware setup in a fixed wing UAV. This improves not only the confidence in the navigation solution but also allows to meet stringent size, weight, and power requirements in low-cost applications. The architecture is platform-specific and therefore requires careful modelling.

The proposed architecture is preferred over the model-aided architectures [18,26,32] for a number of reasons. In low-cost applications, the quality of the inertial sensors used is relatively low and inherently affected by significant noise sources. In case of a GNSS outage, the noise in these sensors will cause rapid drift in the navigation solution in a short time and, in case of an IMU failure, the navigation solution is disabled altogether. Further, the use of direct IMU measurements to drive the navigation solution can become unreliable in the presence of significant thermal loading unless thermal models [16] are used to eliminate stochastic variations with temperature. Accurate IMU error modelling requires considerable time and effort and, in some cases, special equipment. This is avoided in the proposed architecture since the VDM is unaffected by thermal loading and uses the IMU and GNSS measurements only to update the navigation solution. Moreover, INS-dependent solutions are generally affected by secondary effects such as coning and sculling [12,15,25] as a result of vibrations on the host platform which, if not compensated, can cause significant drift in the navigation solution. The VDM-derived solution is unaffected by the platform’s vibrations making the architecture considerably robust. Multi-process models [17], in which the final navigation solution depends on the INS, have similar limitations and therefore the proposed approach is preferred over them as well.

The proposed approach is also preferred over the more recent model-based schemes [20,22] even though similarities exist in using the VDM as the main process model. Full-scale oscillation tests or other related inertia modelling schemes take considerable time and require special equipment. The proposed concept does not rely on an accurate inertia matrix allowing the estimation of the terms in flight. This minimizes the effort in system design and improves confidence in the derived navigation solution, especially in the presence of perturbations. Moreover, the process model adopted allows for the variation of these terms during the course of a flight and even though not investigated in this work, could improve the robustness of a model-based approach in applications where mass and moment of inertia could change allowing for a more general application of the scheme.

3. Performance Assessment

This section details the performance assessment of a model-based navigation architecture with the VDM as the main process model, aided by the IMU and GNSS measurements using a UKF. A description of the coordinate frame, atmospheric model, equations of motion and filtering methodology follows.

3.1. Coordinate Frame

A local navigation frame initialised at the position of the airplane’s centre of gravity just before take-off is treated as an inertial frame. The body-fixed frame defines a right-handed orthogonal coordinate frame with the origin at the centre of gravity of the aircraft, as can be seen in Figure 4.

Inertial speed in the body frame (${\mathrm{v}}^{\mathrm{b}}$) is given as the sum of wind speed in the local navigation frame (${\mathrm{W}}^{\mathrm{n}}$) transformed to the body frame and airspeed vector in the body frame (${\mathrm{V}}^{\mathrm{b}}$).

$${\mathrm{v}}^{\mathrm{b}}{ = \mathrm{V}}^{\mathrm{b}}{+ \mathrm{R}}_{\mathrm{n}}^{\mathrm{b}}{\mathrm{W}}^{\mathrm{n}}$$

(1)

where ‘${\mathrm{R}}_{\mathrm{n}}^{\mathrm{b}}$’ is the rotation matrix from the local level frame to the body frame.

The airspeed, angle of attack ($\mathsf{\alpha }$), sideslip angle ($\mathsf{\beta }$), and dynamic pressure ($\overline{\mathrm{q}}$ ) are represented as:

$$\begin{gathered} {\mathrm{V}}^{\mathrm{b}}=\sqrt{{\mathrm{u}}_{\mathrm{T}}^2 + {\mathrm{v}}_{\mathrm{T}}^2+ {\mathrm{w}}_{\mathrm{T}}^2} , \\ \mathsf{\alpha } = \arctan \left(\frac{{\mathrm{w}}_{\mathrm{T}}}{{\mathrm{u}}_{\mathrm{T}}}\right), \mathsf{\beta } = \arcsin \left(\frac{{\mathrm{v}}_{\mathrm{T}}}{{\mathrm{V}}^{\mathrm{b}}}\right), \\ \overline{\mathrm{q} }= \frac{{\mathsf{\rho }\mathrm{V}}^{{\mathrm{b}}^2}}{2} \end{gathered}$$

(2)

where, ‘${\mathrm{u}}_{\mathrm{T}}$’, ‘${\mathrm{v}}_{\mathrm{T}}$’ and ‘${\mathrm{w}}_{\mathrm{T}}$’ represent the components of the airspeed vector in the body frame.

3.2. Atmospheric Model

The atmospheric model adopted for use is modelled in accordance with the international standard atmosphere [34,35] given by:

$$\begin{gathered} {\mathrm{T} = \mathrm{T}}_0{[1 + \mathrm{ah}/\mathrm{T}}_0] \\ \mathsf{\rho } = \frac{{\mathrm{p}}_{\mathrm{o}}{\left[{1 + \mathrm{ah}/\mathrm{T}}_0\right]}^{5.2561}}{\mathrm{RT}} \end{gathered}$$

(3)

where ‘$\mathsf{\rho }$’ is the density, ‘$\mathrm{R}$’ is the specific gas constant, ‘$\mathrm{To}$’ is the sea level temperature, ‘${\mathrm{p}}_{\mathrm{o}}$’ is the sea level pressure, and ‘$\mathrm{a}$’ is the lapse rate [34]. This atmospheric model is valid up to an altitude of 11,000 m and therefore can be used to capture the relatively small altitude variations considered in simulation.

3.3. Equations of Rigid-Body Motion

For the purpose of simulation, it is assumed that the aircraft is flying over a small region of the earth and thus the earth is assumed locally flat (${\mathrm{R}}_{\mathrm{earth}}\to \infty$) and therefore, centripetal acceleration resulting from earth’s curvature is neglected. Coriolis acceleration due to rotation of the earth is neglected and thus the earth is treated as an inertial (Galilean) frame of reference, where Newton’s laws are applicable. The equations of motion are presented as:

$$\begin{gathered} \left[\begin{array}{c}\dot{{\mathrm{x}}_{\mathrm{N}}} \\ \dot{{\mathrm{x}}_{\mathrm{E}}} \\ \dot{{\mathrm{x}}_{\mathrm{D}}}\end{array}\right]{= \mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}\left[\begin{array}{c}{\mathrm{v}}_{\mathrm{x}}^{\mathrm{b}} \\ {\mathrm{v}}_{\mathrm{y}}^{\mathrm{b}}\\ { \mathrm{v}}_{\mathrm{z}}^{\mathrm{b}}\end{array}\right], \\ {\mathrm{R}}_{\mathrm{b} }^{\mathrm{n}}= \left[\begin{array}{ccc}\cos \mathsf{\psi }\cos \mathsf{\theta }& \cos \mathsf{\psi }\sin \mathsf{\varphi }\sin \mathsf{\theta }-\cos \mathsf{\varphi }\sin \mathsf{\psi }& \sin \mathsf{\varphi }\sin \mathsf{\psi }+\cos \mathsf{\varphi }\cos \mathsf{\psi }\sin \mathsf{\theta }\\ \cos \mathsf{\theta }\sin \mathsf{\psi }& \cos \mathsf{\varphi }\cos \mathsf{\psi }+\sin \mathsf{\varphi }\sin \mathsf{\psi }\sin \mathsf{\theta }& \cos \mathsf{\varphi }\sin \mathsf{\psi }\sin \mathsf{\theta }-\cos \mathsf{\psi }\sin \mathsf{\varphi }\\ -\sin \mathsf{\theta }& \cos \mathsf{\theta }\sin \mathsf{\varphi }& \cos \mathsf{\varphi }\cos \mathsf{\theta }\end{array}\right] \end{gathered}$$

(4)

where ‘${\mathrm{x}}_{\mathrm{N}}{, \mathrm{x}}_{\mathrm{E}}{, \mathrm{x}}_{\mathrm{D}}$’ represent position in the North, East, and Down direction, ‘${\mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}$’is the rotation matrix from the body frame to the local level frame, and ‘$\mathsf{\varphi }, \mathsf{\theta }, \mathsf{\psi }$’ represent the Euler angles: roll, pitch, and yaw, respectively.

$$\begin{gathered} \left[\begin{array}{c}\dot{{\mathrm{v}}_{\mathrm{x}}^{\mathrm{b}}} \\ \dot{{\mathrm{v}}_{\mathrm{y}}^{\mathrm{b}}}\\ \dot{{\mathrm{v}}_{\mathrm{z}}^{\mathrm{b}}}\end{array}\right]=\left[\begin{array}{c}-\mathrm{gsin}\mathsf{\theta }\\ \mathrm{gsin}\mathsf{\varphi }\cos \mathsf{\theta }\\ \mathrm{gcos}\mathsf{\varphi }\cos \mathsf{\theta }\end{array}\right]+\frac{1}{\mathrm{m}}\left[\left(\begin{array}{c}{\mathrm{F}}_{\mathrm{T}}\\ 0\\ 0\end{array}\right){+\mathrm{R}}_{\mathrm{w}}^{\mathrm{b}}\left(\begin{array}{c}{\mathrm{F}}_{\mathrm{X}}^{\mathrm{w}}\\ {\mathrm{F}}_{\mathrm{Y}}^{\mathrm{w}}\\ {\mathrm{F}}_{\mathrm{Z}}^{\mathrm{w}}\end{array}\right)\right]-\left[\begin{array}{c}{\mathsf{\omega }}_{\mathrm{y}}{\mathrm{v}}_{\mathrm{z} }^{\mathrm{b}}{- \mathsf{\omega }}_{\mathrm{z}}{\mathrm{v}}_{\mathrm{y}}^{\mathrm{b}}\\ {\mathsf{\omega }}_{\mathrm{z}}{\mathrm{v}}_{\mathrm{x}}^{\mathrm{b}}{ - \mathsf{\omega }}_{\mathrm{x}}{\mathrm{v}}_{\mathrm{z}}^{\mathrm{b}}\\ {\mathsf{\omega }}_{\mathrm{x}}{\mathrm{v}}_{\mathrm{y}}^{\mathrm{b}}{ - \mathsf{\omega }}_{\mathrm{y}}{\mathrm{v}}_{\mathrm{x}}^{\mathrm{b}}\end{array}\right] \\ {\mathrm{R}}_{\mathrm{w}}^{\mathrm{b}} = \left[\begin{array}{ccc}\cos \mathsf{\alpha }\cos \mathsf{\beta }& -\cos \mathsf{\alpha }\sin \mathsf{\beta }& -\sin \mathsf{\alpha }\\ \sin \mathsf{\beta }& \cos \mathsf{\beta }& 0\\ \cos \mathsf{\beta }\sin \mathsf{\alpha }& -\sin \mathsf{\alpha }\sin \mathsf{\beta }& \cos \mathsf{\alpha }\end{array}\right] \end{gathered}$$

(5)

where ‘m’ is the mass and ‘${\mathrm{R}}_{\mathrm{w}}^{\mathrm{b}}$’ is the rotation matrix from the wind frame to the body frame. ‘${\mathrm{F}}_{\mathrm{T}}$’ is the thrust force along the body x-axis, ‘${\mathrm{F}}_{\mathrm{Z}}^{\mathrm{w}}$’ is the lift force in the wind frame, ‘${\mathrm{F}}_{\mathrm{Y}}^{\mathrm{w}}$’ is the lateral force, and ‘${\mathrm{F}}_{\mathrm{X}}^{\mathrm{w}}$’ is the drag force in the wind frame and ‘$\mathrm{g}$’ is the acceleration due to gravity.

$$\left[ \begin{array}{c}\dot{\mathsf{\varphi }}\\ \dot{\mathsf{\theta }}\\ \dot{\mathsf{\psi }}\end{array}\right]={\mathrm{R}}_{\mathsf{\varphi }}\left[ \begin{array}{c}{\mathsf{\omega }}_{\mathrm{x}}\\ {\mathsf{\omega }}_{\mathrm{y}}\\ {\mathsf{\omega }}_{\mathrm{z}}\end{array}\right], {\mathrm{R}}_{\mathsf{\varphi }} = \left[\begin{array}{ccc}1& \tan \mathsf{\theta }\sin \mathsf{\varphi } & \tan \mathsf{\theta }\cos \mathsf{\varphi }\\ 0& \cos \mathsf{\varphi }& -\sin \mathsf{\varphi }\\ 0& \sin \mathsf{\varphi }/\cos \mathsf{\theta }& \cos \mathsf{\varphi }/\cos \mathsf{\theta } \end{array}\right]$$

(6)

where ‘${\mathsf{\omega }}_{\mathrm{x}}{, \mathsf{\omega }}_{\mathrm{y}}{, \mathsf{\omega }}_{\mathrm{z}}$’ represent the roll, pitch, and yaw angular rates around the respective body axes.

$$\begin{gathered} \left[ \begin{array}{c}\dot{{\mathsf{\omega }}_{\mathrm{x}}}\\ \dot{{\mathsf{\omega }}_{\mathrm{y}}}\\ \dot{{\mathsf{\omega }}_{\mathrm{z}}}\end{array}\right] = {\left({\mathrm{I}}^{\mathrm{b}}\right)}^{-1}\left(\left[\begin{array}{c}{\mathrm{M}}_{\mathrm{x}}^{\mathrm{b}}\\ { \mathrm{M}}_{\mathrm{y}}^{\mathrm{b}} \\ {\mathrm{M}}_{\mathrm{z}}^{\mathrm{b}}\end{array}\right]-\left[\begin{array}{c}{\mathsf{\omega }}_{\mathrm{x}}\\ {\mathsf{\omega }}_{\mathrm{y}}\\ {\mathsf{\omega }}_{\mathrm{z}}\end{array}\right]{\times \mathrm{I}}^{\mathrm{b}}\left[\begin{array}{c}{\mathsf{\omega }}_{\mathrm{x}}\\ {\mathsf{\omega }}_{\mathrm{y}}\\ {\mathsf{\omega }}_{\mathrm{z}}\end{array}\right]\right), \\ {\mathrm{I}}^{\mathrm{b}} = \left[\begin{array}{ccc}{\mathrm{I}}_{\mathrm{xx}}& 0& {\mathrm{I}}_{\mathrm{xz}}\\ 0& {\mathrm{I}}_{\mathrm{yy}}& 0\\ {\mathrm{I}}_{\mathrm{zx}}& 0& {\mathrm{I}}_{\mathrm{zz}}\end{array}\right] \end{gathered}$$

(7)

where ‘${\mathrm{M}}_{\mathrm{x}}^{\mathrm{b}}{, \mathrm{M}}_{\mathrm{y}}^{\mathrm{b}}{, \mathrm{M}}_{\mathrm{z}}^{\mathrm{b}}$’ represent the roll, pitch, and yaw moments around the body axes respectively, and ‘${\mathrm{I}}^{\mathrm{b}}$’ represents the body-fixed inertia matrix with ‘${\mathrm{I}}_{\mathrm{xx}}{, \mathrm{I}}_{\mathrm{yy}}{, \mathrm{I}}_{\mathrm{zz}}{, \mathrm{I}}_{\mathrm{xz}}$’ representing the components of the inertia matrix about the respective axes.

The propeller first-order dynamics model is given by:

$$\dot{\mathrm{n}}= -\frac{1}{{\mathsf{\tau }}_{\mathrm{n}}}\mathrm{n} + \frac{1}{{\mathsf{\tau }}_{\mathrm{n}}}{\mathrm{n}}_{\mathrm{c}}$$

(8)

where ‘${\mathrm{n}}_{\mathrm{c}}$’ represents the commanded propeller speed and ‘${\mathsf{\tau }}_{\mathrm{n}}$’ is the time constant. Other actuators could also be modelled with similar dynamics but for simplicity this is not considered.

In developing the equations of motion, mass ($\mathrm{m}$) and moments of inertia (${\mathrm{I}}^{\mathrm{b}}$) are assumed constant. The aerodynamic forces (${\mathrm{F}}_{\mathrm{X}}^{\mathrm{w}}{,\mathrm{F}}_{\mathrm{Y}}^{\mathrm{w}}{, \mathrm{F}}_{\mathrm{Z}}^{\mathrm{w}}$) and thrust force (${\mathrm{F}}_{\mathrm{T}}$) are represented as:

$${\mathrm{F}}_{\mathrm{Z}}^{\mathrm{w}}=\overline{\mathrm{q}}\mathrm{S}({\mathrm{CF}}_{\mathrm{Z}1}+{\mathrm{CF}}_{{\mathrm{Z}}_{\mathsf{\alpha }}}\mathsf{\alpha })$$

(9)

$${\mathrm{F}}_{\mathrm{Y}}^{\mathrm{w}}=\overline{\mathrm{q}}\mathrm{S}{\mathrm{CF}}_{\mathrm{Y}1}\mathsf{\beta }$$

(10)

$${\mathrm{F}}_{\mathrm{X}}^{\mathrm{w}}=\overline{\mathrm{q}}\mathrm{S}({\mathrm{CF}}_{\mathrm{X}1}+{\mathrm{CF}}_{{\mathrm{X}}_{\mathsf{\alpha }}}\mathsf{\alpha }+{\mathrm{CF}}_{{\mathrm{X}}_{\mathsf{\alpha }2}}{\mathsf{\alpha }}^2+{\mathrm{CF}}_{{\mathrm{X}}_{\mathsf{\beta }2}}{\mathsf{\beta }}^2)$$

(11)

$${\mathrm{F}}_{\mathrm{T}}{ = \mathsf{\rho }\mathrm{n}}^2{\mathrm{D}}^4{(\mathrm{CF}}_{{\mathrm{T}}_1}{+\mathrm{CF}}_{{\mathrm{T}}_2}{\mathrm{J} + \mathrm{CF}}_{{\mathrm{T}}_3}{\mathrm{J}}^2 )$$

(12)

where, $\mathrm{J} = \frac{\mathrm{V}}{\mathrm{D}\mathsf{\pi }\mathrm{n}}$ and ‘$\mathrm{n}$’ is the propeller speed, ‘${\mathrm{CF}}_{{\left[\mathrm{X} \mathrm{Y} \mathrm{Z}\right]}_{\mathrm{j}}}$’ are the aerodynamic force derivatives and ‘${\mathrm{CF}}_{{\mathrm{T}}_{\mathrm{i}=[1 2 3]}}$’ are the thrust derivatives, ‘$\mathrm{S}$’ is the wing area, and ‘$\mathrm{D}$’ is the propeller diameter.

The aerodynamic moments are represented as:

$$\begin{gathered} {\mathrm{M}}_{\mathrm{X}}^{\mathrm{b}}=\overline{\mathrm{q}}\mathrm{Sb}({\mathrm{CM}}_{{\mathrm{X}}_{{\mathsf{\delta }}_{\mathsf{\alpha }}}}{\mathsf{\delta }}_{\mathsf{\alpha }}+{\mathrm{CM}}_{{\mathrm{X}}_{{\overline{\mathsf{\omega }}}_{\mathrm{x}}}}{\overline{\mathsf{\omega }}}_{\mathrm{x}}+{\mathrm{CM}}_{{\mathrm{X}}_{{\overline{\mathsf{\omega }}}_{\mathrm{z}}}}{\overline{\mathsf{\omega }}}_{\mathrm{z}}+{\mathrm{CM}}_{{\mathrm{X}}_{\mathsf{\beta }}}\mathsf{\beta }) \\ {\overline{\mathsf{\omega }}}_{\mathrm{x}}=\frac{{\mathsf{\omega }}_{\mathrm{x}}\mathrm{b}}{2\mathrm{V}}, {\overline{\mathsf{\omega }}}_{\mathrm{z}}=\frac{{\mathsf{\omega }}_{\mathrm{z}}\mathrm{b}}{2\mathrm{V}} \end{gathered}$$

(13)

$$\begin{gathered} {\mathrm{M}}_{\mathrm{Y}}^{\mathrm{b}}=\overline{\mathrm{q}}\mathrm{S}\overline{\mathrm{c}}({\mathrm{CM}}_{{\mathrm{Y}}_{{\mathsf{\delta }}_{\mathrm{e}}}}{\mathsf{\delta }}_{\mathrm{e}}+{\mathrm{CM}}_{{\mathrm{Y}}_{{\overline{\mathsf{\omega }}}_{\mathrm{y}}}}{\overline{\mathsf{\omega }}}_{\mathrm{y}}+{\mathrm{CM}}_{{\mathrm{Y}}_{\mathsf{\alpha }}}\mathsf{\alpha }+{\mathrm{CM}}_{{\mathrm{Y}}_1}) \\ {\overline{\mathsf{\omega }}}_{\mathrm{y}}=\frac{{\mathsf{\omega }}_{\mathrm{y}}\overline{\mathrm{c}}}{2\mathrm{V}} \end{gathered}$$

(14)

$${\mathrm{M}}_{\mathrm{Z}}^{\mathrm{b}}=\overline{\mathrm{q}}\mathrm{Sb}({\mathrm{CM}}_{{\mathrm{Z}}_{{\mathsf{\delta }}_r}}{\mathsf{\delta }}_r+{\mathrm{CM}}_{{\mathrm{Z}}_{{\overline{\mathsf{\omega }}}_{\mathrm{z}}}}{\overline{\mathsf{\omega }}}_{\mathrm{z}}+{\mathrm{CM}}_{{\mathrm{Z}}_{\mathsf{\beta }}}\mathsf{\beta })$$

(15)

where, ‘$\mathrm{c}$’ is the wing cord, ‘$\mathrm{b}$’ the wing span, ‘${\mathrm{CM}}_{{\left[\mathrm{X} \mathrm{Y} \mathrm{Z}\right]}_{\mathrm{j}}}$’ are the moment derivatives, and ‘${\mathsf{\delta }}_{\mathsf{\alpha }}, {\mathsf{\delta }}_{\mathrm{e}}, {\mathsf{\delta }}_{\mathrm{r}}$’ are the aileron, elevator, and rudder deflections, respectively.

3.4. Filtering Methodology

An unscented Kalman filter [4,36,37] is used to provide an integrated navigation solution where the VDM forms the main process model and the inertial sensors and GNSS provide correcting measurements. As mentioned earlier, the state vector is augmented to include the vehicle navigation states (${\mathrm{X}}_{\mathrm{n}}$), IMU errors (${\mathrm{X}}_{\mathrm{e}}$), wind velocity (${\mathrm{X}}_{\mathrm{w}}$), and 22 VDM parameters and 4 moment of Inertia terms (${\mathrm{X}}_{\mathrm{P}}$). A UKF captures higher-order moments and avoids biases introduced from first-order linearization assumptions. The UKF avoids the derivation of the Jacobian matrices, a nontrivial aspect of a highly nonlinear system. A complete description of the UKF is not given here but the interested reader can see the referenced text.

3.4.1. Process models

The VDM states (${\mathrm{X}}_{\mathrm{n}}$) are propagated using the equations of motion already presented directly, without the need for any linearization.

IMU errors in the navigation filter are modelled as a random constant superposed in a random walk process given by:

$$\begin{gathered} {\dot{\mathrm{X}}}_{\mathrm{e}}={\mathrm{G}}_{\mathrm{ee}}{\mathrm{W}}_{\mathrm{e}} \\ {\mathrm{X}}_{\mathrm{e}} = {\left[{\mathrm{b}}_{\mathrm{ax}}{ \mathrm{b}}_{\mathrm{ay}}{ \mathrm{b}}_{\mathrm{az}}{ \mathrm{b}}_{\mathrm{gx}}{ \mathrm{b}}_{\mathrm{gy}}{ \mathrm{b}}_{\mathrm{gz}}\right]}^{\mathrm{T}} \\ {\mathrm{b}}_{\mathrm{a}\left|\mathrm{g}\right[\mathrm{x} \mathrm{y} \mathrm{z}]} :\mathrm{Accelerometer} \mathrm{and} \mathrm{Gyroscope} \mathrm{bias} \end{gathered}$$

(16)

where, ‘${\mathrm{G}}_{\mathrm{ee}}$’ represents the noise shaping matrix and ‘${\mathrm{W}}_{\mathrm{e}}$’ is the noise vector. The actual sensor model implementation follows a first-order Gauss–Markov (GM) process. Bias variations with temperature, quantization noise, and scale factors are not considered in the simulation.

$${\dot{\mathrm{X}}}_{\mathrm{e}}=-{\mathsf{\beta }}_{\mathrm{e}}{\mathrm{X}}_{\mathrm{e}}+{\mathrm{n}}_{{\mathrm{X}}_{\mathrm{e}}}$$

(17)

where, ‘${\mathrm{n}}_{\mathrm{Xe}}$’ represents the GM process driving noise and ‘${\mathsf{\beta }}_{\mathrm{e}}$’ is the inverse of the correlation time.

Wind velocity in the navigation filter is modelled as a random walk process to capture small transitions.

$$\begin{gathered} {\dot{\mathrm{X}}}_{\mathrm{w}}={\mathrm{G}}_{\mathrm{w}}{\mathrm{W}}_{\mathrm{w}} \\ {\mathrm{X}}_{\mathrm{w}} = {\left[{\mathrm{w}}_{\mathrm{N}}{ \mathrm{w}}_{\mathrm{E}}{ \mathrm{w}}_{\mathrm{D}}\right]}^{\mathrm{T}} \end{gathered}$$

(18)

where, ‘${\mathrm{G}}_{\mathrm{ww}}$’ represents the wind vector noise shaping matrix and ‘${\mathrm{W}}_{\mathrm{w}}$’ is the noise vector. The actual wind implementation in simulation follows a first order Gauss–Markov process with a constant component having a magnitude of 3.8 m/s, a correlation time of 200s, and process uncertainty of 0.1m/s.

Most of the VDM parameters and inertia terms are static (${\dot{\mathrm{X}}}_{\mathrm{p}}$) otherwise changes in mass distribution during flight could change the mass moment of inertia terms and therefore, for generality, the parameters are modelled as a random walk process with very small process noise to capture small transitions in time. It should be noted that during the simulation the parameters are fixed.

$$\mathrm{Xp} = [\begin{array}{c}{\mathrm{CF}}_{{\mathrm{T}}_1}{ \mathrm{CF}}_{{\mathrm{T}}_2}{ \mathrm{CF}}_{{\mathrm{T}}_3}{ \mathrm{CF}}_{{\mathrm{X}}_1}{ \mathrm{CF}}_{{\mathrm{X}}_{\mathsf{\alpha }}}{ \mathrm{CF}}_{{\mathrm{X}}_{\mathsf{\alpha }2} }{\mathrm{CF}}_{{\mathrm{X}}_{\mathsf{\beta }2}}{ \mathrm{CF}}_{{\mathrm{Z}}_1} \hfill \\ {\mathrm{CF}}_{{\mathrm{Z}}_{\mathsf{\alpha }}}{ \mathrm{CF}}_{{\mathrm{Y}}_1}{ \mathrm{CM}}_{{\mathrm{X}}_{{\mathsf{\delta }}_{\mathsf{\alpha }}}}{ \mathrm{CM}}_{{\mathrm{X}}_{\mathsf{\beta }}}{ \mathrm{CM}}_{{\mathrm{X}}_{{\overline{\mathsf{\omega }}}_{\mathrm{x}}}}{ \mathrm{CM}}_{{\mathrm{X}}_{{\overline{\mathsf{\omega }}}_{\mathrm{z}}}} {\mathrm{CM}}_{{\mathrm{Y}}_1} \hfill \\ {\mathrm{CM}}_{{\mathrm{Y}}_{\mathsf{\alpha }}}{ \mathrm{CM}}_{{\mathrm{Y}}_{{\mathsf{\delta }}_{\mathrm{e}}}}{ \mathrm{CM}}_{{\mathrm{Y}}_{{\overline{\mathsf{\omega }}}_{\mathrm{y}}}}{ \mathrm{CM}}_{{\mathrm{Z}}_{{\mathsf{\delta }}_{\mathrm{r}}}}{ \mathrm{CM}}_{{\mathrm{Z}}_{{\overline{\mathsf{\omega }}}_{\mathrm{z}}}} {\mathrm{CM}}_{{\mathrm{Z}}_{\mathsf{\beta }}} {\mathsf{\tau }}_{\mathrm{n}}\hfill \\ {\mathrm{I}}_{\mathrm{XX}}{ \mathrm{I}}_{\mathrm{YY}}{ \mathrm{I}}_{\mathrm{ZZ}}{ \mathrm{I}}_{\mathrm{XZ}}\end{array}]$$

3.4.2. Observation Model

The measurement vector consists of IMU (${\mathrm{Z}}_{\mathrm{imu}}$) and GNSS receiver (${\mathrm{Z}}_{\mathrm{gnss}}$) measurements presented as:

$$\mathrm{Z} = {\left[{\mathrm{Z}}_{\mathrm{imu}}{ \mathrm{Z}}_{\mathrm{gnss}}\right]}^{\mathrm{T}}$$

(19)

The discrete measurement model (${\mathrm{z}}_{\mathrm{k}}$) is a function of the measurement function ($\mathrm{h}$) presented as:

$${\mathrm{z}}_{\mathrm{k}} = \mathrm{h}\left[{\mathrm{x}}_{\mathrm{k}}{, \mathrm{u}}_{\mathrm{k}}\right]{+\mathrm{r}}_{\mathrm{k}}$$

(20)

where ‘${\mathrm{x}}_{\mathrm{k}}$’ is the predicted state vector at the current time index ‘k’, ‘${\mathrm{u}}_{\mathrm{k}}$’ is the known control input vector at the current time index and, ‘${\mathrm{r}}_{\mathrm{k}}$’ is the white sequence vector. The expectation operator on the white sequence and its transpose is given by:$\mathrm{E}\left[{\mathrm{r}}_{\mathrm{k}}{\mathrm{r}}_{\mathrm{k}}^{\mathrm{T}}\right]{ = \mathrm{R}}_{\mathrm{k}}$. ‘${\mathrm{R}}_{\mathrm{k}}$’ is the measurement covariance matrix.

The measurement function for the IMU (${\mathrm{h}}_{\mathrm{imu}}$) is presented as:

$$\begin{gathered} {\mathrm{h}}_{\mathrm{imu} }= \left[\begin{array}{c}{\mathrm{h}}_{\mathrm{accel}}\\ {\mathrm{h}}_{\mathrm{gyro}}\end{array}\right] =\left[\begin{array}{c}\dot{\mathrm{v}}-\mathrm{g}+[\mathsf{\omega }\times ]{\mathrm{v}}_{\left[3\times 1\right]}+ {\mathrm{X}}_{\mathrm{e}}\left(\left[1 2 3\right]\right)\\ {\mathsf{\omega }}_{\left[3\times 1\right] }{+ \mathrm{X}}_{\mathrm{e}}\left(\left[4 5 6\right]\right)\end{array}\right] \\ \left[\mathsf{\omega }\times \right] = \left[\begin{array}{ccc}0& {-\mathsf{\omega }}_{\mathrm{z}}& {\mathsf{\omega }}_{\mathrm{y}}\\ {\mathsf{\omega }}_{\mathrm{z}}& 0& {-\mathsf{\omega }}_{\mathrm{x}}\\ {-\mathsf{\omega }}_{\mathrm{y}}& {\mathsf{\omega }}_{\mathrm{x}}& 0\end{array}\right] \end{gathered}$$

(21)

where ${\mathsf{\omega }}_{\left[3 \times 1\right]} = {\left[{\mathsf{\omega }}_{\mathrm{x}}{ \mathsf{\omega }}_{\mathrm{y}}{ \mathsf{\omega }}_{\mathrm{z}}\right]}^{\mathrm{T}}$ and ${\mathrm{v}}_{\left[3 \times 1\right]} = {\left[{\mathrm{v}}_{\mathrm{x}}^{\mathrm{b}}{ \mathrm{v}}_{\mathrm{y}}^{\mathrm{b}}{ \mathrm{v}}_{\mathrm{z}}^{\mathrm{b}}\right]}^{\mathrm{T}}$. The measurement function for the GNSS receiver (${\mathrm{h}}_{\mathrm{gnss}}$) is presented as:

$${\mathrm{h}}_{\mathrm{gnss}} = \left[\begin{array}{c}{\mathrm{x}}_{\mathrm{N}}\\ {\mathrm{x}}_{\mathrm{E}}\\ {\mathrm{x}}_{\mathrm{D}}\end{array}\right]$$

(22)

3.4.3. Structure

The process model and observation model together are used in the implementation of the UKF, as can be seen in Figure 5. It should be noted that the states are propagated internally in the sigma points processing block.

The sigma points processing block has been expanded, as can be seen in Figure 6.

The update block outputs correction values for the states and consists of standard Kalman filter update routines after the mean and covariance have been estimated in the sigma point processing block. The interested user can refer to Gelb et al. [38] and Brown and Hwang [4] for a complete overview.

3.4.4. Implementation

Table 1. IMU error characteristics.

Property	Accelerometer	Gyroscope
Random bias ($\mathsf{\sigma }$)	$10 \mathrm{mg}$	$1000 °/\mathrm{hr}$
White noise ($\mathrm{PSD}$)	$100 \mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{Hz}}$	$21.6°/\mathrm{hr}/\mathrm{sqrt}\left(\mathrm{Hz}\right)$
First-order Gauss–Markov	$0.05 \mathrm{mg}$	$20 °/\mathrm{hr}$
Correlation Time ($\mathsf{\tau }$)	$200 \mathrm{s}$	$200 \mathrm{s}$
Sampling Frequency	$100 \mathrm{Hz}$	$100 \mathrm{Hz}$

presents a summary of the error characteristics implemented in simulation. The filter does not use the true error stochastics to maintain a situation close to reality that error stochastics cannot be precisely known but only approximated. Since the application is targeting low-cost applications, most of these stochastics, in an experimental implementation, would come either directly from Manufacturer specifications or characterisation techniques such as Allan Variance or Generalised Wavelet Moments.

For the GNSS receiver, independent white noise on each channel (north, east, down) is modelled with a standard deviation of 1$\mathrm{m}$ and the sampling frequency is 1 Hz.

The initial uncertainties considered for the augmented state vector are presented in

Table 2. Initial uncertainty [${\mathrm{P}}_{\mathrm{o}}$].

State	Standard Deviation $\left(\mathsf{\sigma }\right)$
Position	$1 \mathrm{m}$
Velocity	$[1, 0.5, 0.5] \mathrm{m}/\mathrm{s}$
Attitude	$[3.5°, 3.5°, 5°]$
Rotation rates	$1.5 °/\mathrm{s}$
Propeller speed	$15 \mathrm{rad}/\mathrm{s}$
Model parameters	$10\%$
Moment of Inertia	$10\%$

.

The process noise for the filter is presented in

Table 3. Process Noise.

State	Standard Deviation $\left(\mathsf{\sigma }\right)$
Position	${10}^{-6}$
Velocity	$0.008$
Attitude	${10}^{-6}$
Rotation rates	${10}^{-4}$
Propeller speed Accelerometer Bias Gyroscope Bias Wind	${10}^{-4}$ ${2 \times 10}^{-5}$ ${2 \times 10}^{-6}$ ${10}^{-3}$
Model parameters	$0.015\% \mathrm{of} \mathrm{True} \mathrm{Values}$
Moment of Inertia	$0.015\% \mathrm{of} \mathrm{True} \mathrm{Values}$

in terms of the standard deviations.

For a full list of aerodynamic coefficient, moments of inertia and their values considered for the purpose of simulation, the reader is directed to Ducard [35].

A hundred Monte Carlo runs were performed in Matlab to evaluate autonomous navigation performance and investigate the system robustness against random initialisation errors and sensor errors. Evaluating the sample statistics for different runs revealed that 60 Monte Carlo runs resulted in a precision, difference between the population mean and sample mean, of less than 1 metre in all the estimated position states with a 95% confidence level, as can be seen in Figure 7. With 60 runs, the precision for all the other states is well below the uncertainty bounds considered for the states. The combined standard deviation from different sample runs for each state is used in evaluating the precision for the state. Therefore, 100 Monte Carlo runs seemed reasonable in attaining a precision of less than 1 metre in the estimation of position states whilst guaranteeing similarly lower precision for all the other states and include a margin of safety to account for some of the underlying assumptions adopted.

The same trajectory is used for all of the scenarios and the wind profile is kept constant during the runs. The trajectory is presented in Figure 8, which includes a take-off segment which the autopilot system marks as complete at an altitude of 200 m, a climb segment which completes at 700 m, and a cruise segment where a GNSS outage is induced followed by a descent approach segment. The outage is induced 200 s into the flight and lasts for 140 s. The total flight time is 340 s. The impact of autopilot agility on navigation performance is not investigated in this research.

Control activity during the different flight segments is presented in Figure 9 with altitude used as a representative of the flight segment. The commands from the autopilot are logged at 100 Hz and used alongside simulated sensor data and processed with the developed algorithm.

4. Results

The accelerometer and gyroscope bias estimation error is presented in Figure 10. The filter is able to effectively and consistently estimate accelerometer and gyroscope bias errors within the first 50 s with GNSS presence. Ninety-eight percent of the initial turn-on gyroscope bias and bias variation are resolved well within 100 s of GNSS presence. The errors are also seen to be bounded during 140 s of GNSS outage owing to the inherent ability of the dynamics model to offer a bounded navigation solution during periods of GNSS outage. Also, the 1$\sigma$ prediction error is seen to be consistent with the true error, attributed to a correctness in filter setup.

Results indicate that the Z-axis and X-axis accelerometer bias are slightly delayed in their estimation, this might be attributed to the inherent coupling between the gyroscope and accelerometer errors under low dynamics and more separation and observability might be achieved with high dynamics.

Wind magnitude error is shown in Figure 11. As stated earlier, a slow varying wind is implemented in simulation and the filter estimates the constant component of wind during the flight. Wind is estimated within 60 s of GNSS presence and the error is less than 0.12m/s 150 s into the flight. The error in wind speed estimation remains bounded even after 140 s of GNSS outage, with the final wind estimation error being less than 0.2m/s. The filter is also seen to be consistent in the estimation of wind speed as it can be seen from the $1 \mathsf{\sigma }$ prediction even without an air data system attributed to correctness in the filter setup and the ability of the UKF to capture higher-order moments. The navigation performance of a model-based approach is generally prone to errors resulting from unknown external disturbances, such as wind, and therefore the filters ability to estimate wind even without an air data system makes this approach robust against external wind disturbances.

The position errors are presented in Figure 12 in the local level frame. It can be seen that for the most part during periods of GNSS availability, the filter prediction is consistent with the actual errors except during short periods of high dynamics between 34 s and 50 s where the filter slightly underestimates the north and east position errors. The slight underestimation might be attributed to the unresolved initialisation errors. During periods of GNSS outage, the filter is seen to be slightly conservative in prediction of position errors, however, the growth rate is rather slow reaching only 14.5 m in the North channel, 8 m in the East, and 4.3 m in the Down direction after 140 s of VDM coasting. The slight conservative prediction of position errors during periods of GNSS outage might be attributed to the coupling between wind speed and position errors.

Velocity is well-estimated within the first 60 s of GNSS availability and the error is seen to be less than 0.1m/s after 150 s of GNSS availability. After resolving initialisation errors, the filter is seen to be consistent in the estimation of velocity in the body-fixed frame. There is a marginal growth in velocity errors after GNSS outage, but errors remain well within 0.1m/s with very slight conservative $1 \sigma$ prediction, as can be seen in Figure 12.

Roll and pitch attitude errors are estimated well within 50 s with GNSS availability but yaw errors are slightly delayed and only well-resolved after 80 s of GNSS availability, as seen in Figure 13. The lack of a direct heading reference as well as large initialisation errors in heading might be the cause of the delayed and slightly optimistic estimate during GNSS availability. The angular rates are quickly estimated within 20 s of GNSS availability due to the presence of direct observations from the gyroscope. Roll and pitch errors remain within 0.06 degrees after 140 s of GNSS outage while yaw error increases slowly and remains within 0.65 degrees after 140 s of GNSS outage. The $1 \sigma$ prediction is seen to be consistent with the true error even during periods of GNSS outage.

Figure 14 shows the root mean squared (RMS) of position errors for the proposed UKF/VDM architecture with perturbed and augmented moment of inertia terms compared to the EKF/VDM architecture with perturbed but not augmented moment of inertia terms. Two important deductions are made from the results. First, the navigation performance in terms of position errors is similar for the two setups with marginal differences for 100 Monte Carlo runs. Secondly, both setups provide consistent position estimates and the differences are well within the precision of the Monte Carlo runs. It is important to note that these deductions are relatively similar for the remaining navigation states. Even though the relative strength of the proposed architecture as opposed to the EKF/VDM architecture is not obvious at this point, the similarities in navigation performance validates the proposed architecture.

The RMS of the mean error of 22 VDM parameters and 4 moment of inertia terms, collectively addressed as VDM terms in this context, is presented in Figure 15. With an initial uncertainty of 10% considered in each of the VDM terms, the filter is seen to be slightly optimistic in their estimation. Generally, the initial error in the VDM terms is seen to reduce quickly during periods of GNSS availability and the mean of the VDM terms remains bounded during periods of GNSS outage. The mean VDM error reduces quickly to less than 7.5% within 50 s of GNSS availability and then gradually to 6.6% at the onset of the GNSS outage. The optimistic filter estimation might be attributed to the strong coupling and correlation of the VDM terms but the difference between the mean error and the filter prediction is less than 19% at the end of the flight. Perhaps, more dynamic manoeuvres, exciting different modes, could improve their overall estimation but the current results are deemed well enough for the purpose of navigation owing to the consistent estimate in navigation states. It is worth mentioning that, for a similar setup, the EKF/VDM architecture with perturbed but not augmented inertia terms is seen to be overly optimistic in the estimation of the VDM parameters with a 48.5% difference between the mean error and the prediction at the end of the flight. This is not very surprising and highlights the strength of the proposed UKF/VDM architecture in the estimation of highly correlated terms. This also shows that the proposed architecture could be used with greater confidence in the presence of inertia tensor perturbations saving both time and cost associated with inertia modelling.

The error in the estimation of the moment of inertia terms is presented Figure 16. With an initial uncertainty of 10% in the moment of inertia terms it can be seen that errors in roll and pitch inertia terms are quickly resolved within 50 s of GNSS availability, even though the filter appears to be slightly optimistic in estimating these terms. The estimates remain bounded even during periods of extended GNSS outage lasting 140 s and the difference between the filter’s estimate and the actual error at the end of the flight is 24.4% and 33% for roll and pitch inertia terms, respectively. The moment of inertia term in yaw seems to be resolved after the initial errors in the roll and pitch terms have been resolved and continues to be observable even during periods of GNSS outage with a difference of 23% between the filter’s estimate and the actual error at the end of the flight. The product of inertia term seems to be slightly estimated in the initial phase of the flight within 30 s but then gradually diverges afterwards for the remainder of the flight. This might be attributed to the degree of coupling and lack of enough excitation for the product of inertia to be observable and perhaps more dynamic manoeuvres could improve the overall estimation.

Since the filter has shown consistency in the estimation of the navigation states, IMU errors, and wind, whilst being slightly optimistic in the estimation of VDM terms, the degree of correlation between the actual errors and the filter prediction warrants further discussion on uncertainty evolution of all states as well as correlation between the states.

The uncertainty evolution of all the states during periods of GNSS availability is presented in Figure 17. The uncertainty evolution during GNSS availability is presented as a ratio of the uncertainty at the moment of GNSS outage (200 s) to the uncertainty at initialisation. Generally, most states, including the VDM parameters and moments of inertia terms, are observable and remain bounded during GNSS availability. Some parameters including thrust coefficients and lift coefficients are not very well-estimated during periods of GNSS availability. Generally, the observability of VDM parameters is trajectory-dependent but the estimation is sufficient for the purpose of navigation.

The uncertainty evolution of all of the states during 140 s of GNSS outage is presented in Figure 18. This is presented as a ratio of the uncertainty at the end of the flight or outage period (340 s) to the uncertainty at the onset of the outage (200 s). Interestingly, the moment of inertia terms are well-bounded during the outage period and some terms are even estimated. Further, the VDM parameters are also well-bounded with the uncertainty of some parameters increasing slightly.

For a better understanding of the correlation properties between the different states, a correlation matrix is presented in Figure 19. It can generally be seen that the moment of inertial terms were decorrelated from most of the navigation states, however, they present strong correlation with the moment derivatives which is not surprising, owing to the formulation of the rigid body equations. Further, it can be seen that the moment derivative terms are correlated within groups. Pitch and yaw moment terms presents significant cross correlation while roll moment terms do not present significant cross correlation with the other terms. The high dynamics in the roll might be the reason for the overall group observability in roll moment terms as opposed to low-level dynamics in pitch and yaw. It’s also important to note that the VDM parameters are well-decorrelated from all the other states. Wind presents significant correlation with position and velocity states and could help explain the significant growth in position error during periods of GNSS outage.

5. Conclusions

In this paper, the use of a UKF and direct measurements from a low-cost MEMs-grade IMU and GNSS receiver in a loosely coupled approach has been investigated for a fixed wing UAV. It was found that the approach is able to consistently and efficiently estimate the navigation states whilst also calibrating or estimating model parameters. Further the state vector was augmented to include the moment of inertia terms since it was of interest to estimate this in flight as well. It was shown that the filter was able to estimate the moment of inertia terms even with a 10% initial uncertainty. Further, the filter was able to consistently estimate wind velocity without additional sensors which helped in improving the navigation solution especially during periods of extended GNSS outage where the position error in all directions was shown to be less than 14.5 m. The use of a UKF has been found to produce consistent estimates even with considerable initialisation errors. The real-time implementation of the approach is a subject for future work where other effects could also be investigated including control input noise, presence of coloured noise in measurements, and other practical considerations such as derivation of initial aerodynamic parameters and proper time stamping of measurements and inputs.