An Improved Method for Swing State Estimation in Multirotor Slung Load Applications

Abstract

A method is proposed to estimate the swing state of a suspended payload in multirotor drone delivery scenarios. Starting from the equations of motion of the coupled slung load system, defined by two point masses interconnected by a rigid link, a recursive algorithm is developed to estimate cable swing angle and rate from acceleration measurements available from an onboard Inertial Measurement Unit, without the need for extra sensors. The estimation problem is addressed according to the Extended Kalman Filter structure. With respect to the classical linear formulation, the proposed approach allows for improved estimation accuracy in both stationary and maneuvering flight. As an additional contribution, filter performance is enhanced by accounting for aerodynamic disturbance force, which largely affects the estimation accuracy in windy flight conditions. The validity of the proposed methodology is demonstrated as follows. First, it is applied to an octarotor platform where propellers are modeled according to blade element theory and the load is suspended by an elastic cable. Numerical simulations show that estimated swing angle and rate represent suitable feedback variables for payload stabilization, with benefits on flying qualities and energy demand. The algorithm is finally implemented on a small-scale quadrotor and is investigated through an outdoor experimental campaign, thus proving the effectiveness of the approach in a real application scenario.

1. Introduction

The use of drones for the transportation of loads is a topic of recent interest [1,2]. Both UPS and Amazon, for example, have projects to use drones for their deliveries, with solutions ranging from the adoption of manipulation devices to the equipment of ad hoc cargo compartments [3]. However, there is another field of load transportation that has scientific relevance, namely the use of unmanned rotary-wing platforms for transporting cable-suspended loads. In this respect, it is noted that multirotor vehicles exhibit complex dynamic behavior and the presence of a cable-suspended payload inherently increases the complexity of such systems because additional degrees of freedom are introduced. Moreover, if uncontrolled, a cable-suspended payload changes the dynamics of the flying vehicle and can result in an unstable system, with potential damage to the payload or its environment after collision with obstacles [4].

The problem of a single rotorcraft carrying a load has been addressed in the literature considering minimal load swing, with a number of works investigating different control approaches [5,6,7,8,9,10,11,12,13,14,15,16,17,18]. In [16], for example, the transportation problem is solved by an energy-based control strategy and a nonlinear feedback controller based on Linear Matrix Inequality (LMI). In [17], a Real-Time Dynamic Programming (RTDP) algorithm is proposed to optimize journey time and energy consumption. The RTDP algorithm is developed by discretizing the journey into distance interval horizons and applying the RTDP sweep to the current horizon to obtain the optimal velocity decision. In [18], a path-tracking controller is developed based on existing Lyapunov-based path tracking control laws for free-flying aerial vehicles, which are further backstepped through the vehicle rotation dynamics.

In order to achieve anti-sway capabilities, the accurate measurement of load position information is fundamental, with solutions that are often inspired from bridge crane applications [19]. Typical methods include the use of motion capture systems, where accurate estimation comes at the cost of complex and expensive installations [20,21]. Although optical-based alternatives can be conceived, such as visual detection [22], they require extra sensor devices that cause growth in take-off weight and power consumption or sensitivity to environmental conditions [23].

Methods that are not based on the use of ad hoc camera installations rely on instrumentations that are fixed with respect to the load. It is the case of high-accuracy GNSS devices [24], whose application would be needed for both the drone and the load in outdoor scenarios. In [25], a novel method for measuring the sway angle by using a clinometer attached to the load is proposed. In [26], the use of potentiometers is suggested to estimate the orientation of appendages for aerial manipulation tasks, with the possibility to perform swing angle measurements of suspended rigid links. In [27], a swing angle estimation method is developed without any external sensors except an attached IMU module. First, the authors derive an estimation algorithm of the disturbance force caused by the slung load using a disturbance observer. Since the direction of the tension force is the same as the direction of the wire, the swing angle is directly estimated from force components. The limitations of the method are pointed out and discussed, based on the inherent difficulty to discriminate the load-induced forces from propulsive or other disturbance contributions. To this end, the authors suggest a practical alternative where a single load-cell is mounted on the tether. In a recent work by one of the authors of the present paper [28], a method is presented to estimate swing angles in multirotor applications, with no need to rely on extra sensors, except for the available accelerometers. Sensor readings and dynamic model information are combined by a Fading Gaussian Deterministic Filter (FGDF), based on a recursive linear formulation derived in [29]. The approach is validated by numerical simulations and filter performance is measured and optimized in the presence of model uncertainties and measurement errors. In a similar manner, two types of anti-sway trajectory generation methods are derived in [30] on a linear basis to provide dynamically feasible and optimal trajectories for the system. In such a work, the authors remarkably release the aggressive motion and the payload swing in the transient response, while a Kalman Filter (KF) is proposed to estimate payload state from onboard accelerometers and gyroscopes.

The contribution of the present paper is twofold: (1) a new formulation of the estimation procedure described in [28] is derived and implemented to overcome performance limitations related to model uncertainties and agile maneuver conditions. To this aim, the point mass equations of the coupled slung load system are first derived through the Lagrangian approach, based on the assumption of a mass-less and rigid cable, and an Extended Kalman Filter (EKF) structure is implemented to allow for improved estimation performance. Then, the state vector of the EKF is extended by including the three components of aerodynamic disturbance force. Such a strategy is adopted to enhance estimation performance in windy conditions and provide an accurate estimation of the overall thrust generated by the rotors, making the need of a load cell unnecessary. (2) As a further contribution, the estimated swing angles and rates are used in a proportional feedback control logic aiming at the stabilization of payload oscillations. A controller structure that stems from the idea in [15] is designed as an auxiliary contribution to available control loops specifically developed for rotorcraft GNC tasks and possibly shaped in an optimal control fashion [31]. As a byproduct, it is shown that, for the same mission, the activation of the payload stabilization controller noticeably reduces the energy required from the battery pack with respect to the case when trajectory-tracking only is performed.

In what follows, preliminary definitions and the point mass system dynamic model are provided in Section 2. EKF recursive equations are detailed in Section 3, where the innovation vector is defined as the difference between model-based and measured vehicle accelerations, projected in the inertial frame. Validation is performed in Section 4. The proposed algorithm is first validated by means of numerical simulations. Mission tasks related to a real operational scenario are considered, where a battery-powered octarotor aircraft is modeled as a rigid body equipped with an elastic cable. System modeling includes electric propulsion system components and Blade Element characterization of propellers [32,33]. Environmental disturbances and model uncertainties are accounted and state estimation is evaluated during sample maneuvers, showing satisfactory accuracy for both swing state and aerodynamic disturbance characterization. The recursive estimation algorithm is then deployed on a commercial off-the-shelf Pixhawk controller, and filter performance is investigated for a small-scale quadrotor equipped with a two-axis potentiometer calibrated for direct measurement of cable swing angles. A comparison is thus provided between measured and estimated states for both the original FGDF of [28] and the updated EKF algorithm, the latter showing improved accuracy and robustness against windy conditions. A section of concluding remarks ends this paper.

2. System Modeling

Starting from the definition of reference frames, a 3 degrees-of-freedom mathematical model is adopted to describe both the multirotor and the load. After formulating the control problem, numerical validation is performed for a vehicle assumed to be a rigid body with center of gravity $CG$.

2.1. Reference Frames

Right-handed orthogonal reference frames are introduced according to the definitions in [32,34]:

1.

an Earth-fixed north–east–down frame, ${\mathcal{F}}_E=\{O_E;{\mathit{x}}_E,{\mathit{y}}_E,{\mathit{z}}_E\}$: the origin, $O_E$, is arbitrarily fixed to a point on the Earth’s surface, ${\mathit{x}}_E$ aims in the direction of geodetic north, ${\mathit{z}}_E$ points downward along the Earth ellipsoid normal, and ${\mathit{y}}_E$ completes a right-handed triad. This frame is assumed to be inertial under the assumption of flat and non-rotating Earth;

2.

