A Square-Root Unscented Kalman Filter for attitude and
relative position estimation of a tethered unmanned helicopter
Luis A. Sandino1 , Manuel Bejar2
Abstract— The use of tethered Unmanned Aircraft
Systems (UAS) in aerial robotic applications is a relatively
unexplored research field. In this work a numerically
efficient implementation of a sigma-point Kalman filter
is applied to the attitude and relative position estimation of a small-size tethered unmanned helicopter. For
that purpose, the state prediction is performed using a
kinematic process model driven by measurements of the
inertial sensors (accelerometer and gyroscope) onboard
the helicopter and the subsequent correction is done using
information from additional sensors like magnetometer,
radar altimeter and magnetic encoders measuring the
tether orientation relative to the helicopter. Assuming
the tether is kept taut by an actuated device on ground
during the system operation, this approach avoids the
need of a global positioning system (GPS) as the position
is estimated relative to the anchor point. The filter
performance is evaluated in simulations
Keywords—Unmanned Aircraft Systems, helicopter,
tethered systems, Unscented Kalman Filter, state estimation
I. I NTRODUCTION
Research on UAS has seen a huge growth during
the last years. Although the field have been dominated by military applications, there is increased
interest in using UAS in civilian and public domain
applications such as aerial photography, inspection
and monitoring, accurate measurement, etc. When
comparing different configurations, helicopters and
other rotor-based aircraft have capabilities such
as hovering and vertical take-off and landing that
cannot be achieved by conventional fixed-wing
aircraft. These features allow remotely piloted and
autonomous helicopters to be extensively used
for aerial robotic applications nowadays. However,
their potential is often limited by key factors such
as flight time, stability or safety regulations.
1 L.A. Sandino is with GRVC, University of Seville, Seville,
SPAIN (B) lsandino at us.es
2 M. Bejar is with GRVC, University Pablo de Olavide, Seville,
SPAIN mbejdom at upo.es
One possible way to overcome some of these
limitations, is the use of tethered configurations.
To the best of the authors’ knowledge, only a few
references on tethered UAS can be found in the
existing literature. The first references to consider
a tethered configuration for rotorcraft prototypes
are those of [1], where the linearized equations
describing the perturbed longitudinal motion of a
tethered rotorcraft are presented and [2], where
a discussion of control and stability problems
involved in the tethered configuration for a rotor
platform prototype is presented, with special emphasis on tether dynamics.
A very useful application of a tether can be
aided-landing. In this scenario, the only contributions are those of [3], [4] where the tether tension
is used as an additional helicopter control resource,
and [5], where the GPS is replaced by a tetherbased helicopter position estimate in the landing
maneuver. The use of tether tension to improve
the helicopter hovering stability in the presence of
wind disturbances was almost unexplored before
the contributions of [6], [7]. Last but not least,
the cable connecting the helicopter to the ground
station can also be potentially used as unlimited
power supply and wideband data link.
Regarding the field of nonlinear state estimation the Extended Kalman Filter (EKF) has been
extensively applied over the last decades. The
principal drawback of the EKF is that its linearization approach can result in large errors and
divergence of the filter. The Unscented Kalman
Filter (UKF) proposed by [8], addresses these issues with the same computational complexity. The
superior performance of the UKF over the EKF in
terms of prediction and estimate errors, has been
demonstrated in a great number of applications
[9], [10], including integrated navigation of and
autonomous helicopter [11]. This paper is focused
H
h3’
h3
c3=h3’’
q2
h2’=h2’’
h1=h1’
q1
h2
PH
h1’’
TC
C
Fig. 1. Tethered helicopter scheme. Dextral sets of orthogonal
unit vectors hi , si , ni (i = 1, 2, 3) are fixed in helicopter H, platform
S and inertial reference frame N respectively.
in the attitude and relative position estimation of
a small-size tethered unmanned helicopter using
the numerically efficient Square-Root Unscented
Kalman Filter (SR-UKF) implementation proposed
by [12]. It is worth mentioning the position and
orientation estimation of a 2D tethered quad-rotor
model using inertial sensors only in [13].
This paper is organized as follows. In section
II the tethered helicopter system under study is
described in detail. Section III is a review of the
SR-UKF algorithm. In section IV, the derivation
of the process and measurement models for the
filter is addressed. Simulations results and the filter
performance analysis are presented in section V.
Finally, section VI is devoted to the conclusions.
II. S YSTEM DESCRIPTION
The system under consideration is the small-size
autonomous helicopter H tethered to a platform S
shown in Fig. 1. The tether connects points PH
of helicopter fuselage and PS on the platform as
depicted in the figure. It is assumed that the system
operates with a controlled device on the platform
that applies a constant tension TC to the tether,
thus it is always lying on a straight line between
PH and PS .
The device linking the tether to the helicopter
is an universal joint as illustrated in Fig. 2. As
can be seen, angular variables q1,2 (t) correspond
to two successive rotations at point PH that align
vector h3 with the tether line defined by vector c3 .
Thus, the dextral sets of orthogonal unit vectors ci
Fig. 2.
Universal joint at point PH . Upper body is fixed to
helicopter H while lower body C is fixed to the tether.
and hi (i = 1, 2, 3) are geometrically related by the
following direction cosine matrix:


