Sensor Fusion Algorithm Using a Model-Based Kalman Filter for the Position and Attitude Estimation of Precision Aerial Delivery Systems

Abstract

In this research, we focus on the use of Unmanned Aerial Vehicles (UAVs) for the delivery of payloads and navigation towards safe-landing zones, specifically on the modeling of flight dynamics of lightweight vehicles denoted Precision Aerial Delivery Systems (PADSs). While a wide range of nonlinear models has been developed and tested on high-end applications considering various degrees of freedom (DOF), linear models suitable for low-cost applications have not been explored thoroughly. In this study, we propose and compare two linear models, a linearized version of a 6-DOF model specifically developed for micro-lightweight systems, and an alternative model based on a double integrator. Both linear models are implemented with a sensor fusion algorithm using a Kalman filter to estimate the position and attitude of PADSs, and their performance is compared to a nonlinear 6-DOF model. Simulation results demonstrate that both models, when incorporated into a Kalman filter estimation scheme, can determine the flight dynamics of PADSs during smooth flights. While it is validated that the double integrator model can adequately operate under the proposed estimation scheme for up to small acceleration changes, the linearized model proves to be capable of reproducing the nonlinear model characteristics even during moderately steep turns.

1. Introduction

The use of Unmanned Aerial Vehicles (UAVs) has spread to numerous disciplines and will continue to expand in the foreseeable future, as more affordable sensors, actuators, and processing units become available, together with the advances in their legislation around the globe. Nowadays, UAVs are used in several applications in the scientific, civil, and commercial fields; for instance, in precision agriculture, weather monitoring, search and rescue, mining, remote sensing, and delivery of goods [1,2].

In this research, we focus on the use of UAVs for the delivery of payloads. A large number of applications make use of UAVs to deliver food, merchandise, medical supplies, or rescue equipment, among others. When natural disasters occur, such as earthquakes, landslides, blizzards, wildfires, or floods, affecting the terrestrial infrastructure, UAVs can promptly deliver emergency supplies. Additionally, UAVs provide a solution in situations where the rugged topography of a region inhibits fast transportation. For example, the company Zipline is delivering daily blood and blood products to health centers in the regions around the countries of Rwanda and Ghana, in Africa [3]. Given the coronavirus disease 2019 (COVID-19) pandemic, Zipline is also distributing medical supplies and personal protective equipment, and recently pursuing the possibility to provide COVID-19 supplies in the USA [4].

Several applications of UAVs rely on vehicles instrumented with expensive and delicate sensors. For instance, in remote sensing scenarios, it is common the use of Light Detection and Ranging (LiDAR); optical, multispectral, hyperspectral, and thermal cameras; radar; global navigation satellite system (GNSS) receivers; and processing units [5]. In the case of a vehicle malfunction, these instruments could be damaged given a collision with the ground, or lost in the event of loss of flight control or communication, causing landing in an unknown or unreachable location. In these situations, a safe-landing feature is most needed to recover the vehicle and its equipment. Precision Aerial Delivery Systems (PADSs) make use of guidance, navigation, and control, to maneuver parafoil-payload systems towards safe-landing zones, increasing the chances of successfully delivering payloads on target, and diminishing the possible damage while landing. A typical descent flight of a PADS is illustrated in Figure 1. The trajectory is composed of three segments: an initial flight towards the intended landing point (homing); a lemniscate (figure-eight) or circular pattern while losing altitude (energy management); and finally, a narrow turn to reach the target facing into the wind (landing). In this illustration three vehicles are depicted to represent different application scenarios, for example, a cargo aircraft for the delivery of goods, or as a recovery system for a weather balloon or a multicopter.

Design, modeling, and characterization of PADSs have been taking place since the beginning of the 1970s. Static and dynamic longitudinal stability was first studied by Goodrick, together with the early development of the equations of motion governing parafoil-payload systems, considering 3- and 6-degrees of freedom (DOF) [6,7], and the characterization of a 150 kg test payload. In the following years, the Small Autonomous Parafoil Landing Experiment (ALEX I and II), described in detail by Jann [8,9], set a milestone on modeling, validation, and verification of PADSs using 3- and 4-DOF, employing a ram-air parachute with a payload of 100 kg.

Different models have been developed for a varying range of DOF, either considering the parafoil-payload system as a rigid body or allowing relative motion between their components [10,11,12,13]. Comparisons between models happen to be a complex task, since different considerations, payload masses, parafoil aerodynamic properties, and general assumptions are used by different authors. To be able to compare the effectiveness between models ranging from 6- to 9-DOF, Gorman and Slegers [14,15] developed a parafoil model using the same aerodynamic properties for each scenario, realizing different DOF by modifying the kinematics constraints between the parafoil and the payload, under the same control inputs and initial conditions. The 6-DOF model represents the three inertial position components of the joint connecting the parafoil and the payload, as well as three Euler orientation angles. The 7-, 8-, and 9-DOF models incorporate extra Euler orientation angles for the payload, depending on the connection constraints. The 7-DOF model allows for yaw relative motion, the 8-DOF model allows yaw and pitch, whereas the 9-DOF model allows yaw, pitch, and roll relative motion between the parafoil and the payload. By contrasting experimental data with the models, Gorman and Slegers were able to conclude that the 6-DOF trajectory differed from the 7-, 8-, and 9-DOF trajectories mainly because of the great yaw relative motion (parafoil-payload twist). Since the relative roll and pitch motion is negligible in comparison to the relative yaw motion, the 7-DOF represents the model with the minimum DOF that captures the most significant flight dynamics, while the 6-DOF successfully characterizes the parafoil-payload system as a rigid body, presenting lower complexity than the rest of the proposed models. Since the PADS analyzed in this investigation is considered a rigid body, a 6-DOF model is adopted.