a body-fixed frame, ${\mathcal{F}}_B=\{CG;{\mathit{x}}_B,{\mathit{y}}_B,{\mathit{z}}_B\}$: the longitudinal axis ${\mathit{x}}_B$ is positive out the nose of the rotorcraft in its selected plane of symmetry, ${\mathit{z}}_B$ aims in the direction of fuselage/frame bottom, and ${\mathit{y}}_B$ completes a right-handed triad.

3.

a rotorcraft structural reference frame, ${\mathcal{F}}_S=\{O_S;{\mathit{x}}_S,{\mathit{y}}_S,{\mathit{z}}_S\}$, used to locate $CG$ and all vehicle components: axes are parallel to the body-fixed frame axes, such that ${\mathit{x}}_S=-{\mathit{x}}_B$, ${\mathit{y}}_S={\mathit{y}}_B$, and ${\mathit{z}}_S=-{\mathit{z}}_B$. Stations ($ST$) are measured positive aft along the longitudinal axis. Buttlines ($BL$) are lateral distances, positive to the right, and waterlines ($WL$) are measured vertically, positive upward. Without loss of generality, it is assumed that $O_S$ lies on the top surface of multirotor frame and is aligned vertically with multirotor geometric center over the ${\mathit{x}}_S-{\mathit{y}}_S$ plane.

A sketch of a multirotor including the selected reference frames is reported in Figure 1.

Vector transformation between ${\mathcal{F}}_E$ and ${\mathcal{F}}_B$ is provided by the rotation matrix ${\mathit{T}}_{BE}\left(\mathit{\alpha }\right)$ [15], obtained by a 3-2-1 Euler rotation sequence where $\mathit{\alpha }={[\varphi ,\theta ,\psi ]}^T$ defines the attitude of the rotorcraft in terms of classical ‘roll’, ‘pitch’, and ‘yaw’ angles, respectively.

In addition to the reference frames introduced above, a third useful definition is provided for a particular Local-Vertical Local-Horizontal frame, ${\mathcal{F}}_H=\{CG;x_H,y_H,z_H\}$, with origin in $CG$. Vector transformation between ${\mathcal{F}}_B$ and ${\mathcal{F}}_H$ is provided by rotation matrix ${\mathit{T}}_{BH}={\mathit{T}}_{BE}\left({\mathit{\alpha }}_0\right)$, where ${\mathit{\alpha }}_0={[\varphi ,\theta ,0]}^T$ is characterized by null elementary rotation about $z_E$. Although the definition of ${\mathcal{F}}_H$ does not provide additional information to vehicle attitude characterization, which is defined by ${\mathit{T}}_{BE}$, it will be useful in Section 4 for the outline of a proposed cable-swing controller.

2.2. Equations of Motion

Let $\mathit{p}={[p_1,p_2,p_3]}^T$ be the position of multirotor and $v={[v_1,v_2,v_3]}^T$ its velocity with respect to ${\mathcal{F}}_E$. The payload is assumed to be a point with mass $m_l$ and is connected to the multirotor through a mass-less and rigid cable. The dynamics of the vehicle as expressed in ${\mathcal{F}}_E$ (the subscript E will be dropped for simplicity) are described by

$$\dot{\mathit{p}}=\mathit{v}$$

(1)

$$m\dot{v}={\mathit{f}}_g+{\mathit{f}}_l+{\mathit{f}}_a+{\mathit{f}}_c$$

(2)

where m is mass of the rotorcraft, ${\mathit{f}}_g={[0,0,mg]}^T$ is gravity force vector, ${\mathit{f}}_l$ is the force induced by the load on the rotorcraft through the cable, and ${\mathit{f}}_a={[f_{a_1},f_{a_2},f_{a_3}]}^T$ is vehicle aerodynamic force, that accounts for the contributions of rotorcraft frame and rotor in-plane forces. The net thrust vector ${\mathit{f}}_c={[f_{c_1},f_{c_2},f_{c_3}]}^T$, directed along $z_B$, represents the only control input to Equation (2).

Provided the cable has constant length L, payload position with respect to the rotorcraft is uniquely identified by angles $-\pi /2<\xi <\pi /2$ and $-\pi /2<\zeta <\pi /2$, respectively, obtained from cable rotations about ${\mathit{x}}_H$- and ${\mathit{y}}_H$-axis (see Figure 2). In such a case, the coupled slung load dynamic model of generalized coordinate $\mathit{\lambda }={[{\mathit{p}}^T,\xi ,\zeta ]}^T\in {\mathbb{R}}^5$ is retrieved from the work in [28], whose same nomenclature is adopted in the present framework. It is

$$\dot{\mathit{\lambda }}=\mathit{\mu }$$

(3)

$$\mathit{M}\left(\mathit{\lambda }\right)\dot{\mathit{\mu }}+\mathit{C}(\mathit{\lambda },\mathit{\mu })\mathit{\mu }+\mathit{G}\left(\mathit{\lambda }\right)=\mathit{\tau }$$

(4)

with $\mathit{\mu }={[v^T,\dot{\xi },\dot{\zeta }]}^T\in {\mathbb{R}}^5$ and

$$\mathit{M}\left(\mathit{\lambda }\right)=L{m}_l\left[\begin{array}{ccccc}M/\left(L{m}_l\right)& 0& 0& 0& c\zeta \\ 0& M/\left(L{m}_l\right)& 0& -c\xi c\zeta & s\xi s\zeta \\ 0& 0& M/\left(L{m}_l\right)& -s\xi c\zeta & -c\xi s\zeta \\ 0& -c\xi c\zeta & -s\xi c\zeta & L{c}^2\zeta & 0\\ c\zeta & s\xi s\zeta & -c\xi s\zeta & 0& L\end{array}\right]$$

(5)

$$\mathit{C}(\mathit{\lambda },\mathit{\mu })=L{m}_l\left[\begin{array}{ccccc}0& 0& 0& 0& -\dot{\zeta }s\zeta \\ 0& 0& 0& \dot{\xi }s\xi c\zeta +\dot{\zeta }c\xi s\zeta & \dot{\xi }c\xi s\zeta +\dot{\zeta }s\xi c\zeta \\ 0& 0& 0& -\dot{\xi }c\xi c\zeta -\dot{\zeta }s\xi s\zeta & \dot{\xi }s\xi s\zeta -\dot{\zeta }c\xi c\zeta \\ 0& 0& 0& -L\dot{\zeta }s\zeta c\zeta & -L\dot{\xi }s\zeta c\zeta \\ 0& 0& 0& L\dot{\xi }s\zeta c\zeta & 0\end{array}\right]$$

(6)

$$\mathit{G}\left(\mathit{\lambda }\right)=gL{m}_l{[0,0,-M/\left(L{m}_l\right),s\xi c\zeta ,c\xi s\zeta ]}^T$$

(7)

provided $M=m+m_l$ is the total mass of the system. The generalized force vector $\mathit{\tau }$ is made of an active control contribution, ${\mathit{\tau }}_c$, and a dissipative term, ${\mathit{\tau }}_a$, which mostly accounts for the aerodynamic drag effects. In particular, it is ${\mathit{\tau }}_c={[{\mathit{f}}_c^T,0,0]}^T$, while ${\mathit{\tau }}_a={[{\mathit{f}}_a^T,0,0]}^T-\mathit{D}\mathit{\mu }$ includes the rotorcraft aerodynamic force ${\mathit{f}}_a$ and an additional linear damping term incorporating the effects of payload drag at low oscillation speeds, provided $\mathit{D}=\mathrm{diag}(0, 0, 0, d, d)$ and $d>0$ is a drag coefficient.

3. Filter Recursive Equations

Let $\mathit{x}={[\xi ,\zeta ,\dot{\xi },\dot{\zeta },f_{a_1},f_{a_2},f_{a_3}]}^T={[x_1,x_2,x_3,x_4,x_5,x_6,x_7]}^T\in {\mathbb{R}}^7$ be the vector describing the oscillatory state of the payload and the aerodynamic disturbance affecting the rotorcraft. Define $\mathit{u}={\mathit{f}}_c={[u_1,u_2,u_3]}^T\in {\mathbb{R}}^3$ as the only input to the nonlinear system $\dot{\mathit{x}}=\mathit{f}(\mathit{x},\mathit{u})$, provided

