Reliable strategies for implementing model-based navigation on fixed-wing drones

Abstract

A relatively novel approach of autonomous navigation employing platform dynamics as the primary process model raises new implementational challenges. These are related to: (i) potential numerical instabilities during longer flights; (ii) the quality of model self-calibration and its applicability to different flights; (iii) the establishment of a global estimation methodology when handling different initialisation flight phases; and (iv) the possibility of reducing computational load through model simplification. We propose a unified strategy for handling different flight phases with a combination of factorisation and a partial Schmidt–Kalman approach. We then investigate the stability of the in-air initialisation and the suitability of reusing pre-calibrated model parameters with their correlations. Without GNSS updates, we suggest setting a subset of the state vector as ‘considered’ states within the filter to remove their estimation from the remaining observations. We support all propositions with new empirical evidence: first in model-parameter self-calibration via optimal smoothing and second through applying our methods on three test flights with dissimilar durations and geometries. Our experiments demonstrate a significant improvement in autonomous navigation quality for twelve different scenarios.

1. Introduction

1.1 Motivation

For small, lightweight drones, inertial-based dead-reckoning remains the default fall-back navigation solution when radio-frequency ranging is perturbed and optical or line-of-sight navigation is absent or becomes unavailable. Due to factors related to size, weight and cost, many such drones are equipped with inertial sensors that have considerably lower quality than those of navigation or tactical-grade systems. However, as proposed by Cork (2014) for a large aircraft, or by Khaghani and Skaloud (2016) for small fixed-wing drones, the drift of inertial dead-reckoning can potentially be mitigated without the use of additional sensor(s) by constraining the drone-motion through additional modelling. This particular realisation of the so-called vehicle dynamic model (VDM) approach is attractive for its potential to improve dead-reckoning precision as well as for the capacity to perform model-parameter self-calibration with nominal satellite positioning. Nevertheless, the real-time implementation of the VDM raises new challenges related to the numerical stability of the relatively large filter ($\approx$50 states). For instance, the need to re-adapt some model coefficients due to small changes in the physical properties of the drone (e.g. changes in the payload) between subsequent flights is possibly causing large fluctuations in the estimated corrections to navigation states during the in-air initialisation (cf. Section 6.4, Figure 6), which is undesirable for drone guidance. Furthermore, the estimation strategy during dead-reckoning must be re-adapted from the nominal scenario with satellite positioning as a large part of the state-vector containing auxiliary parameters related to model coefficients becomes non-observable. This contribution addresses the aforementioned concerns first conceptually and then by verifying their suitability with a large amount of new empirical evidence.

1.2 Estimation issues

An important factor in the forthcoming real-time implementation is the general stability of the estimation of filter states. Divergence or failure to correctly estimate some states can result in false positioning and/or confidence levels, which may hamper the safety and reliability of a drone's operation.

The stability of the filter is especially important in the case where the system is used in autonomous navigation, i.e. when satellite positioning is not available.

Numerical instability of Kalman filters is a common challenge and has been observed in filters with as few as six states, as shown by Grewal and Andrews (2002). Although the double-precision used on small on-board processors considerably reduces the accumulation of round-off errors that lead to filter divergence, the finite precision of variables, the large ratio of their relative magnitude, and the large number of numerical operations performed in the filter are all potential sources which can lead to a significant loss of precision and subsequent deterioration in the performance of a filter over time (Martel, 2006). From this perspective, a direct inclusion of all model parameters within the state estimation as auxiliary variables increases the likelihood of numerical instability due to significant non-homogeneity of their magnitudes. Investigation of this matter is therefore important to better understand its root causes and to identify and test effective remedies.

A drone using the VDM-based navigation system is prone to two potentially problematic stages occurring: (i) during filter initialisation; (ii) during a loss of GNSS signal reception occurring for longer than a period of a few seconds. VDM-based navigation requires in-air state initialisation as it models aerodynamic forces and not those acting on the drone through contact with the ground. The initial conditions of the navigation states can be provided by another system, such as that based on conventional INS/GNSS. However, obtaining reliable initial values of model parameters (part of auxiliary states) is challenging due to VDM complexity, the interdependence of the same parameters and their possible change once the drone is assembled after transport (e.g. wings are attached, battery secured, camera type or other payload selected and installed, etc.). The state initialisation thus poses an additional challenge due to the high dimensionality of state space and correlations that, in conditions of limited observability, may lead to filter divergence.

In the event of a GNSS signal outage, the uncertainty of the position estimation increases rapidly. As a consequence, after a certain time, some states may get adjusted aggressively by the remaining inertial/baro observations. This may cause somewhat erratic movements in navigation states and potentially destabilise the autopilot. To address both issues, the combination of a partial-Schmidt (Grewal and Andrews, 2002) and Bierman–Thornton (Bierman and Thornton, 1977) Kalman filter is proposed with empirically tuned parameters that optimise filter performance. The Schmidt–Kalman filter allows the selection of a subset of states that in the update phase can be partially or completely set aside, also called ‘considered’ states (Grewal and Andrews, 2002). The Bierman–Thornton filter, also known as the ‘UDU factorisation filter’, decomposes the state covariance matrix $\boldsymbol {P}$ into orthogonal triangular $\boldsymbol {U}$ and diagonal $\boldsymbol {D}$ matrices, such that $\boldsymbol {P} = \boldsymbol {UDU}^T$. This factorisation improves the computational stability and guarantees the symmetry of the covariance matrix (Bierman and Thornton, 1977).

Similar concerns are related to the elevated number of states and their inter-relations. Indeed, within the polynomial structure of the model for aerodynamic forces and moments, some model-related states manifest strong correlation (Laupré and Skaloud, 2020) with one another. Combining such states would reduce the total number of states in the system and consequently decrease the processing load in the filter. It is therefore interesting to investigate whether such an approach can be taken without an appreciable reduction of the navigation performance.

1.3 Experimental flights