Due to the versatility of these delivery systems, and their possible applications in military scenarios, different sectors of the US Army teamed to form the Joint Precision Airdrop System (JPADS), categorizing their systems by weight as micro-light (4.5 kg to 68 kg), ultra-light (113 kg to 318 kg), extra-light (318 kg to 998 kg), light (2268 kg to 4536 kg) and medium (6804 kg to 19,051 kg) [16,17]. Initially, one of the JPADS requirements was to achieve $13.22 USD per kilogram of payload, a goal only met for the heavy categories, but far from being met for the light categories (micro-, ultra-, and extra-light) [18]. Making more affordable systems has been a priority, even at the expense of reducing their delivery precision. The Affordable Guided Airdrop System (AGAS), formed in 1999 [19], has the goal to minimize as much as possible the cost of delivery systems, obtaining the best achievable accuracy within a threshold of 100 m circular error probability (CEP). The AGAS initiative has been focusing on the extra-light and lightweight ranges, where mainly 900 kg payload implementations have been tested using circular parachutes, achieving 211 m CEP when 12 hour-old wind forecast information was used, and 38 m CEP when near real-time wind profiles were used [20]. Lighter systems have been able to achieve up to 10 m in specific cases [18]. Reduction in the actual landing distance to the intended point of impact, which translates to small CEP, has been achieved by the development of optimal control and guidance [21,22], strongly dependent on high-quality information about the wind profiles during the entire flight, from the beginning of the descent up to touchdown, and the capability to account for variable winds [23]. Nevertheless, achieving such high precision comes with a price, requiring computational power and elevated costs on sensors, actuators, and parachutes.

Although the 5 to 19,000 kg weight-range has been widely analyzed and tested, delivery systems for lighter applications have not been thoroughly explored, especially for low-cost solutions. Lightweight applications of PADSs face the challenge of relying on light hardware to perform guidance, navigation, and control. Current parafoil-payload systems make use of high-end processing units, capable of handling sophisticated real-time implementations of highly nonlinear dynamical models to estimate their attitude and position to operate their guidance system. If low-cost miniaturized versions of these delivery systems are to be explored, simplified models that capture their flight dynamics need to be developed. Additionally, estimation schemes for their position and attitude need to be taken into account, especially for further guidance and control implementations on vehicles with limited sensors, actuators, and processing capabilities.

The state estimation process based on the fusion of dynamic models and measurements obtained from different sensors is commonly and efficiently performed through a model-based Kalman filter. Whether a total state-space formulation of a Kalman filter is used; an error state-space, either with a feedforward or feedback implementation; an extended Kalman filter; an unscented Kalman filter; or any of modern developed variants, a Kalman filter is one of the most used algorithms for sensor fusion, particularly in navigation applications [24,25,26,27]. The type of Kalman filter implementation strongly depends on the intended application, the dynamics of the process to estimate, the required accuracy, and the computational capabilities at disposal.

According to a study performed by Zhang et al. [28], in which the performance of a Kalman filter, an extended Kalman filter, an unscented Kalman filter, and variations of these types of filters were compared for inertial navigation systems, the best accuracy is obtained by the unscented Kalman filter for their experiments. However, the unscented Kalman filter is the algorithm that demands the most computational effort among their comparison. On the other hand, the computational time required for the Kalman filter (not extended nor unscented) showed to be the lowest, 3 to 10 times smaller than the extended or unscented Kalman filter implementation, at the cost of having six to eight times smaller accuracy in the estimates.

Following the current development needs of PADSs, particularly for the micro-lightweight category, as well as the goals established by the AGAS program, in which the improvements of low-cost systems are prioritized even at the expense of decreasing the landing accuracy, a total state-space formulation of a Kalman filter is adopted for the estimation scheme proposed in this investigation. This type of implementation enables the use of low-cost, small, and lightweight sensors and processing units, requiring a linear model that represents the flight dynamics of the system.

This research presents the development, implementation, and comparison of a sensor fusion algorithm and estimation scheme for the position and attitude of PADSs, employing a Kalman filter based on two proposed 6-DOF dynamic models, suitable for lightweight low-cost applications. Section 2 details the proposed estimation scheme. The dynamic models that characterize the flight of the parafoil-payload system are presented in Section 3, firstly introducing a nonlinear 6-DOF dynamic model, and then the development of two linear alternatives: a linearized version of the nonlinear 6-DOF model, and a double integrator model. The required sensors and their characteristics suitable to perform the position and attitude estimation are presented in Section 4, whereas Section 5 presents the Kalman filter algorithm that incorporates the dynamic models and measurements from different sensors. The performance of the proposed estimation scheme is evaluated through simulations in Section 6, followed by the discussion of the results in Section 7. Finally, the conclusions of the proposed estimation scheme are outlined and the considered future work is depicted.

2. Proposed Estimation Scheme

The purpose of the estimation scheme is the determination of the position and attitude of PADSs, given the availability of onboard sensors as an inertial measurement unit (IMU) and a GNSS receiver, together with a model that represents the flight dynamics of the parafoil-payload system. The estimation scheme developed and implemented in this research is presented in Figure 2.

The estimation process begins with the control inputs $\mathbf{u}\left(t\right)$, which are executed by the actuators of the PADS as brake deflections that modify the shape of the canopy, steering the system towards the desired landing target. The control inputs are initially processed through an analog-to-digital converter (ADC), to be incorporated into a discrete linear model representative of the flight dynamics of the system, with process noise $\mathcal{W}$. The complete description of the linear models developed for this implementation is detailed in Section 3.1 and Section 3.2. The epoch-wise output states of the linear model ${\widehat{\mathbf{x}}}_k^{-}$ are combined with the measurements ${\mathbf{z}}_k$ from the IMU and GNSS receiver, contaminated with noise $\mathcal{V}$, through the implementation of a total state-space Kalman filter, providing the best estimate ${\widehat{\mathbf{x}}}_k$ of the position and attitude of the system, together with its error covariance $P_k$. A detailed explanation of the filter implementation is presented in Section 5.