$$\mathit{f}(\mathit{x},\mathit{u})=\left[\begin{array}{c}{x}_3\\ x_4\\ \begin{array}{cc}& [u_{2}cos\left(x_1\right)+x_{6}cos\left(x_1\right)+u_{3}sin{x}_1+x_{7}sin{x}_1\hfill \\ & +2Lm{x}_3{x}_{4}sin{x}_2]/[Lmcos\left(x_2\right)]\hfill \end{array}\\ \begin{array}{cc}& -[Lmcos\left(x_2\right)sin\left(x_2\right){\dot{\xi }}^2+u_{1}cos\left(x_2\right)+x_{5}cos\left(x_2\right)-u_{3}cos\left(x_1\right)sin\left(x_2\right)\hfill \\ & -x_{7}cos\left(x_1\right)sin\left(x_2\right)+u_{2}sin\left(x_1\right)sin\left(x_2\right)+x_{6}sin\left(x_1\right)sin\left(x_2\right)]/\left(Lm\right)\hfill \end{array}\\ {\mathbf{0}}_{3\times 1}\end{array}\right]$$

(8)

is obtained from Equations (3)–(7), with the additional assumptions of constant or slowly varying disturbance force, namely $\dot{{\mathit{f}}_a}={\mathbf{0}}_{3\times 1}$, and null payload drag, $d=0$. The inertially fixed acceleration of the rotorcraft is expressed as $\dot{\mathit{v}}=\mathit{h}(\mathit{x},\mathit{u})$, where

$$\mathit{h}(\mathit{x},\mathit{u})=\left[\begin{array}{c}\begin{array}{cc}& [u_{1}m+x_{5}m+u_1{m}_l{cos}^2\left(x_2\right)+x_5{m}_l{cos}^2\left(x_2\right)+Lm{m}_l{x}_4^{2}sin\left(x_2\right)\hfill \\ & -u_3{m}_{l}cos\left(x_1\right)cos\left(x_2\right)sin\left(x_2\right)-x_7{m}_{l}cos\left(x_1\right)cos\left(x_2\right)sin\left(x_2\right)\hfill \\ & +u_2{m}_{l}cos\left(x_2\right)sin\left(x_1\right)sin\left(x_2\right)+x_6{m}_{l}cos\left(x_2\right)sin\left(x_1\right)sin\left(x_2\right)\hfill \\ & +Lm{m}_l{x}_3^2{cos}^2\left(x_2\right)sin\left(x_2\right)]/\left(mM\right)\hfill \end{array}\\ \begin{array}{cc}& [u_{2}m+x_{6}m+u_2{m}_l+x_6{m}_l-u_2{m}_l{cos}^2\left(x_2\right)-x_6{m}_l{cos}^2\left(x_2\right)\hfill \\ & +u_2{m}_l{cos}^2\left(x_1\right){cos}^2\left(x_2\right)+x_6{m}_l{cos}^2\left(x_1\right){cos}^2\left(x_2\right)\hfill \\ & +u_3{m}_{l}cos\left(x_1\right){cos}^2\left(x_2\right)sin\left(x_1\right)+x_7{m}_{l}cos\left(x_1\right){cos}^2\left(x_2\right)sin\left(x_1\right)\hfill \\ & +u_1{m}_{l}cos\left(x_2\right)sin\left(x_1\right)sin\left(x_2\right)+x_5{m}_{l}cos\left(x_2\right)sin\left(x_1\right)sin\left(x_2\right)\hfill \\ & -Lm{m}_l{x}_4^{2}cos\left(x_2\right)sin\left(x_1\right)-Lm{m}_l{x}_3^2{cos}^3\left(x_2\right)sin\left(x_1\right)]/\left(mM\right)\hfill \end{array}\\ \begin{array}{cc}& [u_{3}m+x_{7}m+u_3{m}_l+x_7{m}_l+g{m}^2+gm{m}_l\hfill \\ & -u_3{m}_l{cos}^2\left(x_1\right){cos}^2\left(x_2\right)-x_7{m}_l{cos}^2\left(x_1\right){cos}^2\left(x_2\right)\hfill \\ & +u_2{m}_{l}cos\left(x_1\right){cos}^2\left(x_2\right)sin\left(x_1\right)+x_6{m}_{l}cos\left(x_1\right){cos}^2\left(x_2\right)sin\left(x_1\right)\hfill \\ & -u_1{m}_{l}cos\left(x_1\right)cos\left(x_2\right)sin\left(x_2\right)-x_5{m}_{l}cos\left(x_1\right)cos\left(x_2\right)sin\left(x_2\right)\hfill \\ & +Lm{m}_l{x}_4^{2}cos\left(x_1\right)cos\left(x_2\right)+Lm{m}_l{x}_3^{2}cos\left(x_1\right){cos}^3\left(x_2\right)]/\left(mM\right)\hfill \end{array}\end{array}\right]$$

(9)

Let $\widehat{\mathit{x}}\left(t\right)$ be the estimated state at time t obtained through the EKF predict–update dynamic model:

$$\dot{\widehat{\mathit{x}}}\left(t\right)=\mathit{f}\left(\widehat{\mathit{x}}\left(t\right),\mathit{u}\left(t\right)\right)+\mathit{K}\left(t\right)\left(z\left(t\right)-\mathit{h}\left(\widehat{\mathit{x}}\left(t\right),\mathit{u}\left(t\right)\right)\right)$$

(10)

where $z\left(t\right)={[a_1,a_2,a_3]}^T\in {\mathbb{R}}^3$ is the vector of measured acceleration projected in ${\mathcal{F}}_E$ and $z\left(t\right)-\mathit{h}\left(\widehat{\mathit{x}}\left(t\right),\mathit{u}\left(t\right)\right)$ is innovation residual. Model-based prediction is affected by multi-variate process noise $\mathit{w}\left(t\right)\sim \mathcal{N}\left(0,\mathit{Q}\left(t\right)\right)$ with zero-mean normal distribution $\mathcal{N}$ and covariance $\mathit{Q}\left(t\right)\in {\mathbb{R}}^{7\times 7}$. In the same way, measurement noise $\mathit{\nu }\left(t\right)$ characterizes accelerometer readings with covariance matrix $\mathit{R}\left(t\right)\in {\mathbb{R}}^{3\times 3}$. Kalman gain matrix $\mathit{K}\left(t\right)\in {\mathbb{R}}^{7\times 3}$ is derived from

$$\mathit{K}\left(t\right)=\mathit{P}\left(t\right){\mathit{H}}^T\left(t\right){\mathit{R}}^{-1}\left(t\right)$$

(11)

provided that covariance estimate propagates according to

$$\dot{\mathit{P}}\left(t\right)=\mathit{F}\left(t\right)\mathit{P}\left(t\right)+\mathit{P}\left(t\right){\mathit{F}}^T\left(t\right)-\mathit{K}\left(t\right)\mathit{H}\left(t\right)\mathit{P}\left(t\right)+\mathit{Q}\left(t\right)$$

(12)

Unlike the discrete-time EKF, it is noted that the prediction and update steps are coupled in a continuous-time formulation [35]. From a practical standpoint, this implies that the propagation of both $\widehat{\mathit{x}}$ and P must be performed by explicit or implicit numerical methods [36]. In this respect, further details are provided in the Section 4.

Finally, the state transition $\mathit{F}\left(t\right)\in {\mathbb{R}}^{7\times 7}$ and observation $\mathit{H}\left(t\right)\in {\mathbb{R}}^{3\times 7}$ matrices are respectively defined as the Jacobians:

$$\mathit{F}\left(t\right)={\left.{\displaystyle \frac{\partial \mathit{f}}{\partial \mathit{x}}}\right|}_{\widehat{\mathit{x}}\left(t\right),\mathit{u}\left(t\right)},\mathit{H}\left(t\right)={\left.{\displaystyle \frac{\partial \mathit{h}}{\partial \mathit{x}}}\right|}_{\widehat{\mathit{x}}\left(t\right),\mathit{u}\left(t\right)}$$

(13)

whose components are listed in Appendix A.