Most of the reported results employing VDM-based navigation are still performed with simulated data. Hence, there is a need to validate the proposed methodologies with new and dissimilar empirical scenarios to establish convincing, practical evidence for their applicability in complex real-time implementation scenarios. The proposed modifications of the navigation system are therefore validated with a real, small, fixed-wing drone on four flights of diverse shapes. The longest of the four experimental trajectories is used as a calibration/training flight to estimate a set of aerodynamic coefficients using the augmented system-state estimation with the new implementation of an optimal smoother. These coefficients are then applied as prior initial values for the other three ‘application’ flights. There, different aspects of the filter stability are examined and the proposed solutions are evaluated for each of the aforementioned challenges. The investigations made during this study are summarised in Table 1.

Table 1. Overview of investigations performed on flights with different geometrical form as shown in Figure 2

The remaining sections of the paper are organised as follows. A brief review of the VDM-based navigation principles is provided in Section 2 with a proposition of lumping some of the aerodynamic parameters during estimation (in-flight calibration) and using their previously determined relationship (e.g. by linear regression) in trajectory prediction. Section 3 describes how the computation stability of the filter can be improved by addressing the round-off errors that occur in the estimator as a result of large differences in the magnitude of some state variables. Section 4 details how the stability of some estimated states is assessed and reinforced. The combined Schmidt–Kalman filter with UDU factorisation proves to successfully achieve this with particular settings tuned during testing. A brief description of the drone and its payload and the four experimental flights conducted to investigate the proposed strategies is presented in Section 5. Section 6 presents the results of the proposed modifications on the filter stability and navigation performance during: (i) self-calibration; (ii) initialisation; (iii) nominal and (iv) autonomous navigation flight scenarios. The final Section 7 summarises these findings.

2. Vehicle dynamic model navigation

2.1 Model definition

Despite the general idea being known for some time (Koifman and Bar-Itzhack, 1999), the practical confirmation of VDM-based navigation is relatively recent (Khaghani and Skaloud, 2018). Indeed, from the large spectrum of possible approaches to apply VDM towards the task of sensor fusion (Sensedobry, 2014), only that which uses the VDM as the main process model and inertial and other sensors outputs as observations seems to produce satisfactory results, as investigated in simulations by Cork (2014) for manned aircraft or by Khaghani and Skaloud (2016) for a small fixed-wing drone operation at low speeds and limited areas.

In short, this integration scheme uses autopilot's control command (ailerons, elevators, rudder and propeller speed) as an input to the aerodynamic model that specifies forces and moments acting on the drone. Its movement is predicted by resolving equations of motion. The necessary information on the wind is inferred through information redundancy between the aerodynamic model and inertial measurements.

The consistently available data are used as regular observations within the state-space estimation scheme. As detailed by Khaghani and Skaloud (2018), the VDM is employed as a system model and the three angular rates and three specific forces of the IMU are used as measurement updates (Khaghani and Skaloud, 2018, Equation (35)). Data from other sensors are used when available. This is the case for GNSS position and velocity (Khaghani and Skaloud, 2018, Equations (35) and (36)). The navigation filter (EKF or UKF) estimates the corrections to the navigation ($\boldsymbol {x_n}$) and auxiliary states and its associated covariance matrix ($\boldsymbol {P}$). Due to the large complexity, the elements of the state transition matrix within EKF are derived by an automated linearisation software for the general $l$-frame navigator (Khaghani and Skaloud, 2018, Equations (7)–(10)), yet its analytical form is presented in full within the Appendix of Khaghani (2018) for a local approximation. The same reference contains the models for all formerly mentioned sensors and actuators. The compound error state-vector has the following components and a minimum length:

(1)$$\boldsymbol{x}(46) = [ \boldsymbol{x}_n (12), \, \boldsymbol{x}_p (21), \, \boldsymbol{x}_a (4), \, \boldsymbol{x}_w (3), \, \boldsymbol{x}_e (6) ]^{\rm T},$$

where the auxiliary states are related to sensor (e.g. IMU) time-correlated errors ($\boldsymbol {x}_e$) containing at least three biases (modelled as random constant), actuators errors ($\boldsymbol {x}_a$) containing at least one constant per actuator, wind ($\boldsymbol {x}_w$) corrections per axis as well as VDM model-coefficient errors ($\boldsymbol {x}_p$). The latter is related to all aerodynamic coefficients (hereafter interchangeably referred to as ‘VDM parameters’) quantifying the forces and moments applied to a particular aircraft as depicted in Figure 1 and later detailed in this section. Such platform-dependant coefficients are either only approximately known or they may slightly evolve in time due to some changes in drone payload within a pre-calibrated system. Specific flight patterns and nominal GNSS reception scenarios can be used for such self (in-air) calibration (Laupré and Skaloud, 2021). The approach used to identify IMU stochastic model parameters follows Guerrier et al. (2015) so $\boldsymbol {x}_e$ may be extended with other time-correlated parameters when required. Their combined effect is subtracted from IMU-specific forces and angular rates before the update. The VDM governing equations following Ducard (2009) are given in Figure 1 (Equations (4) and (5)). The thrust force $F_T$ is defined in the body frame along the $x$-axis. Here, $V$ is the norm of airspeed i.e. the velocity at which the drone moves through the air; $D$ is the diameter of the propeller; $n$ is the rotation speed of the propeller and advance ratio $J = {V}/{D \pi n}$; $\bar {q}$ is the dynamic air pressure ($\bar {q} = {\rho V^2}/{2}$, with $\rho$ being air density) and $S$ is the wing surface. The moments are expressed in the body frame through its known relation (angles $\alpha$ and $\beta$) to the wind frame. The variables $b$ and $\bar {c}$ correspond to the wingspan and the mean aerodynamic chord of the drone, respectively. The position of the ailerons, the elevators and the rudder are the variables $\delta _a$, $\delta _e$ and $\delta _r$, respectively. Additionally, $\tilde {\boldsymbol {\omega }}=[ b\omega _{l,x},\, \bar {c}\omega _{l,y},\, b\omega _{l,z} ]^{\rm T}/{2V}$, where $\omega _{l}$ is the angular velocity vector of the body frame with respect to the local level frame, correction of which is part of the estimated navigation errors $\boldsymbol {x}_n(12)$, together with position, velocity and attitude.