The estimation process is performed every time a set of measurements is received from the sensors, depending on the sample rate defined for the system represented by the delay block. A comprehensive description of the sample rate and the characteristics of the sensors used is presented in Section 4. The a posteriori estimates of the filter (${\widehat{x}}_k$, $P_k$) are used to predict the new a priori estimates (${\widehat{x}}_k^{-}$, $P_k^{-}$), completing the recursive prediction-correction nature of the filter.

3. PADS Dynamic Models

To validate the accuracy of the models proposed in this research, and for comparison purposes, the 6-DOF model developed and tested by Ward et al. [29] is adopted as reference (henceforth denoted as the reference model). This model is based on the system identification of a series of flight tests, using a micro-lightweight PADS with a total mass of $2.37$ kg and a canopy with a wingspan of $1.77$ m, fitting the objective of this investigation. It captures the nonlinearities of the flight dynamics of the parafoil-payload system, therefore, it will serve as a reference to evaluate the performance of the linear models developed in this study.

The equations of motion of the reference model are derived from Newtonian mechanics, considering the parafoil-payload system as a rigid body, i.e., without relative motion between the parafoil and the payload. These equations are described in the body reference frame with a North-East-Down (NED) coordinate system, accounting for 6-DOF corresponding to the linear ($\mathbf{v}$) and angular ($\omega$) velocity vectors, with components $(u,v,w)$ and $(p,q,r)$, respectively. The dynamic equations are obtained by relating the time derivative of the linear and angular momentum to the sum of forces and moments, about the center of gravity in the body reference frame.

The forces under consideration include the aerodynamic forces that act on each element of the canopy $\left({\mathit{F}}_{A,i}\right)$. The canopy is discretized in seven elements, allowing for brake deflections only in the outermost elements. The deflection of the left $\left({\delta }_L\right)$ and right $\left({\delta }_R\right)$ brakes provide steerability to the system, by changing the lift and drag coefficients of the corresponding element of the canopy. These deflections can take any value from $-1$ to 1 (dimensionless).

Additionally, the aerodynamic forces acting on the payload $\left({\mathit{F}}_{A,P}\right)$, and the weight of the parafoil-payload system $\left({\mathit{F}}_W\right)$ are included. Finally, apparent mass forces and moments, caused by the acceleration of the fluid through which the vehicle moves, are considered according to [30]. Since the dynamic equations are obtained in a rotating reference frame (non-inertial), fictitious forces (${\mathit{F}}_F$) and moments (${\mathit{M}}_F$) emerge in the equations for the change of linear and angular momentum. For a detailed explanation of the computation of each term, as well as the system identification for the parafoil-payload system, refer to the full articles [29,31].

The resulting dynamic equations of the reference model [29] (p. 591, Equations (16)–(18)), are presented in compact form in Equation (1), representing the change of the state vector as the change in linear and angular velocity in the body reference frame $\dot{\mathbf{x}}={(\dot{u},\dot{v},\dot{w},\dot{p},\dot{q},\dot{r})}^T$,

$$\dot{\mathbf{x}}={\left[GM\right]}^{-1}\left[\begin{array}{c}{\mathit{F}}_F+{\mathit{F}}_W+{\mathit{F}}_{A,P}+{\mathit{F}}_{A,i}\\ {\mathit{M}}_F+{\mathit{M}}_W+{\mathit{M}}_{A,P}+{\mathit{M}}_{A,i}\end{array}\right]$$

(1)

where the matrix $GM$ incorporates the geometry, mass, and inertial properties of the parafoil-payload system of the reference model. While the forces and moments acting on the system are functions of the state variables in $\mathbf{x}$, the aerodynamic forces and moments of the parafoil and payload, ${\mathit{F}}_{A,i}$, ${\mathit{F}}_{A,P}$, ${\mathit{M}}_{A,i}$, and ${\mathit{M}}_{A,P}$, are also functions of an external wind vector $\lambda$. For the purpose of the development and simulation of the estimation scheme proposed in this research, the three components of the external wind vector $\lambda$ are assumed to be constant and equal to zero throughout the descent trajectory.

By assigning initial conditions and control inputs Equation (1) can be solved, and the resulting components can be transformed to the inertial reference frame by rotating according to the Euler orientation angles. Instead of adopting the start of the descent trajectory as the origin of the inertial z component, this is translated to the intended point of landing to better represent visually the followed trajectory of the PADS. Figure 3 presents the trajectory under study, constructed by implementing the reference model executing the three maneuvers described in

Table 1. Control maneuver segments.

Maneuver Id.	Start (s)	${\mathit{\delta }}_{\mathit{L}}$	${\mathit{\delta }}_{\mathit{R}}$
$t_1$	0	0	0
$t_2$	26	0.1	0
$t_3$	80	0	−0.4

as constant control inputs. The control inputs remain constant during the flight until the next maneuver is executed. This trajectory will serve as a reference for the analysis of the linear models presented in the following sections of this work. A video showing the reference trajectory flight is available as Supplementary Materials.

3.1. Linearized Dynamic Model

The linear approximation of the equations of motion is computed as the first-order Taylor series, evaluated around stable points of the states and inputs (${\mathbf{x}}_s,{\mathbf{u}}_s$). For the multivariable case under study $f(\mathbf{x},\mathbf{u})=f(u,v,w,p,q,r,{\delta }_L,{\delta }_R)$, each of the forces and moments that the system experiences can be expressed as a power series representation in the form:

$$\begin{array}{c}f(\mathbf{x},\mathbf{u})=f({\mathbf{x}}_s,{\mathbf{u}}_s)+\frac{\partial f}{\partial u}{|}_{{\mathbf{x}}_s}(u-u_s)+\frac{\partial f}{\partial v}{|}_{{\mathbf{x}}_s}(v-v_s)+\frac{\partial f}{\partial w}{|}_{{\mathbf{x}}_s}(w-w_s)+\frac{\partial f}{\partial p}{|}_{{\mathbf{x}}_s}(p-p_s)\\ +\frac{\partial f}{\partial q}{|}_{{\mathbf{x}}_s}(q-q_s)+\frac{\partial f}{\partial r}{|}_{{\mathbf{x}}_s}(r-r_s)+\frac{\partial f}{\partial {\delta }_L}{|}_{{\mathbf{u}}_s}({\delta }_L-{\delta }_{L_s})+\frac{\partial f}{\partial {\delta }_R}{|}_{{\mathbf{u}}_s}({\delta }_R-{\delta }_{R_s})+\mathcal{O}\end{array}$$

(2)

where $\mathcal{O}$ represents higher-order terms of the Taylor series, neglected for the linearization.

Since no wind, nor other perturbations are considered, fixing the control inputs in the reference model to a given deflection brake value (${\mathbf{u}}_s$), result in stable states after a stabilization period of approximately 10 s, depending on the magnitude of the maneuver. Thus, for each of the three segments of constant inputs on the flight trajectory, a set of stable points is determined from the reference model as the resulting state vector. The sable points obtained for each maneuver segment, applying the deflection brakes reported in Table 1 as stable control inputs ${{\delta }_L}_s$ and ${{\delta }_R}_s$, are listed in

Table 2. Stable points.

State Variable	Maneuver Segment
${\mathbf{x}}_s$	${\mathit{t}}_{\mathbf{1}}$	${\mathit{t}}_{\mathbf{2}}$	${\mathit{t}}_{\mathbf{3}}$
$u_s$ ($\mathrm{m}$/$\mathrm{s}$)	7.49	7.49	7.59
$v_s$ ($\mathrm{m}$/$\mathrm{s}$)	0	0	−0.01
$w_s$ ($\mathrm{m}$/$\mathrm{s}$)	4.09	4.09	4.12
$p_s$ (deg/s)	0	0.06	0.50
$q_s$ (deg/s)	0	0.04	1.46
$r_s$ (deg/s)	0	1.67	10.55

.

Applying this linearization method to Equation (1) and grouping common terms, results in the equation of motion of the linear 6-DOF model:

$$\dot{\mathbf{x}}={\left[GM\right]}^{-1}\left[\left[\begin{array}{c}{\mathbf{F}}_s\\ {\mathbf{M}}_s\end{array}\right]+\left[\begin{array}{cccccc}\frac{\partial F}{\partial u}& \frac{\partial F}{\partial v}& \frac{\partial F}{\partial w}& \frac{\partial F}{\partial p}& \frac{\partial F}{\partial q}& \frac{\partial F}{\partial r}\\ \frac{\partial M}{\partial u}& \frac{\partial M}{\partial v}& \frac{\partial M}{\partial w}& \frac{\partial M}{\partial p}& \frac{\partial M}{\partial q}& \frac{\partial M}{\partial r}\end{array}\right]\left(\begin{array}{c}u-u_s\\ v-v_s\\ w-w_s\\ p-p_s\\ q-q_s\\ r-r_s\end{array}\right)+\left[\begin{array}{cc}\frac{\partial F}{\partial {\delta }_L}& \frac{\partial F}{\partial {\delta }_R}\\ \frac{\partial M}{\partial {\delta }_L}& \frac{\partial M}{\partial {\delta }_R}\end{array}\right]\left(\begin{array}{c}{\delta }_L-{{\delta }_L}_s\\ {\delta }_R-{{\delta }_R}_s\end{array}\right)\right]$$

(3)

$$\dot{\mathbf{x}}={\left[GM\right]}^{-1}\left[\left[\begin{array}{c}{\mathbf{F}}_s\\ {\mathbf{M}}_s\end{array}\right]+J^{\mathbf{x}}\left(\mathbf{x}-{\mathbf{x}}_s\right)+J^{\mathbf{u}}\left(\mathbf{u}-{\mathbf{u}}_s\right)\right]$$

(4)

where ${\mathbf{F}}_s$ and ${\mathbf{M}}_s$ denote the forces and moments evaluated at the stable points. In addition, $J^{\mathbf{x}}$ and $J^{\mathbf{u}}$ represent the corresponding Jacobian matrices. For convenience, the linearized equation of motion is presented in a state-space representation:

$$\begin{array}{c}\dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u}+\epsilon \\ A={\left[GM\right]}^{-1}\left[J^{\mathbf{x}}\right],B={\left[GM\right]}^{-1}\left[J^{\mathbf{u}}\right],\\ \epsilon ={\left[GM\right]}^{-1}\left[\left[\begin{array}{c}{\mathbf{F}}_s\\ {\mathbf{M}}_s\end{array}\right]-J^{\mathbf{x}}\left({\mathbf{x}}_s\right)-J^{\mathbf{u}}\left({\mathbf{u}}_s\right)\right].\end{array}$$

(5)

Notice that matrices A and B are constant as long as the control inputs remain fixed, considerably simplifying the number of computations to be performed during flight. Similarly, all components of the vector $\epsilon$ are constant under the same condition, except for the components of the force produced by the weight of the vehicle (${\mathit{F}}_W$), which depend on its attitude. Matrices A and B, as well as vector $\epsilon$, evaluated at the stable points for each maneuver segment, are given in Appendix A.

The complete description of the position and attitude of the PADS in the inertial reference frame, as well as the linear and angular velocity in the body reference frame, can be expressed in a concise form by combining the kinematic and dynamic equations of motion in a single state-space representation:

$$\left[\begin{array}{c}\dot{\mathit{p}}\\ \dot{\Omega }\\ \dot{\mathbf{v}}\\ \dot{\omega }\end{array}\right]=\left[\begin{array}{cccc}0& 0& T_{BI}& 0\\ 0& 0& 0& S_{BI}\\ 0& 0& A_{11}& A_{12}\\ 0& 0& A_{21}& A_{22}\end{array}\right]\left[\begin{array}{c}\mathit{p}\\ \Omega \\ \mathbf{v}\\ \omega \end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& 0\\ B_{11}& B_{11}\\ B_{21}& B_{22}\end{array}\right]\left[\begin{array}{c}{\delta }_L\\ {\delta }_R\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ {\epsilon }_1\\ {\epsilon }_2\end{array}\right]$$

(6)

where matrices A and B, as well as the vector $\epsilon$ are distributed in components $A_{ij}$, $B_{ij}$, and ${\epsilon }_i$ respectively, to accommodate an adequate state-space representation. Additionally, $T_{BI}$ and $S_{BI}$ are the rotation matrices that transform the linear and angular velocity components from the body to the inertial reference frame:

$$T_{BI}=\left[\begin{array}{ccc}{\mathrm{c}}_{\theta }{\mathrm{c}}_{\psi }& {\mathrm{s}}_{\varphi }{\mathrm{s}}_{\theta }{\mathrm{c}}_{\psi }-{\mathrm{c}}_{\varphi }{\mathrm{s}}_{\psi }& {\mathrm{c}}_{\varphi }{\mathrm{s}}_{\theta }{\mathrm{c}}_{\psi }+{\mathrm{s}}_{\varphi }{\mathrm{s}}_{\psi }\\ {\mathrm{c}}_{\theta }{\mathrm{s}}_{\psi }& {\mathrm{s}}_{\varphi }{\mathrm{s}}_{\theta }{\mathrm{s}}_{\psi }+{\mathrm{c}}_{\varphi }{\mathrm{c}}_{\psi }& {\mathrm{c}}_{\varphi }{\mathrm{s}}_{\theta }{\mathrm{s}}_{\psi }-{\mathrm{s}}_{\varphi }{\mathrm{c}}_{\psi }\\ -{\mathrm{s}}_{\theta }& {\mathrm{s}}_{\varphi }{\mathrm{c}}_{\theta }& {\mathrm{c}}_{\varphi }{\mathrm{c}}_{\theta }\end{array}\right]$$

(7)

$$S_{BI}=\left[\begin{array}{ccc}1& {\mathrm{s}}_{\varphi }{\mathrm{t}}_{\theta }& {\mathrm{c}}_{\varphi }{\mathrm{t}}_{\theta }\\ 0& {\mathrm{c}}_{\varphi }& -{\mathrm{s}}_{\varphi }\\ 0& {\mathrm{s}}_{\varphi }/{\mathrm{c}}_{\theta }& {\mathrm{c}}_{\varphi }/{\mathrm{c}}_{\theta }\end{array}\right]$$

(8)

where the shorthand notation ${\mathrm{c}}_{\ast }$, ${\mathrm{s}}_{\ast }$, and ${\mathrm{t}}_{\ast }$ denotes $cos(\ast )$, $sin(\ast )$, and $tan(\ast )$, respectively. Rewriting Equation (6) leads to a compact equation, where the superscript $Li$ is used to indicate that an element corresponds to the linearized model:

$${\dot{\mathbf{x}}}^{Li}=A^{Li}{\mathbf{x}}^{Li}+B^{Li}{\mathbf{u}}^{Li}+{\epsilon }^{Li}.$$

(9)

While Equation (9) represents a time-continuous model, information from the onboard sensors arrive at discrete times. Solving the differential equation for the state vector using Euler’s method, with time step $\Delta t$, a time-discrete model is obtained as

$$\begin{array}{c}{\mathbf{x}}_k^{Li}=\left(A^{Li}\Delta t+I\right){\mathbf{x}}_{k-1}^{Li}+B^{Li}\Delta t{\mathbf{u}}_{k-1}^{Li}+{\epsilon }_{k-1}^{Li}\Delta t\\ {\mathbf{x}}_k^{Li}=F^{Li}{\mathbf{x}}_{k-1}^{Li}+G^{Li}{\mathbf{u}}_{k-1}^{Li}+{ϵ}_{k-1}^{Li}.\end{array}$$

(10)

where I represents the identity matrix. It is relevant to observe that Equation (10) describes 12 components in total, three for each vector: the position vector $\mathit{p}={(x,y,z)}^T$ and the angular position vector $\Omega ={(\varphi ,\theta ,\psi )}^T$, expressed in the inertial reference frame, with the latter corresponding to the Euler orientation angles (roll, pitch, yaw); and the linear velocity vector $\mathbf{v}={(u,v,w)}^T$ and angular velocity vector $\omega ={(p,q,r)}^T$, expressed in the body reference frame.

3.2. Double Integrator Dynamic Model

The development of an alternative linear model is achieved by exploiting the properties of the flight dynamics of PADSs. Since these are vehicles navigating in an underactuated controlled descent flight, typically without propulsion, and subject only to variations in the wind profiles, the flight dynamics are normally smooth, i.e., without sudden changes in the state variables of the parafoil-payload system, except for the voluntary control inputs exerted on the vehicle. Taking this into consideration, a double integrator model is proposed, based on the assumption that between consecutive measurements of the sensors acquiring information regarding the position and attitude of the vehicle, the linear $\left(\ddot{\mathit{p}}\right)$ and angular $\left(\ddot{\Omega }\right)$ accelerations remain constant for $\Delta t$ seconds. Integrating twice these accelerations with respect to time leads to the following equations of motion for the position and attitude, in the inertial reference frame:

$${\mathit{p}}_k={\mathit{p}}_{k-1}+{\dot{\mathit{p}}}_{k-1}\Delta t+{\ddot{\mathit{p}}}_{k-1}\Delta t^2/2$$