Remark 1.

Input $\mathit{u}\left(t\right)$ in Equation (10) is assumed to be known. As a matter of fact, the exact knowledge of control force is not directly available in practical applications, where detailed models of powerplant subsystems and propellers aerodynamics would be required. In what follows, a sample solution is proposed for multirotor vehicles to address model uncertainty through measurements without the need for ad hoc devices or accurate characterization of propulsion units. Based on the definition in Section 2.2, the control force is expressed as ${\mathit{f}}_c=-T{z}_B={\mathit{T}}_{BE}^T{[0,0,-T]}^T$, where T is net thrust generated by rotors along $z_B$. Taking into account Equation (2), the estimated thrust magnitude $\widehat{T}$ can be calculated according to

$$\widehat{T}=∥mz-{\widehat{\mathit{f}}}_g-{\widehat{\mathit{f}}}_l-{\widehat{\mathit{x}}}_{5:7}∥$$

(14)

where $∥·∥$ is Euclidean norm, ${\widehat{\mathit{f}}}_g={[0,0,m\widehat{g}]}^T$, $\widehat{g}$ is measured gravity acceleration, and ${\widehat{\mathit{f}}}_l={[0,0,m_l\widehat{g}]}^T$ is the force induced by the load on the rotorcraft under the assumption that the cable is exactly directed along the local vertical, as it occurs in near-hovering equilibrium condition. Finally, ${\widehat{\mathit{x}}}_{5:7}={[{\widehat{x}}_5,{\widehat{x}}_6,{\widehat{x}}_7]}^T$ is the vector of filter-estimated aerodynamic force, that includes external disturbances and rotor forces over the ${\mathit{x}}_B-{\mathit{y}}_B$ plane. Reconstructed input for filtering purposes is thus obtained as $\widehat{\mathit{u}}={\widehat{\mathit{T}}}_{BE}^T{[0,0,-\widehat{T}]}^T$, provided ${\widehat{\mathit{T}}}_{BE}$ is the rotation matrix calculated from measured attitude information. Extension of the proposed method to conventional helicopter configurations can be conceived with minimum effort by accounting both tail rotor and main rotor contributions for the estimation of net control force.

4. Results

Filter validation is performed by numerical simulations and an experimental campaign. In the first case, a cargo octarotor is shown to perform sample delivery maneuvers of a relatively large payload with and without the activation of a basic swing-damping controller. In the second case, the approach is addressed for a small-scale quadrotor application, provided the estimated swing state is compared to direct measurements obtained through potentiometer readings.

4.1. Simulation Results

4.1.1. Simulation Setup

Simulations are performed in a Matlab^® environment with a fourth-order Runge–Kutta solver frequency of 500 Hz [36]. The octarotor is modeled as a rigid body, while the payload is assumed to be a point mass attached through a linear-elastic cable to a hook point H, distinct from the rotorcraft center of gravity $CG$. The modeling follows the same approach and nomenclature adopted in [15], while relevant system parameters are listed in

Table 1. Multirotor slung load system parameters.

Parameter	Symbol	Value	Units
Multirotor
Mass	m	70	kg
Center of gravity position	${STA}_{CG}={BL}_{CG}$	0	m
	${WL}_{CG}$	−0.15	m
Moments of inertia	$J_{11}$	10.61	kg m2
	$J_{22}$	10.31	kg m2
	$J_{33}$	19.74	kg m2
	$J_{12}$	0.037	kg m2
	$J_{13}$	−0.043	kg m2
	$J_{23}$	−0.003	kg m2
Center of pressure position	${STA}_{CP}={BL}_{CP}$	0	m
	${WL}_{CP}$	−0.125	m
Frame drag areas	$A_1=A_2$	0.22	m2
	$A_3$	1.03	m2
Propeller
Number of blades	$n_b$	2
Radius	R	0.5	m
Mean aerodynamic chord	$\overline{c}$	0.086	m
Chord @ $75\%R$	$c_{75}$	0.103	m
Lift curve slope	a	5.9	rad−1
Pre-cone angle	$a_0$	0	rad
Root pitch angle	${\theta }_0$	0.7854	rad
Total twist	${\theta }_t$	−0.6981	rad
Load
Mass	$m_l$	100	kg
Reference area	$A_l$	0.785	m2
Drag coefficient (sphere)	${C_d}_l$	0.5
Cable
Nominal cable length	L	15	m
Hooke’s constant	K	90,950	N/m
Hook point position	${STA}_H={BL}_H$	0	m
	${WL}_H$	−0.3	m

. The payload is affected by aerodynamic drag, provided $A_l=\pi R_l^2$ is the frontal area of an equivalent sphere with radius $R_l=0.5$ m and drag coefficient ${C_d}_l=0.5$. An octarotor platform is considered, characterized by a set of eight contra-rotating propellers (see Figure 1). The cross-shaped planar configuration is characterized by ${STA}_{R_1}={STA}_{R_2}={STA}_{R_5}={STA}_{R_6}=-{STA}_{R_3}=-{STA}_{R_4}=-{STA}_{R_7}=-{STA}_{R_8}=-0.690$ m, ${BL}_{R_1}={BL}_{R_4}={BL}_{R_5}={BL}_{R_8}=-{BL}_{R_2}=-{BL}_{R_3}=-{BL}_{R_6}=-{BL}_{R_7}=0.690$ m, ${WL}_{R_1}={WL}_{R_2}={WL}_{R_3}={WL}_{R_4}=-0.024$ m, and ${WL}_{R_5}={WL}_{R_6}={WL}_{R_7}={WL}_{R_8}=-0.238$ m.

Rotor forces and moments are calculated by means of Blade Element Momentum Theory. With respect to the thrust and torque contributions only, a simplified model is proposed as in [15], such that

$$T_j=k_T{\Omega }_j^2,Q_j=k_Q{\Omega }_j^2$$

(15)

where $k_T=\gamma {\overline{k}}_T$ and $k_Q=\gamma {\overline{k}}_Q$. ${\overline{k}}_T$ and ${\overline{k}}_Q$ are positive constants determined experimentally at air density $\overline{\rho }$, while $\gamma =\rho /\overline{\rho }$ is a density scaling parameter. In the present case, upper rotors (1–4) are characterized by ${\left.{\overline{k}}_T\right|}_{up}=2.90·{10}^{-3}$ N/(rad/s)${}^2$, while lower rotors (5–8) present ${\left.{\overline{k}}_T\right|}_{down}=2.20·{10}^{-3}$ N/(rad/s)${}^2$, accounting for reduced efficiency due to wake interaction with upper rotors [37]. Finally, ${\overline{k}}_Q=1.25·{10}^{-4}$ Nm/(rad/s)${}^2$ at the reference density $\overline{\rho }=1.1229$ kg/m³. The characterization of rotor induced speed is performed by assuming a uniform inflow field. The computation is performed through the static model proposed in [32], which is numerically solved according to fzero Matlab^® routine by Dekker with initial condition for the induced speed equal to $-10$ m/s [38]. Air parameters are calculated from the International Standard Atmosphere (ISA) model as a function of altitude [39].

Although modern control approaches and technologies can be conceived to track a desired condition in complex scenarios [40], it is here assumed that guidance, navigation, and control tasks are performed, without loss of generality, through a programmable standard based on Pixhawk board (which integrates necessary sensors and algorithms for state estimation). In this respect, a cascaded controller shaped on PX4 architecture and notation [41] is considered to stabilize the octarotor according to different flight modes, from high-level position control to direct attitude regulation (see Figure 3). Provided a detailed analysis of PX4 control functions is out of the scope of the present paper, it is supposed that stabilization of the isolated vehicle is performed according to prescribed flying qualities through a dedicated tuning of control parameters. The dynamics of the single electric propulsion unit made of Electronic Speed Controller (ESC) and brushless motor are modeled by a first-order transfer function with time constant ${\tau }_{em}=0.06$ s, accounting for both electric and mechanical non-ideal effects. The transfer function transforms the desired value of angular rate into the actual value generated by the motor shaft.