Figure 1. Left panel shows the used drone with coloured actuating surfaces and relations between airspeed $\boldsymbol {V}$, wind $\boldsymbol {w}$ and drone $\boldsymbol {v}$ velocities through the angle of attack $\alpha$ and side-slip angle $\beta$; right panel shows the modelled moments $M_{xyz}$, thrust force $F_T$ and aerodynamic forces $F_{xyz}$ acting on the drone as a function of respective model coefficients $C_M$, $C_F$, control surface deviations $\delta$, dynamic air-pressure $\bar q$, air-density $\rho$, rotation speed $n$ of propeller with diameter $D$ as well as the advanced ratio $J$ defined in the text together with physical drone parameters $S$, $b$, $\bar c$ and non-dimensional angular rate $\tilde \omega$

In contrast, the platform-dependant geometric parameters such as wing span $b$, mass $m$ and moments of inertia $\boldsymbol {I}^b$ are excluded from $\boldsymbol {x}_p$ since they appear as scaling factors in the model and can be determined with much lower uncertainty a priori as compared to aerodynamic coefficients. Moreover, they are correlated with the aerodynamic coefficients which are already included in $\boldsymbol {x}_p$ (Khaghani and Skaloud, 2018).

2.2 Model reduction

As previously mentioned, the corrections $\boldsymbol {x}_p$ to aerodynamic coefficients (VDM parameters) are (possibly highly) correlated through the polynomial form of the dynamic model (Equations (4) and (5)). First, such correlations should be considered during filter initialisation with confidence prior to enabling them to be tuned if required. Second, there is a need to combine sets of highly correlated states to reduce the system to a size that is feasible for micro-controller implementation, as was suggested by Laupré and Skaloud (2020) but not explored so far. Rather than using very well-known approaches of state reduction via sub-optimal filtering (Gelb, 1974), we suggest lumping some correlated parameters for the estimation but using the full model for their application via the re-projected and previously determined correlations. The goal is thus to maintain as much of the system accuracy as possible, yet optimise the system for real-time implementation by suppressing some states, thereby reducing the computational requirements to update the state vector (the number of multiplications increases with the cube of state-vector size). The candidate pairs of coefficients for a possible reduction are identified by analysing the evolution of the covariance matrix $P$ in time. If the correlation between two states is high and does not vary temporally, the updated values are expected to change in a similar way when refined during the state estimation. Linear regressions can be formulated to express one coefficient as a function of the other as

(9)$$C_j = s_{ij} * C_i + o_{ij}$$

where the coefficient $C_j$ is expressed as a scale $s_{ij}$ function of $C_i$ plus an offset $o_{ij}$. The aerodynamic equations can be modified accordingly, reducing the total VDM parameters in the augmented state $\boldsymbol {x}_p$ by the number of candidates found. The choice of the aerodynamic coefficients to be paired and their linear expressions are given in Section 6.2. The influence of the new aerodynamic model on the VDM navigation performance in nominal flight conditions and under GNSS outage are explored in Section 6.6.

3. Numerical conditioning

3.1 General

The concept of constraining state-space estimators by computer is not new (Gelb, 1974), and for the case of inertial navigation, well analysed (Grewal et al., 2007). Although the general remedies are known (Grewal and Andrews, 1993), the potential numerical weaknesses particular to the implementation of VDM-based navigation should be first identified, before deciding on the necessity as well as a strategy for their mitigation. We first recall two types of problems leading to numerical instabilities: (a) ill-conditioning due to large magnitudinal differences in state values (round-off error) and (b) asymmetry of the state covariance matrix $P$. While the symmetry can be forced using a simple yet redundant operation as $P = (P + P^T)/{2}$, round-off errors are complex to avoid and correct as they occur during mathematical operations with limited precision.

An ill-conditioning indicator proposed by Grewal and Andrews (1993) using the largest $\lambda _{{\rm max}}$ and the smallest $\lambda _{{\rm min}}$ eigenvalue of matrix $S$ in the update Kalman filter equations (Table 2) is ${\rm cond}(S)= \lvert \lambda _{{\rm max}} \rvert / \lvert \lambda _{{\rm min}} \rvert$.

Table 2. Summary of discrete Kalman Filter steps

The following rule of thumb can be used to ensure a well-conditioned matrix (Grewal et al., 2007): ${\rm cond}(S) \ll {1}/({2^{-N}})$ with $N$ being the number of bits used in the mantissa. The software environment with a 64-bit architecture uses 52 bits for its mantissa¹ and therefore, the condition number should be below $10^{15}$ to guarantee numerical stability of the system. Additionally, round-off errors on the solution of the system $Ax=b$ can be shown to be bound by the following formula (Verhaegen and Van Dooren, 1986):

(10)$$\|(A^{{-}1} b)- (\widehat {A^{{-}1} b}) \| \leq \epsilon \,\cdot\, {\rm cond}(\hat A) \,\cdot\, \|(\widehat {A^{{-}1} b})\|$$

where $\|\,\cdot\, \|$ is the norm, $\epsilon$ is the machine/computer precision, ${\rm cond}(\,\cdot\, )$ is the condition number of a matrix and $(A^{-1} b)$ is the exact solution of the system while the symbol $\hat {(\,\cdot\, )}$ denotes its numerical approximation. The error in the resolution of a linear system is directly proportional to the condition number of the inverted matrix as shown in Equation (10). Although numerical errors do not necessarily lead to filter divergence, the filter can become momentarily unstable, leading to a slower steady-state convergence (Grewal and Andrews, 1993) or incorrect state estimation.

3.2 VDM in global-frame

As described by Khaghani and Skaloud (2018), the navigation frame is Earth-referenced to the WGS-84 ellipsoid with local level resolving axes and position expressed as geodetic latitude, longitude and height. For numerical operations, latitude and longitude is expressed in $radians$, and height in $m$. When using these units for their corrections within the filter states $\boldsymbol {x}_n$, the values come close to machine precision. Furthermore, the use of differential GNSS decreases the corresponding variance in position. For example, when kept in radians, the achievable centimetre accuracy in position with Real Time Kinematic (RTK) or Post Processing Kinematic (PPK) results in a standard deviation of approximately $10^{-9}$ [rad], which generates in the corresponding covariance matrix $P$ values of $10^{-18}$ [rad$^2$].

