Self-Localization of Tethered Drones without a Cable Force Sensor in GPS-Denied Environments

Abstract

This paper considers the self-localization of a tethered drone without using a cable-tension force sensor in GPS-denied environments. The original problem is converted to a state-estimation problem, where the cable-tension force and the three-dimensional position of the drone with respect to a ground platform are estimated using an extended Kalman filter (EKF). The proposed approach uses the data reported by the onboard electric motors (i.e., the pulse width modulation (PWM) signals), accelerometers, gyroscopes, and altimeter, embedded in the commercial-of-the-shelf (COTS) inertial measurement units (IMU). A system-identification experiment was conducted to determine the model that computes the drone thrust force using the PWM signals. The proposed approach was compared with an existing work that assumes known cable-tension force. Simulation results show that the proposed approach produces estimates with less than 0.3-m errors when the actual cable-tension force is greater than 1 N.

1. Introduction

Tethered drones have been witnessed in various applications, such as surveillance [1,2,3,4], high-rise building cleaning [5,6], infrastructure monitoring [7], wind turbine cleaning and de-icing [8,9], and firefighting [10]. Although the tether would limit the reachable space of the drone compared to a free-flying drone, it offers unique persistent and secured data transmission link and electricity power to the drone [1]. The potential combination of the tether with a hose also provides capabilities of delivering fluid to a targeting area, such as, spraying pesticides on a crop field [11]. The effective use of tethered drones in these applications requires accurate self-localization information. For example, for surveillance/monitoring applications, the meter-level self-localization accuracy would be acceptable, while for applications such as agricultural chemical spraying and wind-turbine and high-rise-building cleaning, the decimeter/centimeter-level accuracy for self-localization would be preferred.

Small drones notably rely on accurate self-location information for guidance, navigation, and control. Drone self-localization typically counts on IMUs [12,13,14], the Global Positioning System (GPS) [15] (differential GPS [16]), infrared (IR) sensors [17], laser rangefinders [18,19], and optical and vision systems [20,21,22]. While these sensing systems have successfully supported outdoor applications, extensive investment has been made to enhance the capability of self-localization for drone by improving the GPS infrastructure, utilizing cellular network infrastructure [23], or integrating both technologies for a wider range of applications. However, the self-localization of small drones in GPS-degraded/-denied environments (e.g., indoors and street canyons) is still challenging due to their limited size, payload, power, and flight endurance that have prevented them from carrying high-end sensors for self-localization. This poses critical concerns to the safe operation of drones in GPS-degraded/-denied environments.

In previous studies for the self-localization and control of tethered drones, Lupashin and D’Andrea [24] presented an approach to estimating the two-dimensional (2D) location of the drone with respect to a ground station. Tognon and Franchi [25] presented an observer-based control technique to regulate a tethered drone attached to a moving ground platform. Lima and Pereira [26] presented an EKF-based self-localization approach by assuming a catenary-shape cable for a static drone in hovering and assuming that the cable-tension force is known. Companies have also commercialized tethered drones on the market [27,28]. In our previous work [29,30], we presented both a low pass filter (LPF) and an extended Kalman filter (EKF) to estimate the three-dimensional (3D) location of the drone with respect to a ground platform (see Figure 1) while assuming known cable-tension force. In this paper, we assume the cable-tension force is unknown and we extend our previous work by enabling simultaneously estimation of both the 3D drone location and the cable-tension force, using only the measurements of onboard IMUs and altimeter.

To the best of our knowledge, existing literature [24,25,26,27,28,29,30] for the self-localization and control of tethered drones has assumed known cable-tension force and accurate drone thrust forces, which, however, are nontrivial to measure directly. The cable-tension force is usually assumed to be measured by a force sensor that is connected in series with the tether. Connecting a COTS cable-tension force sensor underneath the drone will significantly increase the payload of the drone. Connecting the force sensor on the ground platform would be extremely challenging when the tether length varies with the drone movement. The drone thrust force is usually computed using the pulse width modulation (PWM) signals, but such a computational formula is not usually provided by a drone manufacturer, and it is usually unique for each particular drone. Existing work for computing the motor thrust using a PWM signal has focused on identifying the coefficients of a high-order polynomial of the PWM signal using a load cell to measure the thrust force [31,32,33,34]. However, setting up such experiments by attaching load cells to the drone motors requires considerable efforts of disassembling drone components. To the best of our knowledge, this paper presents one of the first works that apply the system-identification technique to model the relationship between the motor thrust and PWM signals without disassembling the drone, but only using real flight-test data.

The contribution of this paper includes the development of an EKF that enables the estimation of both the 3D position of a moving drone with respect to a ground platform and the cable-tension force, and the development of a system-identification method to compute the motor thrust force using the PWM signal. The measurements used for the proposed EKF are assumed to be measured by the onboard inertial sensors (e.g., accelerometers and gyroscopes), along with the altimeter (e.g., an ultrasound sensor). We evaluate the proposed EKF in simulations in comparison to the 3-state EKF in [29]. The result shows that when the actual cable-tension force is greater than 1 N, the proposed 4-state EKF produces estimates with less than 0.3-N estimation errors, which are equivalent to the performance of the technique, assuming a known cable-tension force [29].