Onboard computer is located in $ST{A}_{PIX}=B{L}_{PIX}=0$ m and $W{L}_{PIX}=-0.1$ m, with axes aligned with ${\mathcal{F}}_B$ axes. To simulate the effects of sensor noise, zero-mean Gaussian white noise is added to each Euler angle and angular velocity measurement, with standard deviations ${\sigma }_{\alpha }=0.5$ deg and ${\sigma }_{\omega }=0.1$ deg/s, respectively. The body-fixed accelerometer readings are obtained by adding white Gaussian noise with ${\sigma }_{acc}=0.0057$ m/s² and a constant bias vector ${\mathit{b}}_{acc}={[0.015,-0.01,0.002]}^T$ m/s² to the simulated values. Estimation error noise is also considered for vehicle position and velocity expressed in ${\mathcal{F}}_E$, where standard deviations ${\left.{\sigma }_p\right|}_{xy}=0.074$ m and ${\left.{\sigma }_v\right|}_{xy}=0.057$ m/s affect motion over the local-horizontal plane, while ${\left.{\sigma }_p\right|}_z=0.034$ m and ${\left.{\sigma }_v\right|}_z=0.036$ m/s characterize the vertical channel. Sampling is performed by a frequency of 250 Hz (sample time $\Delta t=0.004$ s), the same adopted for the generation of control signals and the state-estimation routine, which includes the integration of Equations (10) and (12) through a fourth-order Runge–Kutta solver [36].

Filter initial state is set to ${\widehat{\mathit{x}}}_0={\mathbf{0}}_{7\times 1}$ and the estimated covariance matrix is initialized with ${\mathit{P}}_0=\mathrm{diag}({10}^{-6},{10}^{-6},{10}^{-6},{10}^{-6},2,2,{10}^{-5})$. Let ${\mathit{I}}_m$ be the unit matrix with dimension m. The assigned noise covariance matrix is $\mathit{R}=3.6·{10}^{-5}·{\mathit{I}}_3$ m²/s⁴ and process noise is accounted by $\mathit{Q}=\mathrm{diag}({10}^{-7},{10}^{-7},{10}^{-7},{10}^{-7},1,1,{10}^{-7})$, both matrices assumed to be constant. Uncertainties on relevant filter parameters are also considered. In particular, the assumed payload mass is ${\widehat{m}}_l=0.9·m_l=90$ kg, with $-10\%$ error.

In order to minimize cable swing angles and rates, an auxiliary payload controller (PC) is proposed to generate a set-point acceleration vector ${\mathit{a}}_{sp}^{\left(PC\right)}$ with components expressed in ${\mathcal{F}}_E$. Such acceleration is added to the contribution ${\mathit{a}}_{sp}^{\left(VC\right)}$ determined by the velocity controller (VC) in Figure 3. With the aim to (1) generate the auxiliary control action or (2) to compare filter-estimated variables with potentiometer readings during experiments, swing angles and rates are conveniently estimated with respect to the local frame ${\mathcal{F}}_H$, which is obtained if z and u are projected in ${\mathcal{F}}_H$ (it is noted that, if $\psi$ is a constant or a slowly varying attitude parameter, ${\mathcal{F}}_H$ can be assumed to be an inertial frame). The auxiliary controller, based on a heuristic approach, assumes the form:

$${\mathit{a}}_{sp}^{\left(PC\right)}={\mathit{T}}_{EH}\left(\left[\begin{array}{ccc}{k_D}_{\zeta }& 0& 0\\ 0& {k_D}_{\xi }& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\dot{\zeta }}_H\\ {\dot{\xi }}_H\\ 0\end{array}\right]+\left[\begin{array}{ccc}{k_P}_{\zeta }& 0& 0\\ 0& {k_P}_{\xi }& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\zeta }_H\\ {\xi }_H\\ 0\end{array}\right]\right)$$

(16)

where ${\mathit{T}}_{EH}={\mathit{T}}_{BH}^T{\mathit{T}}_{BE}^T$ accounts for vector transformation between ${\mathcal{F}}_H$ and ${\mathcal{F}}_E$ and is defined by an elementary rotation about $z_H$ with amplitude $\psi$. Control gains are positive with values ${k_D}_{\xi }={k_D}_{\zeta }=2$ m/(rad s) and ${k_P}_{\xi }={k_P}_{\zeta }=9$ m/(rad s²).

4.1.2. Preliminary Definitions

Indicators are preliminarily defined with the aim to characterize the performance of simulation cases. The effect of payload active stabilization will be thus investigated and performance indices will assess the validity of the approach for each proposed maneuver. Five different indicators are introduced:

• Maneuver time, $t_m$. It is the total time required to conclude the considered maneuver, based on a stop criterion. In the present framework, the simulation is stopped when two conditions are met, namely: (1) the positioning error $\epsilon =∥{\mathit{p}}_{sp}-\mathit{p}∥$ falls below $0.1$ m and (2) cable oscillation angle $\chi \ge 0$ remains bounded below 1 deg for a prescribed time of 10 s. In the case when the considered maneuver does not require position stabilization, $t_m$ is calculated on the basis of condition 2 only. Cable oscillation angle $\chi$ is defined as the angle between the local vertical axis and the direction of the cable. Considering the nomenclature adopted in [15], $\chi$ is obtained from the dot product:

  $$\chi =arccos\left(z_H·\widehat{\mathit{c}}\right)$$

  (17)

  where $\widehat{\mathit{c}}=\mathit{c}/∥\mathit{c}∥$ is the unit vector directed from the suspension point to the payload.
• Average swing angle, $\overline{\chi }$. It is calculated through the integral

  $$\overline{\chi }={\displaystyle \frac{1}{t_m}}{\int }_0^{t_m}\chi \left(s\right)ds$$

  (18)

  over maneuver time domain. Although $\overline{\chi }$ correctly describes the overall oscillation behavior on average, the integral quantity $\overline{\chi }·t_m={\int }_0^{t_m}\chi \left(s\right)ds$ is also accounted to provide an indication of how much and how long the cable remains far from the vertical equilibrium condition.
• Average swing rate, $\overline{\nu }$. Let $\nu =\dot{\chi }$ be the swing rate, defined as the time derivative of $\chi \left(t\right)$. Then, $\overline{\nu }$ is calculated through the integral

  $$\overline{\nu }=\sqrt{{\displaystyle \frac{1}{t_m}}{\int }_0^{t_m}{\nu }^2\left(s\right)ds}$$

  (19)
  Similar to the case of $\overline{\chi }$, the integral quantity $\sqrt{{\int }_0^{t_m}{\nu }^2\left(s\right)ds}$ is also determined to provide a better indication of how much and how long the swing rate remains far from the equilibrium null condition.
• Average trajectory-tracking error, $\overline{d}$. It is calculated as the integral

  $$\overline{d}={\displaystyle \frac{1}{t_m}}{\int }_0^{t_m}d\left(s\right)ds$$

  (20)

  of the time-dependent variable $d\left(t\right)\ge 0$. In the present case, $d\left(t\right)$ describes the accuracy of point-to-point positioning maneuvers in the framework of waypoint navigation. Assume A and B are consecutive waypoints, respectively, identified by position vectors ${\mathit{p}}_A$ and ${\mathit{p}}_B$. $d\left(t\right)$ represents the distance at time t of the vehicle from the direction defined by vector ${\mathit{p}}_B-{\mathit{p}}_A$, calculated over the local-horizontal plane ${\mathit{x}}_E-{\mathit{y}}_E$.
• Total propulsive energy, $E_{prop}$. It is the mechanical energy delivered by electrical motors to propellers, evaluated from the integral

  $$E_{prop}={\int }_0^{t_m}\sum _{i=1}^8{P_{sh}}_i\left(s\right)ds$$

  (21)

  where ${P_{sh}}_i=Q_i{\Omega }_i$ is the output shaft power generated by the i–th motor, obtained from required torque $Q_i$ and angular rate ${\Omega }_i$ under the assumption of null friction torque.

4.1.3. Filter Validation

Different test cases are analyzed to validate both the estimation and the control strategies.