Before recalling the need for a re-scaled implementation, the discrete Kalman Filter steps are summarised in Table 2 to define notation, while more details are provided by Grewal and Andrews (1993) and Titterton et al. (2004).

A first step to reduce the condition number is to identify the extreme (smallest and largest) state variances in the initial covariance matrix $\boldsymbol {P}0$: the position ($10^{-18}$ [rad$^2$]) and the propeller speed ($10^2$ [(rad/s)$^2$]). The initial condition number for $P0$ is very high ($10^{22}$), certainly above the aforementioned limit of $10^{15}$. In a second step, chosen scaling factors $S$ are applied to the identified problematic states to reduce the condition number of the covariance matrix $\boldsymbol {P}0$: $S_1 = 10^8$ for latitude/longitude and $S_2 = 10^{-2}$ for the propeller speed. The scaling factors are chosen to change the initial state variances to approach unity. In this work, only the latitude, longitude and propeller speed-related states are scaled ($S_{\varphi }=S_{\lambda }=10^{8}$, $S_n = 0.01$). Practically, a scaled vector $\boldsymbol {x_s}$ is obtained from the initial state vector $\boldsymbol {x}$ multiplied with the scaling diagonal matrix $\boldsymbol {D_x} = [D_1,\, D_2]^{\rm T}$. This transformation can be seen as an arbitrary change of units for (a few) selected variables. Scaling the problematic states needs to be followed by an adaptation of related matrices as detailed in Table 3. As seen in Equation (10), the propagation of round-off errors while performing the matrix inversion is proportional to the condition number. The inversion occurs in the computation of the Kalman gain $\boldsymbol {K}_k$, specifically when the expression $\boldsymbol {G} =\boldsymbol {H} \boldsymbol {P} \boldsymbol {H}^T + \boldsymbol {R}$ is inverted. The units of $\boldsymbol {G}$ are controlled by the measurement noise $\boldsymbol {R}$ and observation $\boldsymbol {H}$ matrices. Hence, in the third step, these matrices must also be scaled with the noise scaling diagonal matrix $\boldsymbol {S}_z$ (if the noise and the states have the same units, $\boldsymbol {S}_z = {\rm diag}(\boldsymbol {S}_x)$ so they correspond to the new state vector $\boldsymbol {x}_s$. We obtain $\boldsymbol {H}_s = \boldsymbol {S}_z \boldsymbol {H}_u\boldsymbol {S}_x^{-1}$ and $\boldsymbol {R}_s = \boldsymbol {S}_z \boldsymbol {R}_u \boldsymbol {S}_z^T$.

Table 3. Adapted scaled matrices

The potentially problematic matrix is thus $\boldsymbol {G}_s = \boldsymbol {S}_z ( \boldsymbol {H}_u \boldsymbol {P}_u {\boldsymbol {H}_{\boldsymbol {u}}}^T +\boldsymbol {R}_u ) \boldsymbol {{S}_{z}}^T$. By carefully choosing the elements of $\boldsymbol {S}_z$, the magnitude of the elements of $\boldsymbol {G}$ can be adjusted to be more homogeneous which in turn lowers the condition number of the matrix. In the same manner, the observations $z$ related to the scaled states should be adapted to match the corresponding scaled observation matrix to compute a meaningful innovation: $\boldsymbol {z} - \boldsymbol {H}\boldsymbol {x} \implies \boldsymbol {S}_{z}\boldsymbol {z} - \boldsymbol {H}_{s}\boldsymbol {x}_{s}$.

In this work, the measurement noise covariance $\boldsymbol {R}$ is chosen to match the sensor characteristics (GNSS and IMU). These are described in Section 5.2. The entries into system noise covariance $\boldsymbol {Q}$ for the relevant states ($\boldsymbol {x}_n$, $\boldsymbol {x}_a$ $\boldsymbol {x}_w$, $\boldsymbol {x}_e$) are presented in Table A1 situated within the Appendix.

4. Factorisation and Schmidt–Kalman

4.1 Mixed approach

We revisit two known implementations of the Kalman filter, the combination of which is expected to address: (i) the general challenge of numerical stability of the estimation; (ii) the oscillations of state variables during the in-air initialisation; (iii) the prospect of maintaining the same filtering methodology during nominal and autonomous operation.

The first method exploits one particular form of factorisation put forward by Thornton and Bierman (1978), also known as $UDU$-factorisation, which separates the state covariance matrix as $\boldsymbol {P} = \boldsymbol {U} \boldsymbol {D} \boldsymbol {U}^T$ with $\boldsymbol {U}$ and $\boldsymbol {D}$ an upper triangular and diagonal matrix, respectively. The Kalman filtering steps as presented in Table 2 are adapted accordingly. This factorisation reduces the dynamic ranges of the variables, leading to more homogeneous scales in the computations, and preserves the symmetry of the covariance matrix $P$.

The second method employs the partial Schmidt–Kalman (Schmidt, 1966) or so-called ‘considered’ filter. The implementation is particularly advantageous when the estimated states are composed in large part of almost constant values such as sensor biases, or in the case of VDM, the aerodynamic coefficients. The partial Schmidt–Kalman filter developed by Brink (2017) is principally defined as a weighted mean between the updated and the predicted ‘partly considered’ states (here abbreviated as ‘considered’ states) and their covariance with a weight factor $\gamma (t) \in \, ]0-1\rangle$ as

(11)$$\begin{align} \hat{\boldsymbol{X}}_{y,k}^c & = \gamma(t) \hat{\boldsymbol{X}}_{y,k} + (1-\gamma(t))\tilde{\boldsymbol{X}}_{y,k} \end{align}$$

(12)$$\begin{align} \hat{\boldsymbol{P}}_{yy,k}^c & = \gamma(t)^2 \hat{\boldsymbol{P}}_{yy,k} + (1-\gamma(t)^2) \tilde{\boldsymbol{P}}_{yy,k} \end{align}$$

The dependency of $\gamma$ on a relative time $t$ is defined later (Equation (14)). The subscript $y$ denotes the elements where the partial Schmidt update is applied. With this definition, $\gamma =1$ computes an optimal Kalman update, whereas $\gamma =0$ creates a Schmidt update on the chosen states.