(11)

$${\Omega }_k={\Omega }_{k-1}+{\dot{\Omega }}_{k-1}\Delta t+{\ddot{\Omega }}_{k-1}\Delta t^2/2$$

(12)

which can be expressed in compact form as

$${\left[\begin{array}{c}\mathit{p}\\ \dot{\mathit{p}}\\ \Omega \\ \dot{\Omega }\end{array}\right]}_k=\left[\begin{array}{cccc}1& \Delta t& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \Delta t\\ 0& 0& 0& 1\end{array}\right]{\left[\begin{array}{c}\mathit{p}\\ \dot{\mathit{p}}\\ \Omega \\ \dot{\Omega }\end{array}\right]}_{k-1}+\left[\begin{array}{cc}\frac{\Delta t^2}{2}& 0\\ \Delta t& 0\\ 0& \frac{\Delta t^2}{2}\\ 0& \Delta t\end{array}\right]{\left[\begin{array}{c}\ddot{\mathit{p}}\\ \ddot{\Omega }\end{array}\right]}_{k-1}$$

(13)

$${\mathbf{x}}_k^D=F^D{\mathbf{x}}_{k-1}^D+G^D{\mathbf{u}}_{k-1}^D$$

(14)

where the superscript D is used to indicate that an element corresponds to the double integrator model. Notice that Equation (14) also describes 12 components in total, corresponding to the position, linear velocity, angular position, and angular velocity vectors, in the inertial reference frame. The position and angular position vectors can be directly compared with the analogous components of the nonlinear and linearized models. Contrarily, the linear and angular velocity vectors obtained from the double integrator model cannot be directly compared with the similar components from the nonlinear or linearized models, since they are expressed in different reference frames. To be able to compare them, the linear and angular velocity vectors obtained from the double integrator model are transformed to the body reference frame with the rotation matrices $T_{BI}^{-1}$ and $S_{BI}^{-1}$, respectively.

4. Sensors Characterization

The hardware under consideration must comply with the requirements of a lightweight mission, where the allocated volume and weight for the low-cost sensors are very limited. Since this estimation scheme is intended to be used in low-cost applications.

To evaluate the performance of the estimation scheme, simulated measurements are generated at 5 Hz sample rate, based on the state variables of the reference model and the statistical characteristics of the sensors acquiring this information in practice. This characterization is obtained through experimentation with real flight data from similar vehicles. Specifically, measurements from a GNSS receiver, a magnetometer, a gyroscope, and an accelerometer, are simulated by corrupting the position, angular position, angular velocity, and linear acceleration, of the reference model with white Gaussian noise, according to the statistical parameters reported in

Table 3. Sensors statistical parameters.

Sensor	Measurement Standard Deviation
GNSS (xy)	1.8 m
GNSS (z)	5 m
Magnetometer	0.5 deg
Gyroscope	0.1 deg/s
Accelerometer	$0.12$$\mathrm{m}$/$\mathrm{s}$${}^2$

.

The obtained deviation values from experimentation are similar in comparison to the noise and bias values reported in similar experiments. For example, Slegers and Yakimenko [32] present a bias in global positioning system (GPS) measurements of 2 m, plus noise of $0.5$ m; 2 deg bias and 1 deg noise for the angular position; and finally, 1 deg/s bias and 1 deg/s noise for the angular velocity. In their study, they also draw upon a linearization process of the flight dynamics with a sample rate of 2 Hz, assuming constant aerodynamic velocity. Cacan et al. [23] report 4 Hz sample rate for their guidance, navigation, and control algorithm, of a micro-lightweight PADS. Ward et al. [31] report noise with a standard deviation of 2 m in positioning measurements, and a standard deviation of 10 deg and 2 deg for the heading measurement bias and noise respectively, using 4 Hz sampling rate.

The sampling rate of 5 Hz for our investigation was chosen according to the processing capabilities of typical low-end microprocessors and sensors. For example, low-cost GNSS receivers as the u-blox NEO-6M or NEO-M8N can provide navigation information up to 1 Hz to 5 Hz, limiting the measurement availability [33,34].

Typical performance of processing units for this lightweight and low-cost application range from a microprocessor with 8-bit, 20 MHz capabilities, as the ATmega328 chip [35]; 32-bit, 216 MHz as the STM32F765 chip [36]; up to 64-bit, 1.5 GHz as the BCM2711 chip [37].

5. Kalman Filter Estimation Algorithm

Typically, PADSs are equipped with a GNSS receiver and an IMU, that provide information at discrete times about the position and attitude of the vehicle during its flight. The objective of any guidance, navigation, and control scheme is to process the incoming information from the sensors, in order to estimate the state variables of the system, to plan the most suitable trajectory towards the landing target while managing the energy budget. Given a linear discrete-time model for the state vector $\mathbf{x}$ and a measurement vector $\mathbf{z}$:

$${\mathbf{x}}_k=F_{k-1}{\mathbf{x}}_{k-1}+G_{k-1}{\mathbf{u}}_{k-1}+{\mathcal{W}}_{k-1}$$

(15)

$${\mathbf{z}}_k=H{\mathbf{x}}_k+{\mathcal{V}}_k$$

(16)

with process noise $\mathcal{W}$ and measurement noise $\mathcal{V}$, both assumed to be white, uncorrelated, zero-mean, and normally-distributed, the Kalman filter provides the best estimate of the states ${\widehat{\mathbf{x}}}_k$. In the following, the implementation of a model-based Kalman filter is described using a total state-space formulation [38].

Initially, an a priori state estimate ${\widehat{\mathbf{x}}}_k^{-}$ and error covariance estimate $P_k^{-}$ are obtained by propagating the a posteriori estimate from epoch $k-1$ to k:

$${\widehat{\mathbf{x}}}_k^{-}=F_{k-1}{\widehat{\mathbf{x}}}_{k-1}+G_{k-1}{\mathbf{u}}_{k-1}$$

(17)

$$P_k^{-}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+Q_{k-1}$$

(18)

where Q represent the process noise covariance matrix, estimated using the autocorrelation function $Q=G{\sigma }^2{G}^T$, with ${\sigma }_{Li}^2=800$ and ${\sigma }_D^2=10$ for each model, determined by tuning the filter during its implementation. Note that Equation (17) has the same form as the state vector obtained in Equation (10) for the linearized model, as well as in Equation (14) for the double integrator model, developed in the previous sections. Next, the a posteriori state estimate ${\widehat{\mathbf{x}}}_k$ is computed as a linear combination of the a priori estimate ${\widehat{\mathbf{x}}}_k^{-}$, and the difference between the measurement vector ${\mathbf{z}}_k$ and the prediction $H{\widehat{\mathbf{x}}}_k^{-}$, weighted by the Kalman gain $K_k$, designed to minimize the a posteriori error covariance $P_k$:

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

(19)

$${\widehat{\mathbf{x}}}_k={\widehat{\mathbf{x}}}_k^{-}+K_k\left({\mathbf{z}}_k-H{\widehat{\mathbf{x}}}_k^{-}\right)$$

(20)

$$P_k=\left(I-K_{k}H\right)P_k^{-}{\left(I-K_{k}H\right)}^T+K_k{R}_k{K}_k^T$$

(21)

where H denotes the observation matrix, R represents the measurement noise covariance matrix, and I corresponds to the identity matrix. The simulated measurements are incorporated into the Kalman filter as the measurement vector $\mathbf{z}$, while the squared of the standard deviations of the characterized sensors are used as the elements of the diagonal measurement noise covariance matrix R. The observation matrix H communicates the availability of the measurements, relating them with the state vector.

The implementation of the filter using the linearized model is realized by executing the constant brake deflections described in Table 1 as control inputs, fusing the simulated measurements obtained by the sensors. On the other hand, the double integrator model requires linear and angular accelerations as control inputs, which typically can be deduced from measurements employing onboard sensors. These accelerations are also simulated based on the reference model, by transforming the linear and angular acceleration from the body reference frame to the inertial reference frame, and adding white Gaussian noise with standard deviations of $0.12$ $\mathrm{m}$/$\mathrm{s}$${}^2$ and 2 deg/$\mathrm{s}$${}^2$ respectively, according to experimental data.

6. Simulation Results

Two separate implementations of the described discrete Kalman filter scheme were performed: one using the linearized model and brake deflections as control inputs, and another using the double integrator model and the simulated accelerations as control inputs. Both implementations were performed using the same initial conditions and simulated measurements.

The Kalman filter estimates based on the proposed linearized model (${\widehat{\mathbf{x}}}^{Li}$) and the double integrator model $\left({\widehat{\mathbf{x}}}^D\right)$ are presented in Figure 4, Figure 5, Figure 6 and Figure 7, together with the reference model $\left({\mathbf{x}}^R\right)$ for comparison purposes. The different maneuver segments are presented in all figures with vertical dashed lines at the time of execution, with the maneuver identifier displayed at the bottom to facilitate the interpretation.

From Figure 4, it can be observed that the Kalman filtering scheme based on each of the proposed models was capable of reproducing the components of the inertial position of the vehicle, for any of the flight scenarios corresponding to the maneuver segments. Due to the smooth evolution of each of the components of the inertial position, both the linearized model and the double integrator model were suitable for emulating the behavior of the position of the nonlinear reference model. This mild progression of the inertial position of the PADS closely represented a real flight scenario. From the beginning of the descent trajectory, after the full inflation of the canopy and stabilization of the gliding flight, the only external perturbation experienced by the parafoil-payload system originated from the wind. While strong wind profiles could substantially affect the flight dynamics of small vehicles, a fuselage designed to reduce drag could be employed to mitigate this effect. Additionally, the heavier the payload, the less prone it was to suffer abrupt changes in position due to its inertia.

The components of the linear velocity, presented in Figure 6, demonstrate that the lateral component of the velocity (v) was virtually zero along the complete descent trajectory. This reflects the fact that there were no perturbations that modify the lateral movement of the vehicle aside from the control inputs, and that the flight was dominated by its longitudinal dynamics. In the three linear velocity components, it was distinctly recognizable that the estimations based on the double integrator model strongly varied around the reference model, in comparison with the estimates from the linearized model. While the bounded variation confirmed that the Kalman filter estimation scheme converged towards the reference model, it was an indication that the Kalman gain was favoring the measurements instead of the model.

Note that along the descent trajectory, the deflection brakes applied on each maneuver were held constant until the next maneuver was reached, or the vehicle landed. Furthermore, the maneuvers were applied instantaneously, as step functions. This means that the delayed response of the actuators, and the elasticity of the lines used to apply the deflection brakes were not taken into account, which would modify the transient response of the state variables. Despite the condition analyzed in this investigation where the brake deflections were applied infinitely fast, the estimation process based on the proposed linearized model was able to follow the dynamics of the reference model, suggesting that for smoother executions of the maneuvers, the performance only could improve. In real applications, the maneuvers cannot be applied instantaneously, but gradually, leading to a smoother response on the state variables. The consequence of this abrupt change in the maneuver segments can be observed, for example, in the angular velocity components $(p,q,r)$ in Figure 7, especially when the last maneuver $\left(t_3\right)$ is applied.