The remainder of this paper is structured as follows. System dynamics and acelerometer principles are introduced in Section 2. The problem statement and state-space model are introduced in Section 3. The EKF development and system identification for motor coefficients are presented in Section 4 and Section 5, respectively. Section 6 shows and discusses the simulation results, and Section 7 concludes the paper. Section 8 presents our future work.

2. System Dynamics and Accelerometer Principles

2.1. Coordinate Frames

We first introduce several key coordinate frames associated with the system dynamics of a drone, i.e., the inertial frame, the vehicle frame, and the body frame [35], as shown in Figure 1.

2.1.1. The Inertial Frame ${\mathcal{F}}^i$

The inertial coordinate frame is an earth-fixed coordinate system with its origin at a pre-defined location. In this paper, this coordinate system is referred to in the North-East-Down (NED) reference frame. It is common for North to be referred to as the inertial x direction, East to the y direction, and Down to the z direction.

2.1.2. The Vehicle Frame ${\mathcal{F}}^v$

The origin of the vehicle frame is at the center of mass of a drone. However, the axes of ${\mathcal{F}}^v$ are aligned with the axes of the inertial frame ${\mathcal{F}}^i$. In other words, the unit vector ${\mathbf{i}}^v$ points toward North, ${\mathbf{j}}^v$ toward East, and ${\mathbf{k}}^v$ toward the center of the earth.

2.1.3. The Body Frame ${\mathcal{F}}^b$

The body frame is obtained by rotating the vehicle frame in a right-handed rotation about ${\mathbf{i}}^v$ by the roll angle, $\varphi$, about the ${\mathbf{j}}^v$ axis by the pitch angle, $\theta$ and about the ${\mathbf{k}}^v$ axis by the yaw angle, $\psi$. The transformation of the drone 3D position from ${\mathbf{p}}^b$ in ${\mathcal{F}}^v$ to ${\mathbf{p}}^v$ in ${\mathcal{F}}^b$ is given by

$${\mathbf{p}}^b=R_v^b\left(\varphi ,\theta ,\psi \right){\mathbf{p}}^v,$$

(1)

where the transformation matrix, $R_v^b(\varphi ,\theta ,\psi )$, is given by

$$\begin{array}{cc}\hfill R_v^b(\varphi ,\theta ,\psi )& =\left(\begin{array}{ccc}{\mathrm{c}}_{\theta }{\mathrm{c}}_{\psi }& {\mathrm{c}}_{\theta }{\mathrm{s}}_{\psi }& -{\mathrm{s}}_{\theta }\\ {\mathrm{s}}_{\varphi }{\mathrm{s}}_{\theta }{\mathrm{c}}_{\psi }-{\mathrm{c}}_{\varphi }{\mathrm{s}}_{\psi }& {\mathrm{s}}_{\varphi }{\mathrm{s}}_{\theta }{\mathrm{s}}_{\psi }+{\mathrm{c}}_{\varphi }{\mathrm{c}}_{\psi }& {\mathrm{s}}_{\varphi }{\mathrm{c}}_{\theta }\\ {\mathrm{c}}_{\varphi }{\mathrm{s}}_{\theta }{\mathrm{c}}_{\psi }+{\mathrm{s}}_{\varphi }{\mathrm{s}}_{\psi }& {\mathrm{c}}_{\varphi }{\mathrm{s}}_{\theta }{\mathrm{s}}_{\psi }-{\mathrm{s}}_{\varphi }{\mathrm{c}}_{\psi }& {\mathrm{c}}_{\varphi }{\mathrm{c}}_{\theta }\end{array}\right),\hfill \end{array}$$

(2)

where ${\mathrm{c}}_{*}={\cos }_{*}$ and ${\mathrm{s}}_{*}={\sin }_{*}$.

2.2. Tethered Drone Dynamics

The equations of motion of a drone tethered to a stationary ground station are expressed by a six-degree-of-freedom model consisting of 12 states [35]

$$\begin{array}{cc}\hfill \left(\begin{array}{c}{\dot{p}}_n\\ {\dot{p}}_e\\ {\dot{p}}_d\end{array}\right)& =R_b^v(\varphi ,\theta ,\psi )\left(\begin{array}{c}u\\ v\\ w\end{array}\right),\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill \left(\begin{array}{c}\dot{u}\\ \dot{v}\\ \dot{w}\end{array}\right)& =\left(\begin{array}{c}rv-qw\\ pw-ru\\ qu-pv\end{array}\right)+\frac{1}{m}\left(\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right),\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill \left(\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right)& =\left(\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \frac{\sin \varphi }{\cos \theta }& \frac{\cos \varphi }{\cos \theta }\end{array}\right)\left(\begin{array}{c}p\\ q\\ r\end{array}\right),\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill \left(\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right)& =\left(\begin{array}{c}\frac{J_y-J_z}{J_x}qr\\ \frac{J_z-J_x}{J_y}pr\\ \frac{J_x-J_y}{J_z}pq\end{array}\right)+\left(\begin{array}{c}\frac{1}{J_x}{\tau }_l\\ \frac{1}{J_y}{\tau }_m\\ \frac{1}{J_z}{\tau }_n\end{array}\right),\hfill \end{array}$$