A practical issue with the $UDU$ decomposition is that this factorisation is not distributive on additions. In a naive implementation, one can reconstruct the covariance matrix, apply the partial reset and factorise it again. However, this would lead to a large increase in computations and partially defeat the purpose, which is to maintain the covariance in a numerically stable form. Carpenter and D'Souza (2018) presents a method to apply to a classic Schmidt–Kalman in the case of $UDU$ factorised filters. This development can be adapted to perform the aforementioned weighted mean. Indeed, by using the symmetry of covariance matrices and denoting $(\boldsymbol {H} \boldsymbol {P} \boldsymbol {H}^T + \boldsymbol {R}) = \boldsymbol {G}$, Equation (12) can be reformulated as

(13)$$\begin{align} \hat{\boldsymbol{P}}_{yy,k}^c & = \gamma(t) ^2 ((\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H})\tilde{\boldsymbol{P}}_{k})_{yy} + (1-\gamma(t)^2) \tilde{\boldsymbol{P}}_{yy,k} \nonumber\\ & = (\gamma(t) ^2 \tilde{\boldsymbol{P}}_{k}-\gamma(t) ^2 \boldsymbol{K}\boldsymbol{H}\tilde{\boldsymbol{P}}_{k})_{yy} + (1-\gamma(t)^2) \tilde{\boldsymbol{P}}_{yy,k} \nonumber\\ & =(\tilde{\boldsymbol{P}}_{k}-\gamma(t) ^2 \boldsymbol{K}\boldsymbol{H}\tilde{\boldsymbol{P}}_{k})_{yy} \nonumber\\ & =\hat{\boldsymbol{P}}_{yy,k} + (1- \gamma(t) ^2) (\boldsymbol{K} \boldsymbol{H} \tilde{\boldsymbol{P}}_{k})_{yy} \nonumber\\ & =\hat{\boldsymbol{P}}_{yy,k} + (1- \gamma(t) ^2)(\tilde{\boldsymbol{P}}_{k} \boldsymbol{H} \boldsymbol{G}^{{-}1} \boldsymbol{H} \tilde{\boldsymbol{P}}_{k})_{yy} \nonumber\\ & = \hat{\boldsymbol{P}}_{yy,k} + (1- \gamma(t) ^2)(\tilde{\boldsymbol{P}}_{k} \boldsymbol{H} \boldsymbol{G}^{{-}1} \boldsymbol{G} \boldsymbol{G}^{{-}1} \boldsymbol{H} \tilde{\boldsymbol{P}}_{k})_{yy} \nonumber\\ & =\hat{\boldsymbol{P}}_{yy,k} + (1- \gamma(t) ^2) (\boldsymbol{K} (\boldsymbol{H} \tilde{\boldsymbol{P}}_{k} \boldsymbol{H}^T + \boldsymbol{R}) \boldsymbol{K}^T)_{yy} \end{align}$$

This leads to a similar expression as found by Carpenter and D'Souza (2018), with the only difference in the term ($1-\gamma (t)^2$). Since the Bierman–Thornton update step requires scalar processing of measurements, the dimensions of $\boldsymbol {K}$ and $\boldsymbol {G}$ are $n\times 1$ and $1\times 1$, respectively. In other words, an update of rank one is applied directly to the decomposed matrices without the need to recompose them to perform the partial reset. The factorisation adds $n^2$ additions, $n^2 +3n + 2$ multiplications and $n-1$ divisions for each scalar measurement.

Such mixed implementation will be tested outside the nominal scenario within two critical flight phases, namely during the filter initialisation and GNSS signal outage. Both are described in the following subsections. For better clarity, the combination of the $UDU$ factorisation with the partial-Schmidt filter implementation will be referred to abbreviated as ‘partial-Schmidt’ in the following sections.

4.2 Initialisation

At the beginning of the filtering process, the Kalman filter passes through a transient phase. This is partly caused by: (i) the level of uncertainty normally expressed by a quasi diagonal matrix $\boldsymbol {P}0$, as most of the correlations between the states are unknown; (ii) the fact that most of the initial states are unknown (set to zero) and (iii) (some) have large uncertainties relative to the measurement precision. During this phase, there is a high probability that some states converge to a local minimum and remain either incorrectly estimated and/or correlated to other states until the conditions on their observability improve (e.g. due to new dynamics or additional measurements). This in turn limits the navigation quality. The ‘initialisation’ employing a partial Schmidt–Kalman filter is proposed to remedy this problem. To allow the states to evolve more smoothly, these are gradually ‘transited’ from ‘considered’ states to full states. This corresponds to increasing $\gamma$ from 0 to 1 during an initialisation period $\Delta T_{{\rm ini}}$ as

(14)$$\gamma (t) = \dfrac{1}{\Delta T_{{\rm ini}}} (T-T_{{\rm start}}), \quad T \in \langle T_{{\rm start}}, T_{{\rm start}}+\Delta T_{{\rm ini}} \rangle$$

where $T$ is the absolute time in a mission, $T_{{\rm start}}$ is when the initialisation period starts in this time and $t=(T-T_{{\rm start}})$ is the current (relative) estimation time. Such ‘initialisation’ is implemented for the VDM parameters $\boldsymbol {x}_p$ (that are usually pre-calibrated), the wind $\boldsymbol {x}_w$ and the sensor error states $\boldsymbol {x}_e$. These are referenced as ‘considered’ states later on.

4.3 GNSS outage

During a GNSS outage, the absence of position and velocity observations from a GNSS receiver prevents the direct update of navigation states that are related to other auxiliary states via an observation model, resulting in a constant increase in their uncertainties. However, the IMU measurements that are still present, update all navigation states $\boldsymbol {x}_n$ at high frequency. Thus, the elevated variance in position allows for large changes to be applied to the navigation states through IMU updates, possibly leading to erratic jumps in the trajectory. Such corrections are even more exaggerated if the IMU biases are poorly estimated. To counter this effect, the following states are set as ‘considered’ with $\gamma = 0$: the position $\boldsymbol {x}_n(pos)$, the aerodynamic coefficients $\boldsymbol {x}_p$, the wind $\boldsymbol {x}_w$ and the sensor error states $\boldsymbol {x}_e$.