While Figure 4, Figure 5, Figure 6 and Figure 7 provide valuable information about the 12 components of the state vector estimated by virtue of the proposed Kalman filter scheme, it is difficult to appreciate the shape and magnitude of the error with respect to the reference model. The error was computed as the difference between the reference model ${\mathbf{x}}^R$ and the Kalman filter state estimates based on the linearized model ${\widehat{\mathbf{x}}}^{Li}$ and the double integrator model ${\widehat{\mathbf{x}}}^D$, respectively. This error is denoted as ${\overline{\mathbf{x}}}^{Li}$ and ${\overline{\mathbf{x}}}^D$ for each implementation.

To obtain a better understanding of how closely the estimation scheme followed the reference model for the inertial position, the error in the magnitude of the position vector obtained with the reference model and the magnitude of the Kalman filter position vector estimates are presented in Figure 8, denoted as $\parallel \overline{\mathit{p}}\parallel$. It can be appreciated that both models were capable of representing the tridimensional position of the vehicle during the different flight segments since in general, the error oscillated around zero. In contrast, the filter estimates based on the linearized model presented a smoother variation, with an error magnitude approximately two times smaller.

To evaluate the performance in estimating the attitude of the PADS, Figure 9 presents the error between the Euler angles of the reference model and the Kalman filter estimates using each of the proposed models. Notice that during the first and second maneuver segments ($t_1$ and $t_2$), corresponding to a straight flight and a wide turn, the two models captured the same behavior as the reference model. Nevertheless, the double integrator model presented a larger and consistent error for roll $\left(\varphi \right)$ and pitch $\left(\theta \right)$ angles, and a smaller but still noticeable constant error for the yaw angle $\left(\psi \right)$, after the third maneuver was applied ($t_3$), where the flight trajectory with a narrower turn was followed. This is a consequence of the higher rate of change in the angular position that the vehicle experienced during the last flight segment, where a −0.4 right brake deflection was being applied. The double integrator model relied on the assumption that the parafoil-payload system experienced epoch-wise constant inertial acceleration during the integration period. Consequently, the higher the body-fixed angular velocity, the less this assumption was fulfilled, as can be verified from the error in the angular velocity components presented in Figure 10.

The achievable turn rate for any PADS has a practical limit, since a steep turn rate could induce spiral divergence. For a vehicle with similar characteristics as the simulated in this study, Ward et al. [39,40] report maximum turn rates from 15 deg/s up to 25 deg/s, depending on the flight mode and control scheme. For the case of larger parafoil-payload vehicles, Lingard [11] reports a maximum constant turn rate of $11.5$ deg/s for a canopy with 30 m of wingspan.

7. Discussion

It is relevant to highlight some properties and prerequisites for the operation and deduction of the linearized model and the double integrator model. The linearized model requires the calculation of the Jacobian matrices, increasing the complexity of its deduction depending on the forces and moments representing the interaction between the vehicle and its surroundings. This requires the characterization of the parafoil-payload system, limiting the flexibility of this model to be applied in any other vehicle with different properties. Moreover, the stable points need to be computed prior to the descent trajectory. While this demands additional effort, it is important to note that this task is performed on the ground, where typically more computational resources are available, without increasing the real-time computational burden during the flight. Since the flight conditions variate from flight to flight, it is required to calculate a set of stable points assuming the planned maneuvers to reach the target. If the computed stable points differ greatly from the actual conditions during flight, the performance would deteriorate.

In contrast, the double integrator model does not depend on the physical properties of the parafoil-payload system, simplifying the implementation of this model on vehicles with different characteristics. As a consequence, this model does not directly relate the brake deflections to the state variables, making it not suitable for a straightforward application of a control scheme for guidance. While the implementation of this model is much simpler than the linearized model, without requiring the computation of the stable points or other parameters before the flight, it heavily depends on the sample rate and quality of the measurements provided by the sensors.

The selected 5 Hz sample rate for the measurements is a conservative choice that complies with the capacities of any relatively modern low-cost processing unit and sensors for real-time applications. The higher the capabilities of the sensors and the processing unit, the larger the achievable sample rate during flight. This might be desirable when fast maneuvers are required or strong wind profiles modify the trajectory of the vehicle, although this implies higher costs associated with instrumentation, energy storage, and a heavier payload, sometimes not feasible for micro-lightweight PADSs.

In the case that exogenous forces are present during the flight as a consequence of strong winds, which are not incorporated into the flight dynamic models as the external wind vector $\lambda$, it is expected that the predictions provided by the models would deteriorate. Another source of perturbations could be the effect of mismodeled interactions in the parafoil-payload system, due to the relative motion of its components. Nevertheless, the overall performance of the proposed estimation scheme would not be necessarily deteriorated proportionally to the magnitude of the perturbations, since the Kalman gain would favor the measurements from the sensors or the predictions of the dynamic models, attempting to compensate for these effects by minimizing the a posteriori error covariance.

8. Conclusions

Throughout this research article, two models that represent the flight dynamics of micro-lightweight PADSs were developed: a linearized version of a 6-DOF nonlinear model and a double integrator model. Additionally, simulated measurements representative of sensors onboard the vehicle were fused by implementing a Kalman filter algorithm based on the developed models, to estimate the position and attitude of the parafoil-payload system.

The simulation results demonstrate that both models capture the flight dynamics of micro-lightweight PADSs when incorporated into a Kalman filtering scheme for smooth flights. While the double integrator model excels in simplicity, and it is independent of the physical properties of the parafoil-payload system, it falls behind in precision capturing the flight dynamics, especially when the vehicle is subject to intense accelerations. On the other hand, the linearized model is capable of representing the flight dynamics of the vehicle more accurately and preserves precision even during narrow maneuvers, but depends on the determination of stable points close to the operation point of the vehicle.

Further investigation is required on the inclusion of the proposed estimation scheme into a global closed-loop control strategy for the vehicle. In particular, tracking of constant waypoints is usually a control objective for this type of system. The utilization of the obtained linearized model could be a solid starting point for the development of a model-based robust controller. The design of a suitable control strategy, that includes the proposed estimation scheme, and its stability analysis is left as future work.