(6)

where ${\left(p_n,p_e,p_d\right)}^T\in {\mathbb{R}}^3$ is defined as the drone position in the NED inertial frame, $\left(u,v,w\right)$ is the drone linear velocity vector in the body frame, m is the drone mass, $\left(p,q,r\right)$ is the rotational velocity vector in the body frame, $(f_x,f_y,f_z)$ and $({\tau }_l,{\tau }_m,{\tau }_n)$ are the total external forces and torques applied to the drone in the body frame, respectively, and $J_x$, $J_y$, and $J_z$ are moments of inertia of the drone in x, y, and z directions, respectively.

2.3. Accelerometer Principle

The output of COTS accelerometers for drones contains several specific terms that are derived from the drone acceleration and are important for drone controller design and analysis. In this subsection, the normalized kinematic accelerations and specific forces [36] are introduced, which are used in the proposed self-localization methodology.

Kinematic Accelerations and Specific Forces

Let $\eta ={\left(u,v,w\right)}^T$ be the linear velocity vector, $\Omega ={\left(p,q,r\right)}^T$ be the rotational velocity vector of the drone in the body frame, and ${\mathbf{f}}^b={\left(f_x,f_y,f_z\right)}^T$ be the total external force vector in the body frame. Define the kinematic acceleration vector ${\mathbf{a}}_k^b\triangleq {\left({\mathbf{a}}_{k,x}^b,{\mathbf{a}}_{k,y}^b,{\mathbf{a}}_{k,z}^b\right)}^T$ in the body frame as

$${\mathbf{a}}_k^b=\frac{{\mathbf{f}}^b}{mg}=\frac{\dot{\eta }}{g}=\frac{1}{g}\left(\frac{\partial \eta }{\partial t}+\Omega \times \eta \right),$$

(7)

of which the components are

$${\mathbf{a}}_{k,x}^b=\frac{1}{g}(\dot{u}+qw-rv)=\frac{f_x}{mg},$$

(8)

$${\mathbf{a}}_{k,y}^b=\frac{1}{g}(\dot{v}+ru-pw)=\frac{f_y}{mg},$$

(9)

$${\mathbf{a}}_{k,z}^b=\frac{1}{g}(\dot{w}+pv-qu)=\frac{f_z}{mg},$$

(10)

where g is the gravitational acceleration constant on Earth. Note that ${\mathbf{a}}_k^b$ is in units of g. The accelerometer is assumed to be mounted at the center of gravity of a drone.

The output of accelerometers used by drone autopilots is generated in the form of the specific force, ${\mathbf{a}}_{SF}^b$, also called g-force or mass-specific force (measured in meters/second²), which is actually an acceleration ratio given by

$${\mathbf{a}}_{SF}^b=\frac{{\mathbf{f}}^b-{\mathbf{f}}_g^b}{mg}={\mathbf{a}}_k^b-\frac{{\mathbf{f}}_g^b}{mg},$$

(11)

whose components are given by

$$\begin{array}{ccc}\hfill {\mathbf{a}}_{SF,x}^b& =& {\mathbf{a}}_{k,x}^b+\sin \theta ,\hfill \end{array}$$

(12)

$$\begin{array}{ccc}\hfill {\mathbf{a}}_{SF,y}^b& =& {\mathbf{a}}_{k,y}^b-\cos \theta \sin \varphi ,\hfill \end{array}$$

(13)

$$\begin{array}{ccc}\hfill {\mathbf{a}}_{SF,z}^b& =& {\mathbf{a}}_{k,z}^b-\cos \theta \cos \varphi .\hfill \end{array}$$

(14)

2.4. External Forces of Tethered Drone

The total external force vector for a tethered drone in the body frame is given by

$${\mathbf{f}}^b={\mathbf{f}}_{thrust}^b+{\mathbf{f}}_g^b+{\mathbf{f}}_{cable}^b,$$

(15)

where ${\mathbf{f}}_{thrust}^b$ is the thrust force, ${\mathbf{f}}_g^b$ is the gravity force, and ${\mathbf{f}}_{cable}^b$ is the cable-tension force, all in the body frame. The gravity force vector of the drone in the vehicle frame, ${\mathbf{f}}_g^v$, is given by

$${\mathbf{f}}_g^v=\left(\begin{array}{c}0\\ 0\\ mg\end{array}\right).$$

(16)

Then, we have

$${\mathbf{f}}_g^b=R_v^b{\mathbf{f}}_g^v=\left(\begin{array}{c}-mg\sin \theta \\ mg\cos \theta \sin \varphi \\ mg\cos \theta \sin \theta \end{array}\right).$$

(17)

The thrust force vector in the body frame is given by

$${\mathbf{f}}_{thrust}^b=\left(\begin{array}{c}{f}_{thrust,x}\\ f_{thrust,y}\\ f_{thrust,z}\end{array}\right)=\left(\begin{array}{c}0\\ 0\\ -(f_F+f_R+f_B+f_L)\end{array}\right),$$

(18)

where subscripts $F,R,B$, and L denote the thrust forces provided by the front, right, back, and left motors, respectively. The individual thrust forces have been calculated using the PWM signals commanded to the motors, such as,