c2
0
s2
H C
(1)
C =  s1 s2 c1 −s1 c2 
−c1 s2 s1 c1 c2
where si = sin(qi ) and ci = cos(qi ). The remaining
configuration variable q3 (t) defines the instantaneous tether length, as shown in Fig. 1. Therefore,
the helicopter position relative to the platform can
be defined as:
pP
S →PH
= q3 c3
(2)
The orientation of H and S in the inertial reference frame N is described by Euler parameters
qi (t) and qSi (t) (i = 0, 4, 5, 6) with the unity norm
attitude quaternions q = q0 + q4 n1 + q5 n2 + q6 n3
and qS = qS0 + qS4 n1 + qS5 n2 + qS6 n3 respectively.
Finally, the linear and angular velocity of the
helicopter and the platform in the inertial reference
frame N are defined with generalized speeds ui and
uSi (i = 1, · · · , 6) as:
N HO
= u1 n1 + u2 n2 + u3 n3
(3)
ω H = u4 h1 + u5 h2 + u6 h3
(4)
v
N
PS
S
S
S
N
v = u1 n1 + u2 n2 + u3 n3
N S
ω = uS4 s1 + uS5 s2 + uS6 s3
(5)
(6)
III. R EVIEW OF THE SR-UKF A LGORITHM
The general linear Kalman framework involves
the recursive estimation of the state mean and
covariance for a discrete-time dynamic system
f
(7)
xk+1 = F xk , vk , uk
yk = H xk , nk , uhk
(8)
where xk represent the unobserved state of the
system, yk represent the observed signal, vk and nk
are the process and observation noise respectively
f
and uk and uhk are exogenous inputs, assuming
all relevant random variables can be accurately
modeled as Gaussian random variables. When the
system is nonlinear, the EKF approach consist
in applying the linear Kalman filter covariance
update equations to the first-order linearization of
nonlinear functions F and H. These approximation
can result in large estimate errors and even the
divergence of the filter.
The UKF addresses the EKF flaws by using a
deterministic sampling to calculate mean and covariance terms. Essentially, 2L + 1 samples, called
sigma-points (L is the state dimension) are chosen
based on the square-root of the prior covariance.
Theses sigma-points are propagated through the
nonlinear model and then a weighted mean and
covariance is taken. Furthermore, unlike the EKF,
no explicit derivatives (i.e. Jacobians or Hessians)
need to be calculated.
The most computationally expensive operation
in the standard UKF algorithm corresponds to
calculating a new set of sigma-points at each timeupdate. This requires taking a matrix
√ square-root
of the state covariance matrix S = P. Then the
full covariance matrix P is recursively updated. In
the SR-UKF, matrix S is obtained using a Cholesky
factorization such that SST = P, then it is directly
propagated, avoiding the need to refactorize at each
time step. The SR-UKF algorithm is the same
computational
complexity than EKF and UKF
(O L3 ), but with the added benefit of numerical
stability and guaranteed positive semi-definiteness
of state covariances.
As in the original UKF, the SR-UKF is initialized by calculating the matrix square-root of the
state covariance once via a Cholesky factorization
in (10). However, the propagated and updated
Cholesky factor is then used in subsequent iterations to directly form the sigma-points. In (17) the
time-update of the Cholesky factor is calculated
using a QR factorization of the compound matrix
containing the weighted propagated sigma-points
and the matrix square-root of the additive process noise covariance. The subsequent Cholesky
update (or downdate) in (18) is necessary since
the zeroth weight, may be negative. The same
two-step approach is applied to the calculation
of the Cholesky factor of the observation error
covariance in (21) and (22). In contrast to the way
the Kalman gain is calculated in the standard UKF,
now two nested inverse (or least squares) solutions
to Kk Syk STyk = Pxk yk are calculated. Since Syk is
square and triangular, efficient back-substitutions
can be used to directly solve for Kk without
the need for a matrix inversion in (24). Finally
the posterior measurement-update of the Cholesky
factor of the state covariance is calculated in (27)
by applying sequential Cholesky downdates to S−
xk
where the downdate vectors are the columns of
U = Kk Syk . The following is SR-UKF algorithm
pseudo-code:
•
Initialization:
x̂0 = E [x0 ]
n h
io
Sx0 = chol E (x0 − x̂0 ) (x0 − x̂0 )T
aug T
T
x̂aug
= x̂0 v̄T n̄T
0 = E x0