In the first case (‘1’), the octarotor starts from a hovering condition at $\mathit{p}\left(0\right)={[0,0,-30]}^T$ m with $\xi \left(0\right)=\zeta \left(0\right)=20$ deg and $\dot{\xi }\left(0\right)=\dot{\zeta }\left(0\right)=0$ deg/s and is required to keep the initial position in the absence of wind (${\mathit{p}}_{sp}=\mathit{p}\left(0\right)$). Stabilization of vehicle position, initially perturbed by payload non-null conditions, occurs with ${\mathit{a}}_{sp}^{\left(PC\right)}={\mathbf{0}}_{3\times 1}$ as in Figure 4, where positioning error ${\mathit{e}}_p={\mathit{p}}_{sp}-\mathit{p}={[{e_p}_1,{e_p}_2,{e_p}_3]}^T$ components are plotted over time.

Cable oscillations slowly decay under dissipative effects related to both vehicle speed stabilization and aerodynamic drag affecting the payload, provided maneuver time is $t_m=35.4$ s. With respect to filter performance, a comparison between the true and the estimated angle and rate is reported in Figure 5, showing convergence of the algorithm.

Assume the auxiliary payload controller is activated with control gains given above. Figure 6 shows the effectiveness of the approach based on the feedback of estimated variables, such that maneuver time is reduced to $t_m=23.9$ s and oscillation variables are rapidly damped. Cable swing angle $\chi$ reaches and remains below $5\%$ of the initial value in only $13.5$ s, provided the same condition is obtained in about $24.6$ s when active payload stabilization does not provide contribution.

With respect to other performance indicators,

Table 2. Case 1: summary of performance indicators.

Index	No Active PC	Active PC	Error [%]
$t_m$ [s]	35.4	23.9	−32.6
$\overline{\chi }$ [deg]	4.20	4.16	−1
$\overline{\nu }$ [deg/s]	5.53	5.82	+5.2
$\overline{d}$ [m]	0.33	0.63	+90.9
$E_{prop}$ [kJ]	820.7	553.9	−32.5

summarizes the main results and computes percentage errors. Note that the proposed maneuver is representative of a conservative test case, provided the filter is initialized with zero state estimate in the presence of non-zero real state. Such a condition is not encountered in practice and shows in full the phase before filter stabilization (thus affecting closed-loop performance). The activation of PC determines a strong reduction in the mechanical energy required to conclude the manueuver, which is determined by the faster decay of oscillation variables. Moreover, if one considers the quantity $\overline{\chi }·t_m$, which is the integral of oscillation angle over maneuver time $t_m$, a reduction of $-33.2\%$ is obtained. The same considerations hold for the integral quantity $\sqrt{{\int }_0^{t_m}{\nu }^2\left(s\right)ds}$, for which a reduction of $-13.7\%$ is calculated.

It is noteworthy that improved performance in terms of payload stabilization comes at the cost of increased $\overline{d}$ index ($+90.9\%$). From a physical standpoint, this was to be expected: since the overall system is underactuated, both the positioning and the payload control tasks are performed by making the octarotor simultaneously track the desired accelerations ${\mathit{a}}_{sp}^{\left(VC\right)}$ and ${\mathit{a}}_{sp}^{\left(PC\right)}$. The latter contribution is intentionally designed to force the controlled system to exhibit two-time-scale behavior as in [14], fast for the oscillation dynamics and slow for the velocity-tracking task. While determining time-scale separation, ${\mathit{a}}_{sp}^{\left(PC\right)}$ perturbs ${\mathit{a}}_{sp}^{\left(VC\right)}$ as in Figure 7, where the sign of the dot product ${\mathit{a}}_{sp}^{\left(VC\right)}·{\mathit{a}}_{sp}^{\left(PC\right)}$ is reported as a function of time, showing partial vector opposition in the very first part of the maneuver (when $t\le 6$ s). During such a phase, d reaches higher values and then rapidly decreases while taking benefits of fast payload stabilization.

In the second case (‘2’), the octarotor is requested to leave the initial hovering condition with $\xi \left(0\right)=\zeta \left(0\right)=0$ deg and $\dot{\xi }\left(0\right)=\dot{\zeta }\left(0\right)=0$ deg/s and track the set-point velocity $v_{sp}={[5,0,0]}^T$ m/s for $t<30$ s and $v_{sp}={[0,0,0]}^T$ m/s for $t\ge 30$ s, with components expressed in ${\mathcal{F}}_E$. A horizontal north wind with constant 1 m/s intensity is assumed to oppose rotorcraft motion.

Performance indicators are summarized in

Table 3. Case 2: summary of performance indicators.

Index	No Active PC	Active PC	Error [%]
$t_m$ [s]	66.9	49.0	−26.8
$\overline{\chi }$ [deg]	3.99	2.66	−33.3
$\overline{\nu }$ [deg/s]	5.04	3.86	−23.4
$\overline{d}$ [m]	N/A	N/A	N/A
$E_{prop}$ [kJ]	1 553.1	1 136.5	−26.8

, showing the beneficial effect of ${\mathit{a}}_{sp}^{\left(PC\right)}$. If one considers $\overline{\chi }·t_m$, a reduction of $-51.1\%$ is obtained, while the integral quantity $\sqrt{{\int }_0^{t_m}{\nu }^2\left(s\right)ds}$ is reduced by $-34.4\%$. Stabilization of octarotor speed is depicted in Figure 8, where ${\mathit{e}}_v={\mathit{v}}_{sp}-\mathit{v}={[{e_v}_1,{e_v}_2,{e_v}_3]}^T$ is plotted over time with components in ${\mathcal{F}}_E$. A comparison between the true and the estimated $\zeta$ angle and rate is reported in Figure 9.

The estimation of disturbance force ${\widehat{\mathit{f}}}_a={\widehat{\mathit{x}}}_{5:7}$ represents a fundamental task in the present case, where performance of forward flight in the presence of wind is significantly affected by octarotor frame drag and rotor in-plane forces. In Figure 10, estimated disturbance forces are compared to exact variables. It can be noted how EKF is able to track the force components with satisfactory accuracy after the required transients where both vehicle speed and EKF outputs oscillate prior to stabilization. A residual estimation error characterizes the steady-state response of the algorithm (for example, the estimated ${f_a}_2$ component sets on a constant value equal to $-1.3$ N, different from the expected zero value, as obtained by averaging filter output between 20 s and 30 s). Such a behavior is traceable to the uncertainty in the knowledge of payload mass and the presence of accelerometer bias.

Both non-ideal effects influence algorithm performance in two ways: (1) directly, through EKF equations detailed in Appendix A, and (2) indirectly, through input reconstruction as described in Remark 1, where system data and measurement z are used to estimate the generated thrust amplitude. However, it must be pointed out that the considered simulation setup comes under a conservative approach. First, most commercial off-the-shelf devices, such as Pixhawk platform, already include bias-compensation routines. With respect to the uncertainty on the knowledge of payload or cable parameters, it is recommended to update the filter setup according to the particular flight configuration, provided payload data are preliminarily measured or (possibly) online-estimated [42]. It is the case, for example, when the movement of goods is performed in large storehouses, where scheduling of system parameters, filter covariance matrix Q, and control gains can be modified according to the particular object to relocate. On the other hand, when payload mass is uncertain and cannot be estimated with sufficient accuracy, automated filter tuning techniques can be envisaged, provided that the adaptation of Q is adopted for (at least partial) mitigation of model uncertainties [43].

In the third case (‘3’), the octarotor starts from the same condition of Case 2; it is requested to track a set of prescribed waypoints $W{P}_i$, $i=1,\dots ,7$ (see

Table 4. Case 3: waypoint list (components expressed in ${\mathcal{F}}_E$).

i-th Waypoint	${\mathit{x}}_{\mathit{i}}$ [m]	${\mathit{y}}_{\mathit{i}}$ [m]	${\mathit{z}}_{\mathit{i}}$ [m]
1	0	0	−30
2	10	0	−50
3	20	−10	−50
4	20	10	−50
5	30	0	−50
6	10	0	−50
7	0	0	−30

) and perform a return-to-home maneuver. Each target waypoint is assumed to be reached when its distance from the vehicle decreases below $1.5$ m. The first line of Table 4), relative to the first waypoint, coincides with the initial octarotor position.