$$f_{*}=k_{motor}·{\mathrm{pwm}}_{*},$$

(19)

where $*\in \{F,R,B,L\}$ and $k_{motor}$ is the electric motor coefficient and ${\mathrm{pwm}}_{*}$ is the PWM motor control signal. However, the mapping between the drone motor thrust force and the PWM signals is much more complicated than the linear relationship shown in (19). We will discuss this more in Section 5.

Since the output of the accelerometer is the total acceleration (see Equation (11)) minus the gravity terms [35]

$${\mathbf{a}}_{SF}^b=\frac{{\mathbf{f}}^b-{\mathbf{f}}_g^b}{mg}={\mathbf{a}}_k^b-\frac{{\mathbf{f}}_g^b}{mg}=\frac{{\mathbf{f}}_{thrust}^b+{\mathbf{f}}_{cable}^b}{mg},$$

(20)

assuming a taut cable, ${\mathbf{f}}_{cable}^b$ is given by

$${\mathbf{f}}_{cable}^b=R_v^b{f}_{cable}^v\frac{L}{\ell },$$

(21)

where $L={(p_n,p_e,p_d)}^T$, $\ell =\sqrt{p_n^2+p_e^2+p_d^2}$, and $f_{cable}^v$ is the magnitude of the cable-tension force. We can then obtain

$${\mathbf{f}}_{cable}^b=\frac{R_v^b{f}_{cable}^v}{\sqrt{p_n^2+p_e^2+p_d^2}}\left[\begin{array}{ccc}\left(\begin{array}{c}0\\ 0\\ 0\end{array}\right)& -& \left(\begin{array}{c}{p}_n\\ p_e\\ p_d\end{array}\right)\end{array}\right].$$

(22)

Then, Equation (20) can be written as

$${\mathbf{a}}_{SF}^b=\left[\begin{array}{c}{\mathbf{a}}_{SF,x}^b\\ {\mathbf{a}}_{SF,y}^b\\ {\mathbf{a}}_{SF,z}^b\end{array}\right]=\frac{1}{mg}\left[\left(\begin{array}{c}0\\ 0\\ -(f_F+f_R+f_B+f_L)\end{array}\right)-\frac{R_v^b{f}_{cable}^v}{\sqrt{p_n^2+p_e^2+p_d^2}}\left(\begin{array}{c}{p}_n\\ p_e\\ p_d\end{array}\right)\right].$$

(23)

3. Self-Localization of Tethered Drone

3.1. Problem Statement

Consider a scenario where a drone tethered to a ground robot (see Figure 1). In this paper, the ground robot is assumed to be stationary and the tether is assumed to be controlled by a retractable winch that provides a constant cable-tension force. The problem is to estimate the 3D position of the tethered drone with respect to the ground station (i.e., the origin of the vehicle coordinate frame), using the measurements of the accelerometer, gyroscopes, altimeter, and PWM signals onboard the drone.

3.2. State-Space Model for Self-Localization

In our previous work [29], we presented a 3-state state-space model for self-localization by assuming that the cable-tension force is known. In this paper, we develop a 4-state state-space model to estimate the drone 3D location, as well as the cable-tension force.

Define the state vector as

$${\mathbf{x}}_{\mathbf{4}\mathbf{s}}={\left(p_n,p_e,p_d,f_c\right)}^T\in {\mathbb{R}}^4$$

(24)

and the system dynamics are given by

$${\dot{\mathbf{x}}}_{\mathbf{4}\mathbf{s}}=f({\mathbf{x}}_{\mathbf{4}\mathbf{s}},\mathbf{u}),$$

(25)

where $\mathbf{u}$ is the system input vector. Since we do not know the actual motion plan and the cable-tension force evolution, we will use the following system dynamics to derive the EKF

$${\dot{\mathbf{x}}}_{\mathbf{4}\mathbf{s}}=f({\mathbf{x}}_{\mathbf{4}\mathbf{s}},\mathbf{u})={\mathbf{0}}_{4\times 1}.$$

(26)

Assuming that the measurements of the 3-axis accelerometers and the altimeter (i.e., the ultrasound sensor) are available and according to Equation (23), the output function is given by

$$\begin{array}{cc}\hfill \mathbf{y}& =h\left({\mathbf{x}}_{\mathbf{4}\mathbf{s}}\right)=\left(\begin{array}{c}{\mathbf{a}}_{SF,x}^b\\ {\mathbf{a}}_{SF,y}^b\\ {\mathbf{a}}_{SF,z}^b\\ -p_d\end{array}\right)\hfill \\ \hfill & =\left[\begin{array}{c}\frac{1}{mg}{\left[\left(\begin{array}{c}0\\ 0\\ -(f_F+f_R+f_B+f_L)\end{array}\right)-\frac{R_v^b{f}_{cable}^v}{\sqrt{p_n^2+p_e^2+p_d^2}}\left(\begin{array}{c}{p}_n\\ p_e\\ p_d\end{array}\right)\right]}_{3\times 1}\\ -p_d\end{array}\right].\hfill \end{array}$$

(27)

4. Extended Kalman Filter

In this section, we present the application of the EKF (Algorithm 1) [35] technique to estimate the location of the drone and the cable-tension force. The system dynamics and output equations are described in Section 3. We assume that the available sensor measurements include the 3-axis orientations and accelerations that are distorted by white Gaussian noise. Selecting the system state vector at time k is ${\mathbf{x}}_{\mathit{k}}={\left(p_{n,k},p_{e,k},p_{d,k}\right)}^T\in {\mathbb{R}}^3$ and the system measurement vector at time step k as ${\mathbf{y}}_{\mathit{k}}=\left({\mathbf{a}}_{SF,x,k}^b,{\mathbf{a}}_{SF,y,k}^b,{\mathbf{a}}_{SF,z,k}^b,-p_{d,k}\right){}^T$, the state transition and observation models are given by

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{k+1}& =\mathbf{f}({\widehat{\mathbf{x}}}_k,{\mathbf{u}}_k,{\mathbf{w}}_k),{\mathbf{w}}_k\sim N(0,Q),\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{y}}}_{k+1}& =\mathbf{h}({\widehat{\mathbf{x}}}_{k+1},{\mathbf{v}}_{k+1}),{\mathbf{v}}_{k+1}\sim N(0,R),\hfill \end{array}$$

(29)

where ${\widehat{\mathbf{x}}}_{k+1}$ and ${\widehat{\mathbf{y}}}_{k+1}$ denote the approximated a posteriori state and observation, respectively, and ${\widehat{\mathbf{x}}}_k$ the a priori estimate of the previous step. The process is characterized by random noise variables ${\mathbf{w}}_k$ and ${\mathbf{v}}_k$ that represent respectively the process noise and measurement noise, both of which follow the Gaussian distribution with covariance matrices Q and R, respectively. The diagram in Figure 2 summarizes the EKF process loop [37] with associated equations.

Algorithm 1 Extended Kalman Filter [35].