The benefits of the presented combination of filtering strategies during these sensitive phases of flight (i.e. the initialisation and GNSS outage) are presented in Sections 6.4 and 6.5, respectively.

5. Experimental setup

5.1 Platform

The platform used for the experimental flights is depicted in Figure 1(left). Its wing span is $\sim$$1.6$ m and the take-off weight of $\sim$$2.8$ kg. An open source autopilot (FMUv2) from Dronecode (2008) was modified to accept the messages from a dual frequency (L1, L2), dual-constellation (GPS, GLONASS) receiver on board (Topcon B110 for the presented tests) and to receive precise pulse per second (PPS) for system-GPS time synchronisation.

The payload is composed of a camera, which is not used within this work, and two IMUs from Thales (Intersence, 2012), installed on a custom micro-controller board (Kluter, 2013), having the following specifications. Gyros: range $\pm$480 deg/s; performance, random walk $0.25$ deg/sqrt(h); noise density $0.004$ deg/s/sqrt(Hz); bias in run stability 12 deg/h; accelerometers: range $\pm$ 8 g; performance, random walk $0.045$ m/s/sqrt(h); noise density, 70 $\mathrm {\mu }$g/sqrt(Hz); bias in run stability $0.1$ mg; sampling, 250 Hz. This board references raw IMU observations in GPS time, and saves and streams them for further processing. Differential post-processing with a reference station allows positioning accuracy to be within a few cm ($0.03$ cm for horizontal, and $0.05$ cm for vertical), velocity accuracy to be within a few cm/s ($0.04$ cm/s for horizontal and $0.08$ cm/s for vertical) and attitude accuracy to be within $\sim$$0.05$–$0.1$ deg. GNSS accuracy is considered time-invariant for simplicity. A Javad Triumph2A (Javad, 2018) is chosen as the GNSS base station and a TOPcon B110 GNSS receiver (TOPcon, 2018) is on the aircraft. The resulting trajectory can be used either for the purpose of calibration and/or reference. The recorded raw inertial, GNSS and autopilot data are replayed as observations for the VDM-based navigation system, the quality of which is assessed with respect to the reference trajectory.

5.2 Flights

Four flights of different geometrical configurations are employed to test the benefit of the proposed numerical strategies within VDM-based navigation on this small fixed-wing drone. First, a 33-minute-long flight is used for calibrating the aerodynamic coefficients and is referred to as flight CF_i8. Three other flights, referred to as AF_i7 (28 min), AF_i6x (23 min) and AF_i6u (17 min) are application flights used to test the suggested modifications to the VDM-based navigation system. As depicted in Figure 2, the flights are dissimilar in their geometry, combining dynamics and a block pattern for CF_i8, different dynamics and block for AF_i7, a long straight corridor for AF_i6x, and a u-shape corridor for AF_i6u. Two flights (CF_i8, AF_i6x) were released as open-source data (Skaloud et al., 2021) and we have previously reported them for calibrating the VDM-parameters with the help of attitude updates derived from photogrammetry (Laupré and Skaloud, 2021). Although possible, such calibration is less practical and therefore not used here.

Figure 2. Experimental flights references (blue): (a) CF_i8; (b) AF_i7; (c) AF_i6x and (d) AF_i6u, beginning of the trajectory (red triangle)

5.3 Methodology

At first, the VDM-based navigation system is executed on flight CF_i8 with the nominal (i.e. uninterrupted) reception of GNSS signals for the purpose of self-calibration of VDM coefficients. These are estimated via an optimal recursive smoother. The obtained $\boldsymbol {x_p}$ and corresponding block-covariance matrix $\boldsymbol {P_p}$ are used as priors for all other application flights while increasing the uncertainty of $1\sigma$ to $2\%$ of its initial values. During the three application flights, the VDM parameters are fine-tuned (whenever GNSS positioning is present) to allow for potential small refinements due to changes in the platform configuration and its environment related to battery position, the re-assembled parts with a slightly different orientation between flights, small modification on the payload or changes in the weather conditions.

6. Results and discussion

After demonstrating the impact of numerical conditioning that affects all results, we present first the scenario of nominal GNSS signal reception during the long flight CF_i8 for (i) the self-calibration of VDM-parameters; (ii) the identification of correlated pairs of parameters for model reduction. Second, we evaluate the proposed strategy of filtering on full and reduced models during the initialisation and autonomous phases for all three application flights.

6.1 Numerical conditioning

After scaling the states related to errors in horizontal position and the propeller speed $n$, the condition number of $P$ decreases considerably by seven orders of magnitude (from $10^{18}$ to $10^{11}$) for all flights. The evolution of condition number with respect to time for flight CF_i8 is shown in Figure 3(a). This plot highlights that re-scaling bounds the condition number of $P$ to stay below the theoretical numerical stability threshold for the considered type of architecture and complex model. In contrast, without re-scaling, the condition number of $P$ oscillates above this theoretical threshold. A hazardous situation is further manifested by obtaining periodical warnings on the inversion of $G$ matrix when calculating the gain $K$ during updates (e.g. MATLAB message ‘Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = $1.9$$e-20$.’). This loss of numerical precision is manifested in Figure 3(b) by comparing the prediction of positions during simulated GNSS outages within the same flight. The numerical loss in position precision grows by a rate of $0.31$ m per minute for the unscaled scenario. With the previously described re-scaling of only three variables, the condition number of the matrix $G$ drops by eleven orders of magnitude from $10^{13}$ to $10^2$. This practical example of the improvement in the numerical stability is in agreement with Equation (10), according to which the propagation of round-off errors is diminished by a factor of $10^{10}$.

Figure 3. Flight CF_i8: (a) condition number of $\boldsymbol {P}$ with the unscaled (red) and scaled (blue) model. The dashed line represents the theoretical numerical stability limit ($10^{15}$ for a 52-bit mantissa architecture) over which the numerical stability of the matrix is not anymore; (b) accumulated growth in position error due to the loss of numerical precision during IMU updates