During the mission, gusts are generated to perturb the state of both the octarotor and the payload. The adopted turbulence model, implemented by a dedicate block available from Simulink Aerospace Blockset, adopts the Von Kármán spectral representation to add turbulence by passing band-limited white noise through appropriate forming filters. In particular, the block treats the linear and angular velocity components of continuous gusts as spatially varying stochastic processes and specifies each component’s power spectral density [44]. The approach is based on the mathematical representation in the Military Specification MIL–F–8785C and Military Handbook MIL–HDBK–1797B, respectively, provided in [45,46]. The following parameters are set:

• specification: MIL-F-8785C;
• model type: continuous Von Kármán (+q +r);
• wind speed at 6 m (used to define the low-altitude wind intensity): 10 m/s;
• wind direction at 6 m (degrees clockwise from north): 90 deg;
• scale length at medium/high altitudes: $533.4$ m (default value);
• band-limited noise sample time: $0.002$ s;
• MR wingspan: $2.38$ m (here intended as the maximum octarotor frontal size);
• load wingspan: 1 m (here intended as the maximum load frontal size).

It is noted that the Simulink-based Von Kármán model is well defined for fixed-wing aircraft, for which non-null speed conditions are expected in flight. This is not the case of rotary-wing platforms, where the possibility to perform hovering flight makes the wind model not suitable for application. A conservative approach is, however, adopted, where required input speed to the model is calculated as the sum of octarotor actual speed and the maximum desired speed (10 m/s) that the position autopilot of PX4 algorithm is allowed to generate. In addition to the considered turbulence contribution, a horizontal north wind with constant 8 m/s intensity is considered. By taking into account such severe atmospheric conditions, the termination criterion in Section 4.1.2 is conveniently relaxed. In particular, the maneuver is assumed to be concluded when (1) positioning error $\epsilon$ with respect to the last waypoint falls below $0.5$ m and (2) cable oscillation angle $\chi$ remains bounded below 5 degrees for a prescribed time of 10 s.

In Figure 11, the trajectory followed by the octarotor is reported for the case when ${\mathit{a}}_{sp}^{\left(PC\right)}={\mathbf{0}}_{3\times 1}$. Figure 12 depicts wind contributions in terms of linear velocities and angular rates, with components projected in ${\mathcal{F}}_E$. The obtained performance indicators are reported in the second column of

Table 5. Case 3: summary of performance indicators.

Index	No Active PC	Active PC (EKF)	Active PC (FGDF)
$t_m$ [s]	140.3	139.8 (−0.4%)	140.3 (+0%)
$\overline{\chi }$ [deg]	2.22	1.71 (−23.0%)	1.77 (−20.3%)
$\overline{\nu }$ [deg/s]	1.86	1.21 (−34.9%)	1.27 (−31.7%)
$\overline{d}$ [m]	6.49	6.48 (−0.2%)	6.52 (+0.5%)
$E_{prop}$ [kJ]	3242.5	3230.1 (−0.4%)	3249.9 (+0.2%)

.

Assume that PC is activated based on the closed-loop feedback of EKF-estimated variables. The third column of Table 5 summarizes the main results and computes percentage errors with respect to the case when no active PC is adopted. As expected, no significant difference characterizes the comparison in terms of both maneuver time and energy, provided that waypoint transition is performed only on the basis of positioning error, with no particular requirement on residual cable swing angle (except for the final waypoint). It is a fact, however, that $\overline{\chi }$ and $\overline{\nu }$ are, respectively, reduced by $23\%$ and $34.9\%$, showing improved behavior of swing condition with comparable trajectory-tracking parameter performance, $\overline{d}$ ($-0.2\%$).

The proposed approach, based on a nonlinear extended-state estimator, is further validated in Case 3 with respect to the FGDF solution provided in [28], whose same nomenclature and definitions are adopted. In such a framework, the reduced-order swing state ${\mathit{x}}^{FGDF}={[\xi ,\zeta ,\dot{\xi },\dot{\zeta }]}^T\in {\mathbb{R}}^4$ approximately evolves according to ${\dot{\mathit{x}}}^{\left(FGDF\right)}=\mathit{A}{\mathit{x}}^{\left(FGDF\right)}+\mathit{B}{\mathit{u}}^{FGDF}$, which are system dynamics obtained from linearization of Equations (10)–(14) in [28] about the hovering condition. ${\mathit{u}}^{FGDF}\in {\mathbb{R}}^2$ accounts for the first two components of u, expressed in ${\mathcal{F}}_H$. By accounting for the same model uncertainties described above, matrices A and B result to be, respectively:

$$\mathit{A}=\left[\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& 1\\ -1.4943& 0& 0& 0\\ 0& -1.4943& 0& 0\end{array}\right],\mathit{B}={10}^{-4}·\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 9.524\\ -9.524& 0\end{array}\right]$$

(22)

In terms of difference equations, the true state vector ${\mathit{x}}_k^{\left(FGDF\right)}$ at time k propagates according to ${\mathit{x}}_{k+1}^{\left(FGDF\right)}={\mathbf{\Phi }}_k{\mathit{x}}_k^{\left(FGDF\right)}+{\mathbf{\Gamma }}_k{\mathit{u}}_k^{\left(FGDF\right)}$. The state-transition matrix for the estimation algorithm is obtained as ${\mathbf{\Phi }}_k=\mathbf{\Phi }=exp\left(\mathit{A}\Delta t\right)$ and implemented by means of Matlab^® matrix exponential function $expm(·)$, according to the method in [47]:

$$\mathbf{\Phi }=\left[\begin{array}{cccc}1& 0& 0.004& 0\\ 0& 1& 0& 0.004\\ -0.006& 0& 1& 0\\ 0& -0.006& 0& 1\end{array}\right]$$

(23)

The control-input model is calculated as ${\mathbf{\Gamma }}_k=\mathbf{\Gamma }={\mathit{A}}^{-1}(\mathbf{\Phi }-{\mathit{I}}_m)\mathit{B}$, namely:

$$\mathbf{\Gamma }={10}^{-6}·\left[\begin{array}{cc}0& 0.008\\ -0.0088& 0\\ 0& 3.81\\ -3.81& 0\end{array}\right]$$

(24)

The observation $z_k^{\left(FGDF\right)}={[{\xi }_{syn},{\zeta }_{syn}]}_k^T\in {\mathbb{R}}^2$, derived from simple acceleration information as in [28], approximately tracks the dynamic process as in $z_k^{\left(FGDF\right)}={\mathit{H}}^{\left(FGDF\right)}{\mathit{x}}_k^{\left(FGDF\right)}$, where

$${\mathit{H}}^{\left(FGDF\right)}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$$

(25)

is the observation matrix. Synthetic angle measurements in $z_k^{\left(FGDF\right)}$ are corrupted by noise ${\mathit{\nu }}_k^{\left(FGDF\right)}\in {\mathbb{R}}^2$ with variance ${\mathit{R}}^{\left(FGDF\right)}$, namely $z_k^{\left(FGDF\right)}={\mathit{H}}^{\left(FGDF\right)}{\mathit{x}}_k^{\left(FGDF\right)}+{\mathit{\nu }}_k^{\left(FGDF\right)}$. In the present case, it is ${\mathit{R}}^{\left(FGDF\right)}=2.465·{10}^{-4}·{\mathit{I}}_2$ rad².

For the sake of brevity, the tuning and scaling procedure described in [28] to find the globally optimum FGDF configuration is not reported. The final results are directly provided, where $\beta ={\beta }_T=0.998$ represents the best fading factor at which the estimated measurement noise covariance matrix ${\widehat{\mathit{R}}}^{\left(FGDF\right)}$ satisfies ${\widehat{\mathit{R}}}^{\left(FGDF\right)}\approx {\mathit{R}}^{\left(FGDF\right)}$. EKF and FGDF algorithms are switched on in parallel during the maneuver of Case 3, while no active payload stabilization is performed. FGDF is initialized with null state vector and nominal error covariance ${\left({\mathit{E}}^{-}\right)}_0^{\left(FGDF\right)}={10}^{-7}·{\mathit{I}}_4$.