Sx0 0 0
 0 Sv 0 
Saug
x0 =
0
0 Sn
Sv = chol {Rv }
•
Sn = chol {Rn }
(9)
(10)
(11)
(12)
(13)
T
where xaug = xT vT nT
is the augmented state vector and Rv and Rn are the
process-noise and observation-noise covariance matrices respectively.
For k = 1, · · · , ∞:
1) Set t = k − 1
2) Calculate sigma-points:
aug
S+ S−
χtaug = x̂t
S+ = x̂taug L + γSaug
xt
S− = x̂taug L − γSaug
xt
where χ aug = χ x χ v
sigma-points matrix
χn
T
(14)
is the augmented
IV. ATTITUDE AND RELATIVE POSITION
3) Time-update equations:
x
= F χtx , χtv , utf
χk|t
ESTIMATION
(15)
2L
m x
x̂−
k = ∑ wi χi,k|t
(16)
i=0
− o
c χx
S−
x̂k 2L
(17)
=
qr
−
w
xk
1:2L,k|t
1
o
n
− c
−
x
(18)
S−
xk = cholupdate Sxk , χ0,k|t − x̂k , w0
x
Yt = H χk|t
, χtn , uth
(19)
np
2L
m
ŷ−
k = ∑ wi Yi,k|t
(20)
i=0
4) Measurement-update equations:
(21)
wc1 Y1:2L,k|t − ŷ−
k 2L
− c
Syk = cholupdate Syk , Y0,k|t − ŷk , w0 (22)
2L
T
x
Pxk yk = ∑ wci χi,k|t
− x̂−
Yi,k|t − ŷ−
(23)
k
k
Syk = qr
p
The helicopter is equipped with an inertial measurement unit (IMU) with a 3-axis accelerometer
plus a 3-axis gyroscope, a 3-axis magnetometer
and a high precision radar altimeter. Additionally,
the universal joint holding the tether includes
two absolute magnetic encoders for measuring the
tether orientation relative to the helicopter. Note
that there is no need of a GPS as the position
estimate is relative to the platform. The flight
computer running the helicopter controller as well
as the IMU and magnetic encoders have an update
rate of 100Hz whereas the magnetometer and
altimeter have an update rate of 10Hz.
i=0
Kk = Pxk yk /STyk /Syk
x̂k = x̂−
k + Kk
yk − ŷ−
k
(24)
U = Kk Syk
Sxk = cholupdate S−
xk , U, −1
•
•
(25)
(26)
(27)
{u}N denotes a matrix whose columns are
N copies of vector u; qr {A} denotes the
transpose of the upper-triangular R matrix resulting from QR factorization of matrix A (this equals to the lower-triangular
Cholesky factorization R = chol AAT );
cholupdate {R, U, ±ν} denotes N consecutive lower-triangular
Cholesky
factorizations
√
Ri+1 = chol Ri RiT ± νui uTi using the N
columns ui of matrix U; and / denotes the
efficient least-squares pseudo inverse.
Weights and parameters:
γ=
p
L+λ
λ
wm
0 =
L+λ
2
wc0 = wm
0 +1+β −α
1
wci = wm
i = 1, · · · , 2L
i =
2(L + λ )
A. Process model
In the SR-UKF framework a standard IMU
driven kinematic process model formulation is
used. Low cost IMUs extensively used in robotic
applications usually have large static bias errors.
For this reason, the accelerometer, gyroscope and
magnetometer bias terms are incorporated in the
state vector to be estimated. The estimated values
of these error components are used to correct
the raw IMU acceleration and gyro-rate as well
as magnetometer measurements, before they are
used inside the process model. Therefore, the state
vector is given by:
(28)
(29)
T
x = pT vT qT bTa bTg bTm
(32)
(30)
(31)
where L is the dimension of the augmented
state vector; λ = α 2 L − L is a compound
scaling parameter; 0 < α ≤ 1 is the primary
scaling factor determining the spread of the
sigma-points around x̂aug ; and β is a secondary scaling factor used to incorporate prior
knowledge of the distribution of x (for Gaussian distributions, β = 2 is optimal).
where p = q1 q2 q3 is a vector with the
tether orientation
and length
(helicopter relative
u
u
u
position), v =
1 2 3 is
the linear velocity
vector, q = q0 q4 q5 q6 is the unity
norm
attitude quaternion, ba = ba1 ba2 ba3 is the
IMU acceleration bias vector, bg = bg1 bg2 bg3
is
bias vector and bm =
the IMU gyro-rate
bm1 bm2 bm3 is the magnetometer bias vector.
The continuous-time kinematic equations driven
(36)
of low cost IMUs sensors exhibit non-zero mean
and non-stationary behavior, the errors are modeled as a random walk, in order to improve the
tracking of these time-varying errors by the filter.
The discrete-time updates necessary for the filter
are calculated with a simple forward Euler integration:
ḃg = ng
(37)
xk = xk−1 + ẋk−1 · dt
ḃm = nm
(38)
by the IMU measurements are:
T
ṗ = q̇1 q̇2 q̇3
T
v̇ = N CH (ã − ∆a) + 0 0 −g
1
q̇ = − Ω (ω̃) q
2
ḃa = na
(33)
(34)
(35)
In equation (34), g is the acceleration of gravity,
is the direction cosine matrix which transforms vectors in frame H to the inertial reference
frame N, given by:
N CH
N
(44)
where ẋ is calculated using equations (33)-(38) and
dt is the integration time-step of the system.
Finally, in order to calculate ṗ in equation (33),
O
O
O
S
S
H
the expression pN →H = pN →P + pP →P +
H
O
pP →H is derived with respect to time to get:
CH =
(39)
N H O = N PS + q̇ c + q ċ + d
(45)
v
v