1: Initialize: $\widehat{x}$ 2: At each sample time $T_{out}$, 3: for i = 1 to N do {Prediction} 4:   $\widehat{x}=\widehat{x}+\left(\frac{T_{out}}{N}\right)f(\widehat{x},u)$ 5:   $\mathsf{\Phi }=\frac{\partial f}{\partial x}(\widehat{x},u)$ 6:   $P=P+\left(\frac{T_{out}}{N}\right)(\mathsf{\Phi }P+P{\mathsf{\Phi }}^T+Q)$ 7:   Calculate A, P, and C 8: end for 9: if measurement has been received from sensor i then {Correction:Measurement Update} 10:   $H_i=\frac{\partial h_i}{\partial x}(\widehat{x},u)$ 11:   $K_i=P{H}_i^T{(R+H_{i}P{H}_i^T)}^{-1}$ 12:   $P=(I-K_i{H}_i)P$ 13:   $\widehat{x}=\widehat{x}+K_i(y_i\left[n\right]-h(\widehat{x},u\left[n\right])$ 14:end if

The EKF starts by calculating the Jacobian of the $\mathbf{f}(\mathbf{x},\mathbf{u})$ and $\mathbf{h}\left(\mathbf{x}\right)$ functions that were derived in Section 3.2. The prediction step before acquiring the measurements is given by

$$\begin{array}{ccc}\hfill {\widehat{\mathbf{x}}}_{k+1/k}& =& {\mathsf{\Phi }}_k{\widehat{\mathbf{x}}}_{k/k},\hfill \end{array}$$

(30)

$$\begin{array}{ccc}\hfill P_{k+1/k}& =& {\mathsf{\Phi }}_k{P}_k{\mathsf{\Phi }}_k^T+Q_k,\hfill \end{array}$$

(31)

while the update step after acquiring the measurements is given by

$$\begin{array}{cc}\hfill K_k& =P_{k/k-1}{H}_k^T{[H_k{P}_{k/k-1}{H}_k^T+R_k]}^{-1},\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill {\widehat{\mathrm{x}}}_{\mathrm{k}/\mathrm{k}}& ={\widehat{\mathrm{x}}}_{\mathrm{k}/\mathrm{k}-1}+K_k(z_k-H_k{\widehat{\mathrm{x}}}_{\mathrm{k}/\mathrm{k}-1}),\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill P_{k/k}& =(I-K_k{H}_k)P_{k/k-1},\hfill \end{array}$$

(34)

where K is the Kalman gain matrix, and P is the covariance matrix for the state estimate, containing information about the accuracy of the estimate [38]. Figure 3 shows the localization/EKF algorithm flowchart and diagram that is implemented and coded. The Jacobian of $\mathbf{h}\left(\mathbf{x}\right)$ with respect to $\widehat{\mathit{x}}$ is given by

$$\frac{\partial h}{\partial x}=\left[\begin{array}{cc}{\left[\frac{-R_v^b{f}_{cable}^v}{mg.{\left(\sqrt{p_n^2+p_e^2+p_d^2}\right)}^3}\left[\begin{array}{ccc}{p}_e^2+p_d^2& -p_n{p}_e& -p_n{p}_d\\ -p_n{p}_e& p_n^2+p_d^2& -p_e{p}_d\\ -p_n{p}_d& -p_e{p}_d& p_n^2+p_e^2\end{array}\right]\right]}_{3\times 3}& \frac{-R_v^b}{mg}{\left[\begin{array}{c}\frac{p_n}{\sqrt{p_n^2+p_e^2+p_d^2}}\\ \frac{p_n}{\sqrt{p_n^2+p_e^2+p_d^2}}\\ \frac{p_n}{\sqrt{p_n^2+p_e^2+p_d^2}}\end{array}\right]}_{3\times 1}\\ {\left[\begin{array}{ccc}0& 0& -1\end{array}\right]}_{1\times 3}& 0\end{array}\right].$$

(35)

5. System Identification for Motor Coefficients

In order to compute accurate motor thrust forces using the PWM signals, we present a system-identification strategy in this section to obtain function $f_{*}$ in Equation (19) [39]. The system identification process has to go through a few steps to generate $f_{*}$ that maps the input PWM signals to the total motor thrust [13,14,40,41,42]. The first step is to design flight experiments to collect the data with sufficient accuracy and duration. A good experimental design should ensure that the system is excited adequately by the input commands. The collected measurement data are usually processed by noise filtration and bias removal before being used for deriving high-fidelity models. A model structure is usually selected based on a prior knowledge of the input-output relation for model estimation. After that, the collected data are used to generate and update the selected parameters in the model, such that the model output is matched with the output in the data set. The dataset is usually divided into two subsets, which are used for estimation and validation, respectively. Validating the model and analyzing the uncertainty of the estimated model are the final steps before using the model for the application (e.g., control and state estimation). The estimation-validation process may take several iterations before finding the optimal model with the highest fitting percentage that is used to represent the model accuracy [43]. In this paper, the applied system-identification process [44] is summarized in Figure 4, and was implemented using the System Identification Toolbox in MATLAB^®.

5.1. Experiment Design and Data Acquisition

The input commands to the drone system are the PWM signals of the four motors, and the sensor measurements include the three Euler attitude angles, the 3-axis accelerations, and the altitude. The output of the system-identification model is the total thrust force generated by all four motors, ${\mathbf{f}}_{\mathbf{thrust}}^{\mathbf{b}}$ (see Equation (18)), which is computed using the accelerometer measurement in the z-axis

$$f_{thrust,z}=mg·R_b^v(\varphi ,\theta ,\psi )\left(\begin{array}{c}0\\ 0\\ a_z\end{array}\right).$$

(36)

The input-command sequences for the proposed tethered drone are designed, such that the individual inputs are sufficiently “exciting” system motion and guarantee meaningful identification results [45]. For this reason, indoor flights (see Figure 5) were conducted by first commanding the drone at a steady hovering flight. Then, the roll, pitch, and yaw angles were excited individually, while varying the altitude by changing the collective thrust input commands. Figure 6 shows a collected data set that consists of the computed thrust force (in Newton), the acceleration measurements (±1 g), and the Euler angles (±180 degrees) in response to the PWM commands (from 0 to 255).

5.2. Data Processing

The flight test data were collected using the “rosbags” in the robot operating system (ROS) and imported by MATLAB^® for data processing. The data collected from the flight test were re-sampled at 100 Hz, and only the airborne data were selected. The plots between 10 and 140 s in Figure 6 indicate that the drone was in flight. The data are then filtered by a fifth-order Butter-worth low pass filter with a cut-off frequency of 10 Hz. The resulting data are then divided into two subsets for estimation and validation, respectively, as shown in Figure 6e.

5.3. Model Structure Selection, Estimation, and Validation

In this work, we examined a variety of parametric model structures. Parametric models describe systems using differential equations and transfer functions as black-box models. The general linear-model structure can be represented by

$$y\left(t\right)=G(\xi ,\eta )u\left(t\right)+H(\xi ,\eta )e\left(t\right),$$

(37)

where $u\left(t\right)$ and $y\left(t\right)$ are the input and output of the system, respectively, $e\left(t\right)$ is the system disturbance, $G(\xi ,\eta )$ and $H(\xi ,\eta )$ are the transfer functions of the deterministic and the stochastic parts of the system, respectively, $\xi$ is the backward shift operator, and $\eta$ is the parameter vector [39]. A subset of the general linear model structure, can be represented as

$$A\left(\xi \right)y\left(t\right)=\frac{B\left(\xi \right)}{F\left(\xi \right)}u\left(t\right)+\frac{C\left(\xi \right)}{D\left(\xi \right)}e\left(t\right).$$

(38)

By setting one or more of the $A,B,C$, or D polynomials equal to 1, we can create simpler models, such as autoregressive (AR), autoregressive with exogenous variables (ARX), autoregressive moving average with exogenous input (ARMAX), Box–Jenkins (BJ), and output-error structures [40,41,46]. These methods have their own advantages and disadvantages and are selected based on the dynamics, and the noise characteristics of the system.

A model with more parameters does not necessarily generate more accurate results, as it may capture nonexistent dynamics and noise characteristics. This is where the physical insight into a system is helpful. The model structures that we have tested include the transfer function, process model, black- box ARX, state space, and Box–Jenkins. Black-box modeling is usually a trial-and-error process, where the parameters of various structures are estimated and compared. We started with the simple linear model structure and progressed to more complex ones [46]. ARX is the simplest and the most efficient method that solves linear regression equations in an analytic form with the global minimum of the loss function. The ARX model, therefore, is preferable in this work, as the model order is high. The disadvantage of the ARX model is its weak capability of eliminating disturbances from the system dynamics. The Box–Jenkins structure provides a complete formulation by separating disturbances from the system dynamics.

Transfer function models are commonly used to represent single-input-single-output (SISO) or multiple-input-multiple-output (MIMO) systems [47]. In the MATLAB^® System Identification Toolbox, the process model structure describes the system dynamics, in terms of one or more of these elements, such as static gain, time constants, process zero, time delay, and integration [47].

The models generated were designed for prediction and the results demonstrated are for the five-step-ahead prediction [40,41,46,47]. Equations (A1)–(A8) in the Appendix A represent the two highest best fits models: the ARX and state-space models.

Table 1. Identification results for 5-step prediction.

Structure	Fit%	FPE	MSE
Transfer Function (mtf)	46%	0.002388	0.002343
Process Model (midproc0)	41.41%	0.002796	0.002778
Black-Box model-ARX Model (marx)	96.77%	8.478 × ${10}^{-6}$	8.438 × ${10}^{-6}$
State-Space Models Using (mn4sid)	99.56%	1.589 × ${10}^{-7}$	1.562 × ${10}^{-7}$
Box-Jenkins Model (bj)	94.64%	2.339 × ${10}^{-5}$	2.326 × ${10}^{-5}$

summarizes the quality of the identified models on the basis of fit percentage (Fit%), Akaike’s final prediction error (FPE) [48], and the mean-squared error (MSE) [49]. As can be seen from Table 1, the fit percentages for the ARX, Box–Jenkins, and state space models are all above 94%, among which the state-space model has the best fit percentage, whereas the process models and the transfer functions are below 50%.

6. Simulation Results and Discussion

In order to evaluate the feasibility and performance of the proposed 4-state EKF for the tethered drone self-localization, numerical simulations were performed under MATLAB^®/Simulink^®.

The initial position of the drone is selected as ${\mathbf{p}}_0={\left(0,0,0\right)}^T\mathrm{m}$ and the drone is controlled to follow a circular orbit of 2.5-m radius with a constant velocity of $1\mathrm{m}/\mathrm{s}$ and a varying altitude. The IMUs and ultrasound sensors are assumed to provide measurements with a frequency of 200 Hz [50]. The measurements of the 3-axis accelerometers and the ultrasound sensor are used to generate the outputs of the EKF in Equation (27). We assume that these measurements are corrupted by the Gaussian noise $\mathcal{N}\left(0,{\sigma }_{acc}^2\right)$ (for each axis of the accelerometers) and $\mathcal{N}\left(0,{\sigma }_{ults}^2\right)$, respectively, where ${\sigma }_{acc}^2=0.01$ $\mathrm{m}/{\mathrm{s}}^2$ and ${\sigma }_{ults}^2=0.1\mathrm{m}$ [31]. Thus, the sensor noise covariance matrix, R, is selected as $R=\mathrm{diag}({\sigma }_{acc}^2,{\sigma }_{acc}^2,{\sigma }_{acc}^2,{\sigma }_{ults}^2)=\mathrm{diag}(0.01,0.01,0.01,0.1)$. The 3-axis gyros measurements are used to compute the transformation matrix, $R_v^b$, in Equation (2). We assume that the 3-axis gyros measurements are corrupted by the Gaussian noise $\mathcal{N}\left(0,{\sigma }_{gyros}^2\right)$ (for each axis of the gyros), where ${\sigma }_{gyros}^2=0.01°$. Figure 7 shows the noisy sensor measurements and the ones filtered by LPFs. The noisy measurements were directly used by the EKF and the values obtained by an LPF are used in the self-localization approach presented in [30]. The process noise covariance matrix of the EKF was tuned and selected as Q = diag($5\times {10}^{-3}$, $5\times {10}^{-3}$, $5\times {10}^{-3}$). The initial state estimate was chosen to be ${\widehat{\mathbf{x}}}_{\mathbf{0}}={\left(1.5,2.5,1.5\right)}^T\mathrm{m}$, while the initial error covariance matrix was chosen to be ${\mathbf{P}}_0={\mathbf{I}}_3$. As for the LPF, it is based on a cutoff frequency set to 2 rad/s.

To compare the estimation results obtained from the proposed 4-state EKF and the 3-state EKF in [30], we assume that the 3-state model uses the actual cable-tension force from an onboard force sensor, but the 4-state EKF does not have access to the actual cable-tension force in the estimation process.

Figure 8 shows the ground-truth drone trajectory (“Truth” in the figure) overlaid with the estimated trajectories generated by the 3-state and 4-state EKFs (“EKF3S” and “EKF4S” in the figure, respectively) in the 3D, top-down, and side views with different magnitudes of the cable-tension force (0.5 N, 1 N, 2 N, 4 N, 6 N, and 10 N). Figure 9 shows the estimated North, East, and Down coordinates generated by the 3-state and 4-state EKFs versus the ground truth under different cable-force magnitudes, respectively. Figure 10 shows the estimation errors corresponding to Figure 9. It can be seen that the magnitude of the cable-tension force affects the accuracy of the position estimates obtained from both EKFs. When the cable-tension force is less than 2 N (see Figure 8a,b and Figure 10a,b), both EKFs are unable to generate accurate estimates. Both EKFs generated very close estimates in the first 15 s, but diverged from each other after that. It seems the 3-state-EKF was able to follow the trend of the ground truth waves with smaller magnitude and slower pace, while the 4-state-EKF estimates become relatively flat after 15 s. When the cable-tension force is greater than 1 N, both EKF estimates start to follow the ground truth with increasing accuracy, but become increasingly noisy (see Figure 8, Figure 9 and Figure 10b–f, respectively).

Figure 11 shows the ground-truth cable force (in blue) and its estimates (in red) using the 4-state EKF under different cable forces. Figure 11a shows that the cable-force estimation started to diverge from the beginning and generated impractical negative values and came back towards the ground truth after 20 s and diverged again after 25 s. This observation matches the position estimates in Figure 8, Figure 9 and Figure 10. The cable-force estimates for other cases are consistently accurate within a $\pm 0.3$ N range.

Table 2. Root-Mean-Square-Error (RMSE) comparison of the 3-state (3S) and the 4-state (4S) EKFs.

	${\mathit{f}}_{\mathit{c}}=$ 0.5 N		${\mathit{f}}_{\mathit{c}}=$ 2 N		${\mathit{f}}_{\mathit{c}}=$ 4 N		${\mathit{f}}_{\mathit{c}}=$ 10 N
Position	3S	4S	3S	4S	3S	4S	3S	4S
North (m)	2.022	5.075	0.275	0.276	0.159	0.156	0.236	0.243
East (m)	2.146	3.613	0.294	0.296	0.106	0.105	0.206	0.209
Down (m)	0.010	0.010	0.014	0.013	0.033	0.020	0.120	0.067
$f_c$ (N)	-	0.495	-	0.066	-	0.071	-	0.109

summarizes the estimation results under different cable-force magnitudes using the root mean square error (RMSE) metric. We can see that for small cable-tension force values (i.e., <1 N), the 3-states model produces more accurate position estimates in the north and east directions.

To study the impact of various drone altitudes and velocities on the estimation accuracy, we conducted the simulation with drone altitudes ranging from 1 m to 10 m, and velocities ranging from 0.5 to 3 m/s. Figure 12, Figure 13 and Figure 14 summarizes the RMSE results using different altitudes, velocities, and cable-tension forces. It can be seen from Figure 12 that the 3-states and 4-states EKFs have no significant difference, however, it can seen that at lower altitude of 1 m the error is around [0.7, 0.9] m in North and East position respectively. The error decreases as the altitude increases and it reaches its lowest value at around 5-m altitude. The error increases again as the altitude increases. It can be seen from Figure 13 that the lower the velocity, the lower the postion estimation error in all directions.

Figure 14 shows that the 4-state and 3-state EKFs provide 3D-position estimates with the same level of accuracy (less than 0.3 m, see Table 2) when the actual cable-tension force magnitude is greater than 1 N. The position estimation accuracy of both 4-state and 3-state EKFs degrades when the cable magnitude is less than 2 N, even though the 3-state EKF uses the true cable-tension force. This implies that to produce accurate position estimation using the proposed 4-state EKF, one needs to maintain the cable-tension force to be above 2 N, which can be realized by using a retractable cable system.

7. Conclusions

In this paper, we present a self-localization technique for a tethered drone without a cable-force sensor in GPS-denied environments. To the best of our knowledge, this is one of the first works that estimate both the cable-tension force and the 3D location of a tethered drone without adding additional onboard sensors. A 4-state extended Kalman filter (EKF) was developed for the estimation, and its performance was compared with an existing 3-state EKF that assumes known cable-tension force. We also studied the impact of various cable-force values, altitudes, and velocities on the performance of both the proposed 4-state and the existing 3-state EKFs. The simulation results reveal that both EKFs produce the 3D drone position estimates with less than 0.3-m RMSE (root mean square error) and the cable-force estimates with less than 0.11 N RMSE, when the actual cable-tension force is greater than 1 N. When the actual cable-tension force is less than 2 N, the proposed 4-state EKF produces estimates with up to 5-m error for and the 3-state EKF with up to 2-m error.

This work facilitates the control and self-localization of a tethered drone by enabling the estimation of the cable-tension force, which eliminates the need of equipping a cable force sensor and reduces the complexity of the control system and data-acquisition system for a tethered drone. This ability makes possible the use of a tethered drone in GPS-degraded/-denied environments for real-world applications that need precise self-localization information with the decimeter-level accuracy, such as agricultural chemical spraying, and wind-turbine and high-rise building cleaning.

8. Future Work

In our future work, we plan to further investigate the position and cable-tension force estimation problem by leveraging the sigma-point Kalman filtering techniques (e.g., the unscented Kalman filter) and machine learning techniques (e.g., the decision tree method). Our hope is that these techniques would improve the overall estimation accuracy, especially when the cable-tension force is lower than 2 N. Moreover, we will develop a hardware experimental platform to evaluate our proposed techniques on a real tethered drone.