6.2 Self-calibration

During the self-calibration (flight CF_i8), the differential carrier-phase GNSS approach PPK is used for position and velocity updates during the whole trajectory. Such improved accuracy of aiding is perceived as important for the estimation of auxiliary states related to aerodynamic parameters. Considered time-invariant within the flight, their best estimate is obtained via an optimal forward-backward smoother. The initial aerodynamic coefficients are adapted from a similar shaped platform (Ducard, 2009) to the drone used for the experimental flights (Section 5). As their values were obtained for a different payload by Khaghani and Skaloud (2018), their initial uncertainties are set to 5% of their initial values to allow for possible variations. It should be stressed that the observability of parameters depends on the manoeuvrers (Laupré and Skaloud, 2020). Therefore, some highly dynamic manoeuvrers are part of this flight. The use of the optimal smoother further accentuates the existing structural correlations between the aerodynamic coefficients due to the model (Section 2) while de-correlating them with other states as depicted in Figures 4(b) and 4(c).

Figure 4. Absolute values of state correlation matrix (CF_i8 with highlighted $\boldsymbol {x}_p$ after: (a) 30 s of filtering; (b) the end of forward-filtering and (c) optimal smoothing

6.3 Parameter relations

The relations between model parameters are obtained by analysing the corresponding sub-bloc of the covariance matrix after smoothing ($\boldsymbol {P}_{sm}$). As depicted within the red square in Figure 4(c), the parameters outside the main diagonal in yellow are correlated by more than 90%. The depicted coefficients are calculated with the absolute values of the covariance elements as $\rho _{ij} = |P_{ij}|/\sqrt {P_{ii}P_{jj}}$. Five highly correlated pairs were selected for regression analysis. The resulting linear relations between the selected pairs are detailed in Table 4. As the force parameter $C_{F_y1}$ is correlated to $C_{F_T3}$ as well as to $C_{F_x1}$, the model is reduced by four coefficients.

Table 4. Proposed correlated pairs for model reduction (according to Equation (9)) and their linear relations

6.4 Filter initialisation

In the application flights, the set of aerodynamic coefficients obtained after the calibration flight with optimal smoothing is used while maintaining the possibility to adapt it slightly. Hence, the initial error state of model parameters $\boldsymbol {x}_p(0)$ is zero and its initial covariance follows from that of smoothing ($\boldsymbol {P}_{sm}$) with a 2% increase in variance to allow for the coefficients to be slightly adjusted to account for small modifications in the drone geometry due to re-assembly between flights. The initial value of navigation parameters and IMU sensor errors follows from the conventional INS/GNSS together with their confidence levels (Section 5). The initial value of error states $\boldsymbol {x}_n(0)$ and $\boldsymbol {x}_e(0)$ is therefore zero, as is that of actuator errors $\boldsymbol {x}_a(0)$. The position of the actuators is obtained from the autopilot. The initial wind is set to zero with an uncertainty of 2 m/s and $0.5$ m/s in the horizontal and vertical directions, respectively ($1\sigma$).

Initialisation periods of different duration ($T_{{\rm ini}}$) are tested on the three application flights to observe the influence of the partial updates (Section 4) on the fluctuation of position errors. Their maximum error during the first 200 s after initialisation is shown in Figure 5 for all three application flights. For all cases with $T_{{\rm ini}} \geq 50$ s, the maximum position error is reduced, improving the estimation with respect to the reference trajectories. In a similar trend, the respective norms of the velocity and attitude errors also decrease. These improvements are only marginal for $T_{{\rm ini}}$ being longer than one or two minutes, the reason for which $T_{{\rm ini}} = 100$ s was chosen. Figure 6 further depicts the detailed evolution of the horizontal error (magnitude) within flights AF_i7 and AF_i6x without ($T_{{\rm ini}}=0$ s) and with partial updates ($T_{{\rm ini}}=100$ s). For AF_i6x, the benefit of an ‘initialisation’ phase is substantial as the position error without a partial update is quite large. This is likely due to the instability of the filter caused by incorrect initial values of some parameters. Applying the partial-Schmidt filter reduced the maximum error in position by a factor of 6.

Figure 5. Maximum position error during the first 200 s after initialisation without and with partial-Schmidt ($T_{{\rm ini}}=100$ s) for the three application flights

Figure 6. Evolution of horizontal position error (magnitude) after initialisation for $T_{{\rm ini}}=[0,\, 100]$ s in flights AF_i7 (left) and AF_i6x (right)

Generally, within the three application flights, all the navigation state errors are decreased when an initialisation time close to one minute or longer is selected. For initialisation periods lasting longer than 5 min (300 s), there seems to be a higher dependence on the initial state values that reduces the rate of convergence. Nevertheless, a longer initialisation time (more than 500 s for example) may be considered on some flights with limited dynamics such as those flown for mapping missions (Laupré and Skaloud, 2021). These types of flights are monotonous with repetitive patterns flown at constant height and constant velocity. There, the benefit of partial-Schmidt at the initialisation phase of VDM-based navigation is less certain, because the dynamics during such missions is low, and in turn, the criteria to obtain sufficient observability for refining the aerodynamic coefficients may not be achieved (Johansen et al., 2015).

6.5 Autonomous navigation – full model

When a GNSS outage occurs, the IMU measurements and barometric-derived height are the only observations available in the current experimental setup. As described in Section 4, the filter is modified in such a way that all error states related to VDM parameters and that of position are placed in the ‘considered’ mode. For each application flight, four GNSS outages, each of a 2-min duration, were artificially introduced at different times within the flight, as summarised in Table 5.

Table 5. Start times (in minutes) of 2-min-long GNSS outage within the application flights after take-off

For each outage, the magnitude of observed horizontal errors in VDM-based navigation is evaluated and their maximum (bar) and median (trace) values are depicted in Figure 7. For comparison, a second evaluation is plotted on the same figure for inertial coasting (with barometer height aiding) using the identical sensor-error model. From the total of twelve cases, the reduction of maximum horizontal error for VDM with respect to inertial coasting is very significant on three occasions (more than 10$\times$), and significant on three others (more than 5$\times$). In the remaining six cases, the improvement varies from $1.5\times$ to $2.5\times$.

Figure 7. Maximum (bar) & median (trace) horizontal-position errors during four in total repetitive GNSS outages of 2 min for VDM and INS. Minutes from the take-off denotes the beginning of each outage

To observe further the improvement of the navigation solution via the proposed VDM approach, the duration of the first GNSS outage in AF_i6u was increased to 6 min. The autonomous navigation during this period is detailed in Figure 8(a,b), for the reference (blue), INS (red) and VDM (green). Panel (c) in the same figure shows that the maximum horizontal error in position during the outage for VDM is $\sim$250 m, for the inertial coasting is $\sim$$4.5$ km, i.e. approximately 18$\times$ larger.

Figure 8. Navigation solution in AF_i6u flight with 6-min-long absence of GNSS data: (a) horizontal view with reference trajectory (blue), INS solution (red), VDM solution (green); (b) closeup on reference and VDM; (c) horizontal distance to reference over the whole flight INS (red), VDM (green)

For each application flight, Figure 9 details the maximum and median errors in the horizontal position during two GNSS outages (each of 2 min) with partial (light grey) and with full (dark grey) updates of the state-vector. Apart from one minor exception, the position errors (as well as the velocity and attitude) are lower in all cases when the partial (rather than full) updates are applied. Figure 10 depicts the estimated position during some of the previously described GNSS outages in the application flights AF_i7 and AF_i6u without (dashed red) and with (green) the use of partial filtering. The reference trajectory is depicted as a dotted blue line. The small exception of slightly higher positioning error with partial filtering is related to the first simulated GNSS outage in the flight AF_i6u. There the error in heading is higher with the partial-Schmidt implementation, causing slightly larger deviation in the horizontal position after the nearly $1$-km-long straight line as shown in Figure 10(b).

Figure 9. Maximum and median horizontal errors with full updates (dark grey) and with partial updates (light grey) during 2-min GNSS outages

Figure 10. Two-minute GNSS outage with full updates (red dashes), with partial updates (solid green) on (a) AF_i7, (b) AF_i6u with the reference (dotted blue)

In all cases, the trajectory with the ‘considered’ states is smoother than the trajectory with updates in position. Such differences intensify towards the end of the outage period when the confidence in position is lower. A smooth and continuous estimate of position with a higher confidence level is more suitable for the guidance and control algorithms within the autopilot (DoT, 2015), especially when executing a fail-safe action such as return to land.

6.6 Autonomous navigation – reduced model

The reduced model is compared first with the full model for the nominal case of GNSS signal reception (100 s after the initialisation). There the differences in positions between both models are less than $0.2$ m, and hence practically negligible.

The effect of model reduction is, however, more noticeable within autonomous navigation. Considering the very same cases as in Figure 9, the differences between both models are plotted in Figure 11. The errors with the reduced filter are higher in five out of the six cases by a factor ranging from $1.2\times$ to 2$\times$. In comparison to simulations where both filters performed practically identically, such differences are noteworthy. This may be due somewhat to larger errors in attitude, notably in yaw angle determination. Although the drone guidance aims to fly each line with a constant speed and azimuth, the flying envelope of real tests is certainly richer than that of simulations. Thus, the higher-order coefficients may be accounting for (or absorbing some) real or non-modelled effects. For instance, the yaw angle is correlated with the coefficient $C_{F_y1}$ ‘refined value’ which may be influenced by the (less correctly) estimated side-slip angle and thus the real wind ($\boldsymbol {x_w})$.

Figure 11. Maximum and median horizontal errors during 2-min GNSS outages with the full (dark grey) and reduced (light grey) models

From these findings, it can be concluded that the gain in computational efficiency brought by the reduced model comes at a price of slightly worse navigation accuracy in the case of a GNSS outage. However, the quality of autonomous navigation with the VDM-reduced model is still significantly higher than that of the inertial coasting model. Within the nominal conditions of GNSS signal reception, these differences are practically insignificant.

7. Conclusion

Model-based navigation has good potential to improve the quality of autonomous navigation in small drones that is otherwise limited to platforms where tactical-grade IMUs can be used. However, the practical implementation of such an approach requires addressing a set of systematic challenges. On one side, these are related to the numerical stability of the state-space estimation and the capacity of model adaptation, either after drone re-assembly or due to modification of drone payload. On the other side, during the stages of initialisation and autonomous navigation, the oscillation of model parameters is not desired. Final concerns are related to considerably higher computational demands. The proposed methodology can be summarised as follows.

• • The overall numerical stability seems to be completely alleviated by re-scaling a small subset of state variables and by the employment of $UDU$ factorisation together with partial-Schmidt updates on a subset of filter parameters within the phases of initialisation and autonomous navigation. The employment of partial updates can be further useful for example within flight lines of constant velocity and orientation.

• • The described strategy is supported empirically by four flights of different geometry. First, optimal smoothing with high-precision GNSS aiding was used for the self-calibration of model parameters. These parameters were then used with their respective covariance as priors in the application flights. Second, in a dozen test cases of 2-minute-long dead reckoning within application flights, the drift of VDM/IMU-based navigation was confirmed to be significantly less than that of inertial coasting ($1.5\times$–10$\times$). Third, and even more important, the maximum positioning error of VDM-based navigation was maintained to 100 m or less. In terms of operational safety, it means that in case of GNSS signal loss, a small plane with a nominal speed of $15$ m/s can return close to home (or away from the perturbation in GNSS reception) from a distance of $\sim$2 km.

• • For reducing the computational requirements, dependencies among some model-state variables were first identified empirically by covariance analysis. This allowed for a smaller state vector to be established that, for the case of onboard processing, may be interesting to consider if computational resources are limited. Through the predetermined linear relation between aerodynamic coefficients, a full VDM was then used for motion prediction. In the investigated cases of GNSS outages, the observed median errors were very similar to that of the full VDM, but the maximum errors were somewhat larger ($1.2\times$–2$\times$). Therefore, such a trade-off may be interesting to consider, albeit for maintaining the best possible navigation performance in a GNSS-denied environment, the utilisation of the full model is recommended.