3 3
3 3
PH −H3O ḣ3
2
2
0.5 − q5 − q6 q4 q5 − q0 q6 q4 q6 + q0 q5
2  q4 q5 + q0 q6 0.5 − q24 − q26 q5 q6 − q0 q4  Then, expressing vectors c3 and h3 in N using
q4 q6 − q0 q5 q5 q6 + q0 q4 0.5 − q24 − q25 (1) and (39), doing the time derivative taking (35)
into account and finally solving for q̇1 , q̇2 and q̇3
and the term ∆a includes the acceleration compoyields:
nents when the IMU is not located at the center of
T
mass of the helicopter, given by:
ṗ = q̇1 q̇2 q̇3
(46)
O
H
O
H
d ω̃
T
= M (p, q) · u1−3 ũ4−6 uS1−3
× pH →P + ω̃ × ω̃ × pH →P
(40)
∆a =
dt
The terms ã and ω̃ in the above equations are B. Observation model
the bias and noise corrected IMU accelerometer
Errors in the state estimate may increase with
and gyro-rate measurements, i.e.
time because of the IMU measurements timeã = aIMU − ba − na
(41) integration approach. In order to compensate for
ω̃ = ωIMU − bg − ng
(42) these long-term errors, aiding sensors such as
the magnetometer, the altimeter, and the magnetic
where aIMU and ωIMU are the raw measurements encoders are incorporated in the filter.
coming from the IMU and na and ng are noise
1) Magnetic encoders: These two sensor interms represented by zero-mean Gaussian random stalled in the universal joint holding the tether
vectors.
measure the absolute rotation of the joint, that is
In equation (35), Ω (ω̃) is a 4 × 4 skew- directly the angles q1 and q2 . Thus, the observation
symmetric matrix composed of the bias and noise functions for these sensors are:
corrected IMU gyro-rate measurements, i.e.