Figure 13, Figure 14, Figure 15 and Figure 16 depict the output of the two filters in terms of swing angles and rates. Plot details are also reported to highlight the differences between the results of estimation methods. Improved performance characterizes EKF for all considered variables, especially when high oscillation angles and rates occur (see, for example, what happens during the time interval $40\le t\le 60$ s, when position controller drives the vehicle from $W{P}_3$ to $W{P}_4$ and excites lateral payload oscillations) or when near-hover flight in the presence of wind is performed. The latter case shows up during time interval $120\le t\le 130$ s, when the octarotor is required to keep the position represented by $W{P}_7$ with non-null attitude to counteract the steady wind contribution. During this terminal flight phase, an evident estimation bias characterizes the output of the linear algorithm. It is noteworthy that the estimation bias of FGDF particularly affects the frontal angle $\zeta$ and rate $\dot{\zeta }$, as in Figure 15 and Figure 16. Such a behavior is determined by the presence of a dominant wind contribution from the north that induces disturbance forces on both the vehicle and the suspended load. For the sake of completeness, it is pointed out that estimation error on $\zeta$ also characterizes the EKF output (although to a lesser extent). Such a drawback comes after neglecting payload drag in both the estimation methods, thus introducing model uncertainty. However, overall EKF performance remains satisfactory and proves to be preferable to that obtained in the linear framework.

In the case when PC is activated based on FGDF information, the mission is performed with indicators listed in the fourth column of Table 5. Despite the macroscopic drawbacks of the FGD approach highlighted through Figure 13, Figure 14, Figure 15 and Figure 16, active payload stabilization still shows to be a successful strategy (although with degraded results in terms of $\overline{\chi }$ and $\overline{\nu }$).

4.2. Experimental Results

In order to evaluate the proposed method in a real mission scenario, an experimental campaign is performed by using a Holybro PX4 Development Kit X500 V2 quadrotor with take-off mass $m=2$ kg and a wheelbase of $0.5$ m (see Figure 17a). The avionics is represented by a Pixhawk^® 6X Autopilot Flight Controller with M8N GPS. The propulsion system is made of 4 BLHeli S ESC 20A compatible with 4S Li–Po Battery, 4 Holybro 2216 KV920 motors, and a planar set of 1045 propellers. The vehicle is equipped with a thin woven rope with nominal length $L=1.9$ m, suspended by a hook positioned $0.22$ m below the quadrotor top surface (${STA}_H={BL}_H=0$ m and ${WL}_H=-0.22$ m, see Figure 17b). The hook is fixed to a two-axis potentiometer system whose signals, sampled at 20 Hz, are acquired by the onboard computer, converted to angles relative to ${\mathcal{F}}_B$, and corrected by quadrotor attitude information to provide direct measurement of oscillation angles $\xi$ and $\zeta$ with respect to the local frame ${\mathcal{F}}_H$. The payload is a ball with radius $0.03$ m and mass $m_l=0.192$ kg.

Flight tests are performed outdoors with an environmental temperature of 8 °C and a static pressure of 1020 hPa in the presence of light-unsteady 30 deg NE wind. The quadrotor is piloted by imposing desired speed components expressed in ${\mathcal{F}}_H$ and a desired yaw angle through the remote controller. The EKF and the FGDF algorithms run simultaneously on the Pixhawk device with a frequency of 100 Hz, and no active payload stabilization is performed. With respect to the EKF agorithm, estimated covariance matrix is initialized with ${\mathit{P}}_0=\mathrm{diag}(2.2·{10}^{-5},2.2·{10}^{-5},1.3·{10}^{-4},1.3·{10}^{-4},4.3·{10}^{-5},4.3·{10}^{-5},4.3·{10}^{-5})$, and the assigned noise covariance matrix is $\mathit{R}=3.6·{10}^{-5}·{\mathit{I}}_3$ m²/s⁴ (the same adopted in Section 4.1). The numerical integration of Equations (10) and (12) is performed through a discrete backward Euler method [36]. With respect to the FGDF algorithm, it is ${\mathit{R}}^{\left(FGDF\right)}=2.465·{10}^{-4}·{\mathit{I}}_2$ rad² and ${\left({\mathit{E}}^{-}\right)}_0^{\left(FGDF\right)}=\mathrm{diag}(2.2·{10}^{-5},2.2·{10}^{-5},1.3·{10}^{-4},1.3·{10}^{-4})$, while the state transition matrix and the control-input models are, respectively

$$\mathbf{\Phi }=\left[\begin{array}{cccc}1& 0& 0.01& 0\\ 0& 1& 0& 0.01\\ -0.057& 0& 1& 0\\ 0& -0.057& 0& 1\end{array}\right],\mathbf{\Gamma }={10}^{-3}·\left[\begin{array}{cc}0& 0.013\\ -0.013& 0\\ 0& 2.64\\ -2.64& 0\end{array}\right]$$

(26)

Both filters are initialized with null state and the parameters that optimize estimation performance are, respectively, ${\beta }_T=0.996$ (FGDF) and $\mathit{Q}=\mathrm{diag}(1,1,2,2,1,1,1)·{10}^{-5}$ (EKF), obtained after an intensive experimental campaign.

In Figure 18, a first maneuver (Maneuver 1) is described where the quadrotor is required to track a desired lateral speed of $\pm 3$ m/s along ${\mathit{y}}_H$ with a hovering flight phase in between. A lateral payload oscillation is excited according to Figure 19a, where the time history of lateral oscillation angle is reported. The corresponding angular rate is plotted in Figure 19b after numerical derivation and low-pass filtering of potentiometer readings. EKF-based data show very good agreement with direct measurements in all phases, with some over-estimation tendency during transients. FGDF-based data result to be less accurate with a biased behavior that becomes evident when the quadrotor is stabilized in the hovering condition and even deteriorates when negative lateral speed is achieved. During the maneuver, a disturbance force is observed with values projected over the local horizontal plane equal to $-1.5$ N and $-0.7$ N on average, respectively, rotated back to ${\mathit{x}}_E$ and ${\mathit{y}}_E$. Such results are compatible with the observed direction of wind.

In Figure 20 and Figure 21, a second maneuver (Maneuver 2) is described to specifically excite headwind payload oscillations. Stabilization of speed along ${\mathit{x}}_H$ is represented in Figure 20, where the quadrotor starts from a non-nominal hovering condition with residual payload oscillations ($440\le t\le 468$ s) and is then required to track a time-dependent speed profile between $-2$ and $+3$ m/s ($t>468$ s). Satisfactory performance characterizes EKF results during the entire maneuver, according to Figure 21. With regard to FGDF data, acceptable results are obtained during the first phase, despite an evident overestimation of $\zeta$ angle near hover. Performance is evidently degraded during the final high-agility maneuver.

5. Conclusions

In the present paper, an estimation algorithm is proposed to characterize the swing state of a suspended load carried by a multirotor drone. The approach is based on a recursive Kalman formulation where vehicle acceleration measurements are fused with model information in a nonlinear framework. Equations are derived for a double point mass system with a mass-less and rigid cable. Numerical simulations demonstrate the validity of the approach in a realistic scenario, where a highly nonlinear octarotor vehicle is analyzed in the presence of system uncertainties, external disturbances, and cable elasticity. The nonlinearity of the approach and the possibility to estimate and compensate external disturbances make the filter suitable for inclusion in a closed-loop stabilization framework, where active oscillation damping is shown to determine the reduction in both maneuver time and battery power consumption.

The proposed algorithm is implemented in a real quadrotor platform and an outdoor experimental campaign is performed to address the accuracy of estimation and demonstrate improved performance with respect to that obtainable through a recent work by one of the authors. The approach has the merit of relative simplicity and can be extended for application to any rotorcraft configuration with minimum effort. The results obtained by both numerical and experimental tests prove to be encouraging for future drone delivery scenarios where the adoption of auxiliary instrumentations or ad hoc indoor facilities is not considered.