H1 = q1 + n1
(47)
0
ũ4
ũ5
ũ6
H2 = q2 + n2
(48)
−ũ4 0 −ũ6 ũ5 

Ω (ω̃) = 
(43)
−ũ5 ũ6
0 −ũ4 
where n1 and n2 are zero-mean Gaussian random
−ũ6 −ũ5 ũ4
0
variables representing the sensor noise.
2) Radar altimeter: As seen inf Fig. 1, this
Equations (36)-(38) model the time-varying nature of the IMU sensor bias error terms. Usually, sensor located at point PA measures the vertical
sensor errors are modeled as zero-mean, stationary, distance qalt between the sensor and the platform.
first-order Gauss-Markov process. Since the biases In order to relate the measurement to the state
vector, the dot product of ni (i = 1, 2, 3) with the
expression
xs1 + ys2 + qalt n3 = q3 c3 + dPH −PA h3 +
3
(49)
+ dH O −PA h1
1
is taken and the three equations obtained are solved
for x, y and qalt , where x and y are auxiliary
variables. Therefor, the observation function for
this sensor is:
(50)
H3 = qalt = F p, q, qS + n3
where n3 is a zero-mean Gaussian random variable
representing the sensor noise.
3) Magnetometer: This sensor measures the
earth’s magnetic field vector B in the H reference
frame. The magnetic field vector is constant in
the inertial reference frame N for a given location. This allows to use the following observation
function:
BH = H CN (q) B0 + bm + nm
(51)
where B0 is the local magnetic field vector obtained from the 2010 World Magnetic Model [14],
H CN is the direction cosine matrix (see (39)),
b
bias vector and nm =
m is the magnetometer
T
n4 n5 n6 is a vector of zero-mean Gaussian
random variables representing the sensor noise.
Finally, if the magnetometer measurement is normalized, the observation functions for this sensor
are:
H4,5,6 =
BH
kBH k
(52)
Multi-rate SR-UKF approach: The IMU and
magnetic encoders have a 100Hz update rate
whereas the altimeter and magnetometer have a
10Hz update rate. For this reason, the filter time
and measurement updates are performed every
0.01 s. Nevertheless, different components of the
measurement and observation vectors yk and ŷ−
k
are used in the measurement-update step according
to the different update rates, i.e.
h
iT
 H1 H2
0.01 s (enc. only)
iT
ŷ−
(53)
k = h
0.1 s (all sensors)
H1 · · · H6
TABLE I
S ENSOR NOISE PARAMETERS FOR FILTER INITIALIZATION AND
SIMULATION MODEL
Sensor
InvenSense
MPU-6000
US Digital
MAE3
Roke
MRA Type 2
Freescale SC
MAG3110
Sensor type
accelerometer
gyroscope
σ
400
0.05
Units
√
µg/ Hz
deg/s -rms
magnetic encoders
0.5
deg -rms
radar altimeter
0.1
m -rms
magnetometer
0.4
µT -rms
V. S IMULATION RESULTS AND PERFORMANCE
ANALYSIS
In this section the results of the simulation
experiments are presented. The SR-UKF has been
implemented and tested within the MATLABSimulink framework. The model for simulation
is the same high fidelity model used for control
design for the real system in [5]. This framework
allows to obtain C++ code after validation in
simulation to run it in real-time onboard a flight
computer with the QNX operative system.
For the first experiment, the helicopter controller
tries to keep it at hover while tether tension is kept
constant by the ground device controller. For the
sake of comparison with the real state, the state
estimate is not used as the controller feedback.
Initially, the tether lie on a vertical line with a
length of 10 m. At t = 5 s (instant a) the platform
is moved forward, at t = 15 s (instant b) it is moved
to the left and at t = 25 s (instant c), it is moved
upward. The three movements are performed with
steps of 2 m. The reference for the helicopter yaw
angle is set to 10o at t = 5 s and −10o at t = 15 s.
The values used for matrices Rv and Rn in filter
initialization have been taken from the data sheets
of real sensors as can be seen in Table I. The
standard deviations of random noise variables for
the sensor in the simulation model have been set
to a random value up to σ . All the biases for the
sensor in the simulation model have also been set
to constant random values up to 2σ , considering
them the true state.
As the controller in [5] uses Euler angles in
the orientation loop, in order to evaluate the filter
performance, they are calculated from quaternion
TABLE II
RMS E RRORS IN EXPERIMENT 1
q1
q2
q3
roll
pitch
yaw
S motion
known
0.0657
0.0809
0.0501
0.1246
0.2361
0.7654
S motion
unknown
0.8519
0.7468
0.4466
0.7094
1.7708
4.2032
Units
10
deg
deg
m
deg
deg
deg
5
q 1 [º]
Variable
0
-5
0
a
10
b
20
c
30
40
50
30
40
50
30
40
50
30
40
50
30
40
50
30
40
50
3
(54)
2
1
(55)
(56)
10
20
t [s]
0
q 2 [º]
-5
-10
0
a
10
b
20
c
|error| [º]
2.5
2
1.5
1
0.5
0
10
20
t [s]
10
q 3 [m]
It is important to note that the platform linear
velocity uS1−3 and attitude qS are exogenous inputs
to process and measurement models as they appear
in equations (46) and (50) respectively. This means
that the information related to the platform state
is needed for the tether system state estimation.
Figures 3 and 4 show the simulation results for two
cases: the solid blue line represents the estimate
assuming the platform motion is known and passed
to the filter equations while the dotted red line represents the estimate assuming the platform
motion
T
S
is unknown, so that u1−3 = 0 0 0 and qS =
T
1 0 0 0 . The dashed black line represents
the true state. The absolute value of the error is
plotted below the estimate in order to compare the
filter performance in the aforementioned cases.
Table II summarizes the root mean square errors
for all the variables in both cases. As can be seen,
the filter achieves a very good estimate with the
prior knowledge of the platform motion.
A second experiment has been performed in
order to analyze if the estimate without prior
knowledge of the platform motion is good enough
for stabilizing the helicopter. For that purpose, the
estimate is now used as the controller feedback.
As opposed to the first experiment where the
controller kept the helicopter at hover, in this
experiment the position estimate is used to set a
reference for the helicopter controller in order to
0
9
8
0
a
10
b
20
c
1.2
|error| [m]
roll = arctan 2 (−2(q5 q6 − q0 q4 ) ,
q20 − q24 − q25 + q26
pitch = arcsin (2 (q4 q6 + q0 q5 ))
yaw = arctan 2 (−2(q4 q5 − q0 q6 ) ,
q20 + q24 − q25 − q26
|error| [º]
q as follows:
1
0.8
0.6
0.4
0.2
0
10
20
t [s]
Fig. 3. Experiment 1. Tether angles q1 , q2 and tether length q3
estimates and absolute estimate errors
TABLE III
RMS E RRORS IN EXPERIMENT 2
Variable
q1
q2
q3
roll
pitch
yaw
10
roll [º]
9
8
7
6
0
a
10
b
20
c
30
40
|error| [º]
1
0.5
10
20
30
40
50
30
40
50
30
40
50
t [s]
pitch [º]
2
0
-2
-4
0
a
10
b
20
c
|error| [º]
3
2
1
0
10
20
t [s]
yaw [º]
deg
deg
m
deg
deg
deg
keep the tether lying on the vertical, while the platform moves. Figures 5 and 6 show the simulation
results. Again, the solid blue line represents the
estimate and the dashed black line represents the
true state. Although there is a steady-state error of
approximately 0.5 deg for roll, 1 deg for pitch and
9 deg for yaw, the estimate allows the controller to
overcome the platform motion disturbance stabilizing the helicopter on the vertical line. Table III
summarizes the root mean square errors for all the
variables in this case.
Regarding computational efficiency, Figure 7
shows the Task Execution Time (TET) of the filter,
which is the average measured CPU time to run
one sample interval. As can be seen, the average
time is 0.0035 s for the 100 Hz update rate and
0.0042 s for the 10 Hz update rate which made the
algorithm suitable for real-time execution within
the controller feedback.
VI. C ONCLUSION AND FUTURE WORK
5
0
-5
-10
-15
0
a
10
b
20
c
30
40
50
30
40
50
8
|error| [º]
Units
50
1.5
0
S motion
unknown
0.6570
0.6296
0.3392
0.8535
0.9829
7.6253
6
4
2
0
10
20
t [s]
Fig. 4. Experiment 1. Roll, pitch and yaw angles estimates and
absolute estimate errors
The SR-UKF has been implemented and tested
for attitude and relative position estimation of
a small-size tethered unmanned helicopter. The
state prediction was performed using a kinematic
process model driven by measurements of the
inertial sensors (accelerometer and gyroscope) and
the subsequent correction was done using data
of additional sensors like magnetometer, radar
altimeter and magnetic encoders measuring the
orientation of the tether relative to the helicopter.
It is remarkable that this approach avoids the need
of GPS as the position estimation is relative to the
tether anchor point.
Simulation results have shown a very good
performance in terms of estimate errors as well
as CPU time, making the algorithm suitable for
real-time execution within the controller feedback.
10
8
roll [º]
q 1 [º]
0
-5
6
4
-10
2
0
a
10
b
20
c
30
40
50
60
70
0
a
10
b
20
c
30
40
50
60
70
40
50
60
70
40
50
60
70
40
50
60
70
40
50
60
70
40
50
60
70
3
|error| [º]
|error| [º]
2
2
1
1.5
1
0.5
0
10
20
30
40
50
60
70
0
10
20
30
t [s]
t [s]
6
4
pitch [º]
q 2 [º]
5
0
-5
2
0
-2
-10
-4
0
a
10
b
20
c
30
40
50
60
70
0
10
b
20
c
30
3
2
|error| [º]
|error| [º]
2.5
a
1.5
1
2
1
0.5
0
10
20
30
40
50
60
70
0
10
20
30
t [s]
t [s]
10.5
yaw [º]
q 3 [m]
10
10
0
9.5
0
a
10
b
20
c
30
40
50
60
0
70
1.2
10
1
8
|error| [º]
|error| [m]
5
0.8
0.6
0.4
a
10
b
20
c
30
6
4
2
0.2
0
10
20
30
40
50
60
70
t [s]
Fig. 5. Experiment 2. Tether angles q1 , q2 and tether length q3
estimates and absolute estimate errors
0
10
20
30
t [s]
Fig. 6. Experiment 2. Roll, pitch and yaw angles estimates and
absolute estimate errors
R EFERENCES
0.01
0.009
0.008
TET [s]
0.007
0.006
0.005
0.004
0.003
0.002
0.001
0
0
10
20
30
40
50
t [s]
Fig. 7.
Filter Task Execution Time (TET)
An immediate extension of this work will be field
experiments with the real platform.
Another important fact to highlight is that the
information related to the platform state is needed
for the tether system state estimation. This can be
solved using the cable connecting the helicopter to
the ground station as a data link. Two possibilities
arise. The first one consists in directly passing
the platform state estimate to the helicopter flight
computer, externally calculated by the computer in
charge of the tether tension control device which
runs a filter for inertial and magnetic sensors
installed on the platform. The second consists
in passing the raw measurements from platform
sensors and running a parallel filter in the helicopter flight computer, which may require much
more CPU capacity. Nevertheless, simulation results have also shown the filter performance is
good enough for execution within the controller
feedback in the case the platform motion is totally
unknown.
ACKNOWLEDGMENTS
This work is supported by European Commission Project EC-SAFEMOBIL (FP7-ICT-20117) and Spanish Science and Innovation Ministry
RD&I National Plan Project CLEAR (DPI201128937-C02-01).
[1] D. Rye, “Longitudinal stability of a hovering, tethered rotorcraft.” Journal of Guidance, Control, and Dynamics, vol. 8,
no. 6, pp. 743–752, 1985.
[2] G. Schmidt and R. Swik, “Automatic hover control of an
unmanned tethered rotorplatform,” Automatica, vol. 10, no. 4,
pp. 393–403, 1974.
[3] S. Oh, K. Pathak, S. K. Agrawal, H. R. Pota, and M. Garratt,
“Approaches for a tether-guided landing of an autonomous
helicopter,” IEEE Transactions on Robotics, vol. 22, no. 3,
pp. 536–544, 2006.
[4] B. Ahmed and H. Pota, “Backstepping-based landing control
of a RUAV using tether incorporating flapping correction dynamics.” Proceedings of the American Control Conference,
2008, pp. 2728–2733.
[5] L. Sandino, D. Santamaria, M. Bejar, A. Viguria, K. Kondak, and A. Ollero, “Tether-guided landing of unmanned
helicopters without gps sensors.” Proceedings of the IEEE
International Conference On Robotics And Automation, 2014,
pp. 3096–3101.
[6] L. Sandino, M. Bejar, K. Kondak, and A. Ollero, “On the use
of tethered configurations for augmenting hovering stability
in small-size autonomous helicopters,” Journal of Intelligent
& Robotic Systems, vol. 70, no. 1, pp. 509–525, 2013.
[7] ——, “Advances in modeling and control of tethered unmanned helicopters to enhance hovering performance,” Journal of Intelligent & Robotic Systems, vol. 73, no. 1, pp. 3–18,
2014.
[8] S. J. Julier and J. K. Uhlmann, “A new extension of
the Kalman filter to nonlinear systems.”
Proceedings of AeroSense: the 11th International Symposium on
Aerospace/Defense Sensing, Simulation and Controls, 1997,
pp. 182–193.
[9] E. Wan and R. van der Merwe, “The unscented Kalman
filter for nonlinear estimation.” Proceedings of the IEEE
Symposium on Adaptive Systems for Signal Processing, Communications, and Control, 2000, pp. 153–158.
[10] E. Wan, R. van der Merwe, and A. T. Nelson, “Dual estimation and the unscented transformation,” Neural Information
Processing Systems, vol. 12, pp. 666–672, 2000.
[11] R. van der Merwe and E. Wan, “Sigma-point Kalman filters
for integrated navigation.” Proceedings of the 60th Annual
Meeting of the Institute of Navigation, 2004, pp. 641–654.
[12] ——, “The square-root unscented Kalman filter for state and
parameter-estimation.” Proceedings of the IEEE International
Conference on Acoustics, Speech, and Signal Processing,
2001, pp. 3461–3464.
[13] S. Lupashin and R. D’Andrea, “Stabilization of a flying
vehicle on a taut tether using inertial sensing.” Proceedings of
the IEEE/RSJ International Conference on Intelligent Robots
and Systems, 2013, pp. 2432–2438.
[14] S. Maus, S. Macmillan, S. McLean, B. Hamilton,
A. Thomson, M. Nair, and C. Rollins, The US/UK
World Magnetic Model for 2010-2015, NOAA technical
report NESDIS/NGDC ed., 2015. [Online]. Available:
http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml