2020 IEEE International Conference on Robotics and Automation (ICRA)

31 May - 31 August, 2020. Paris, France

Flydar: Magnetometer-based High Angular Rate Estimation during

Gyro Saturation for SLAM
Chee How Tan1 , Danial Sufiyan bin Shaiful1 , Emmanuel Tang1 , Jien-Yi Khaw1 ,
Gim Song Soh1 and Shaohui Foong1

Abstract— In this paper, the high angular rate estimation for

simultaneous localisation and mapping (SLAM) of a Flying Li-
DAR (Flydar) is presented. The proposed EKF-based algorithm
exploits the sinusoidal magnetometer measurement generated
by the continuously rotating airframe for estimation of the
robot hovering angular velocity. Significantly, the proposed
method does not rely on additional sensors other than existing
IMU sensors already being used for flight stabilization. The
gyro measurement and the gyro bias are incorporated as a
control input and a filter state respectively to enable estimation
even under gyro saturation condition. Additionally, this work
proposes leveraging on the inherently rotating locomotion to
generate a planar lidar scan using only a single-point laser
for possible lightweight autonomy. The proposed estimation
method was experimentally evaluated on a ground rotating rig
up to twice the gyro saturation limit with an effective rms Fig. 1. The Flydar is shown operating in a experimental flying area in the
error of 0.0045Hz; and on the proposed aerial platform − lab. The airframe continuously rotates to generate lift for controlled flight.
Flydar − hovering beyond the saturation limit with a rms
error of 0.0056Hz. Lastly, the proposed method for SLAM
using the rotating dynamics of Flydar was demonstrated with
a localisation accuracy of 0.11m. requirement, which may increase the wingspan. Hence, it is
extremely challenging to achieve both miniaturisation and
I. INTRODUCTION
autonomy as the requirements are conflicting.
Monocopters are a class of samara-inspired rotorcrafts
Tergu et. al. [10] proposed using an array of accelerometer
in which the entire craft continuously rotates, generating
to estimate the high angular rate of a monocopter using
lift for controlled flight. The early developmental work was
an extended kalman filter (EKF). Existing methods using
led by Lockheed Martin Advanced Technology Laboratories
MARG (Magnetic, Angular, and Gravity) sensor data, such
[1][2], MIT [3][4], and the University of Maryland [5].
as in [11] [12] that uses the accelerometer to provide a
Recent work by the Singapore University of Technology
vertical reference are not suitable for a monocopter that
and Design focuses on the mechanical design, optimisation
experiences a significant acceleration due to the rotating
and control of new classes of transformable and unpow-
dynamics [10]. While the approach in [10] was successfully
ered swarm-capable samara-inspired rotorcrafts, known as
demostrated up to 9Hz on monocopter flight, the additional
the Transformable HOvering Rotorcraft (THOR) [6][7] and
accelerometer sensors add weight and complexity to the
Samara Autorotating Wing (SAW) [8][9] respectively.
platform. Instead, this work proposes the use of sensor data
Recently, there has been keen interest towards 1) platform
that is already available on the IMU of the monocopter for
miniaturisation and 2) achieving autonomy of these platform.
high angular rate sensing. This is made possible due to the
The size of the platform is affected by a multitude of
sinusoidal nature of the magnetometer data generated from
factors, such as the rotation speed, wing span, payload,
the rotating dynamics of the airframe. The frequency of
and etc. Significantly, the low dynamic range of commer-
the sinusoid would represent the rotation rate of the robot.
cially available gyroscopic sensors severely inhibit further
Existing literature in the field of power system relating to
miniaturisation effort [10]. This is because smaller platform,
the frequency estimation of single-tone noisy real sinusoid
thereby a shorter wingspan and lesser aerodynamics surface,
is directly relevant to this work. In [13], an EKF-based
would entail a higher rotation speed to generate sufficient
algorithm is proposed for an online recursive frequency
lift for agile flight. Furthermore, the pursuit of autonomy
estimation of a noisy sinusoidal signal. The algorithm was
requires additional sensors that would increase the payload
demonstrated to track abrupt frequency changes of simulated
*Research supported by the SUTD-MIT International Design Centre signal effectively. The EKF-based approach is less compu-
(IDC) and by the Temasek Laboratories Defence Innovation Research tationally demanding compared to fourier transform- based
Programme (DIRP) IGDSP1502041. [14] and neural network-based algorithm [15] proposed in
1 The authors are with the Engineering Product Development Pillar,
Singapore University of Technology and Design (SUTD), 8 Somapah Road, literature. The algorithm proposed in this work extends on
Singapore 487372. [email protected] the the approach in [13] by incorporating the gyro sensor

978-1-7281-7395-5/20/$31.00 ©2020 IEEE 8532

 m̂ = m/kmk. The normalised earth frame magnetic field
mE is defined as
T
m̂E = [ mN 0 mD ] (1)
B
The normalised body frame measurement m̂ can be
modelled by a simple rotation of m̂E using the rotation
B
matrix RB
E . Then, m̂ is defined as
T
m̂B = [ mx my mz ] = RB
E m̂
E
(2)
where RB E describes the rotation from earth frame E to body
frame B in ZYX order and θ, φ, ψ represents the yaw, pitch,
and roll angle respectively.
Fig. 2. The axis of the earth-fixed NED frame and the body-fixed NED cθcφ cθsφ −sθ
 
are illustrated.
RB
E =
 −cψsφ + sψsθcφ cψcφ + sψsθsφ sψcθ  (3)
sψsφ + cψsθcφ −sψcφ + cψsθsφ cψcθ
measurement as a control input to better track transient The robot angular velocity is given as
changes and not just limited to abrupt frequency changes.
T
Our approach also incorporate the gyro bias as an estimated ω B = [ ωx ωy ωz ] (4)
state in the EKF to enable estimation of the high angular
In hover flight, where ωx = ωy = 0, the robot rotation
rate even under gyro saturation.
axis ω̂ B can be assumed to be aligned with the earth frame
The use of an omnidirectional 2D planar lidar or a 3D lidar
vertical axis z E [10]. Then, the robot hovering angular
is a common approach to enable autonomy on conventional
velocity wB h is given as
aerial robots, such as a quatcopter. The pointcloud generated
T B E
is fed into a simultaneous localisation and mapping (SLAM) wB
h = [ 0 0 ωh ] = wh ẑ = wh ẑ (5)
pipeline for autonomous navigation. Leveraging on the ro-
tating dynamics of a samara-inspired rotorcraft, a single- Finally, using (1) - (5), the discrete-time model of the
point laser is dynamically actuated for planar scanning. This magnetometer measurement during hover flight is derived.
work describes the integrated rotorcraft and lidar platform, The k−th normalised sample m̂B k is defined as
the Flying Lidar or Flydar. While conventional 2D lidar 
mN cos(wh kTs + φm )

has rotating mechanism with fixed known rate to generate B
m̂k =  mN sin(wh kTs + φm )  + k (6)
stable scans, the time-varing and unknown rotation rate of
mD
the Flydar poses a key challenge in this work.
In this work, an angular rate estimation algorithm that where the mx and my measures a sinusoid with angular
uses the magnetometer and gyrosope sensor data from frequency ωh , initial phase shift φm , and Ts denotes the
existing inertial measurement unit (IMU) measurement is sampling time, and k describes the measurement noise.
proposed. In addition, a lightweight single laser unit for B. Gyroscopic Sensor
SLAM is presented. The laser leverages on the rotating
locomotion of the rotorcraft to generate a planar lidar scan A 3-axis gyroscope mounted on the robot measures the
for lightweight autonomy. The Flydar running proposed angular velocity ω B
gyro about the body frame B. The dynamic
estimator and equipped with the laser scanning unit is shown range of the sensor is denoted as ωgyro, limit . Then, the gyro
in Fig. 1. output magnitude can be modelled as (7).
(
kωiB k if kωiB k ≤ ωgyro, limit ,
II. M ETHODS B
kωgyro,i k = (7)
ωgyro, limit if kωiB k ≥ ωgyro, limit .
In this work, the North-East-Down (NED) convention is
adopted for all reference frame. The basis of the earth- where the subscript i denotes the x, y, or z axis.
fixed frame E, x̂E , ŷ E , and ẑ E axes are oriented along the The gyro model often incorporates an additive term ωbias
north, east and down direction respectively. The body-fixed to account for the effects of gyro drifts due to thermal
frame B is assumed to be on the robot’s centre of mass, fluctuation. Instead, this work proposes utilising the gyro
with the x̂B , ŷ B , and ẑ B axes pointing towards the front, bias ωbias to capture the difference between the true hovering
starboard, and downward direction respectively. The frames B
angular velocity ωh and the saturated gyro output ωgyro , such
are illustrated in Fig. 2. that
ωbias,k = ωh,k − ωgyro,k (8)
A. Magnetometer Sensor Model
A magnetometer measures the earth’s magnetic field mE Finally, the discrete-time gyro model is given as
experienced in the robot body frame B. The (·) ˆ accent
ωgyro,z,k = ωh,k − ωbias,k + k (9)
denotes that the vector has a unit norm i.e. m̂ = 1 and

8533
 where uk is the control input defined in (15), xk is the state
vector defined in (14), and xk,i refers to the i-th element in
xk .
Thirdly, the measurement function is identified as
zk = g(x̂k , uk ) = xk,2 (17)
where zk is the x-axis measurement from the magnetometer.
Lastly, the hovering angular velocity of the robot can be
estimated as
ŵh = uk − x̂k,1 (18)
where x̂k,1 = ω̂bias is the estimated gyro bias.
D. Polar Extrapolation for Uniform Sampling
Fig. 3. The key components of Flydar are illustrated. The FMU mounts
onto the fuselage of the robot and optical markers are used to track the Significantly, the linear prediction model given in (12)
ground truth pose using motion capture system. used to derived the state transition function (16) assumes
a constant sampling period Ts . Hence, it is desired that
the EKF measurement update step is executed at a fixed
C. EKF-based Angular Rate Estimation frequency with period Ts . Then, zk is extrapolated from the
As derived in (6), the magnetometers measures a sinu- latest sample using the following
soidal signal due to the robot rotating dynamics for aerial cos(θk−1 + ω̂h,k−1 (tk − tk−1 )
zk = zk−1 · ) (19)
flight. As [13], the angular frequency of the sinusoidal signal cosθk−1
can be be estimated using an EKF. Formally, a nonlinear where zk−1 and θk−1 are the last magnetometer measure-
process can be modelled as ments and phase angle respectively, ω̂h,k−1 is the previous
xk+1 = f (xk , uk ) + wk (10) angular velocity estimated, tk is the EKF time now and tk−1
is time of arrival of zk1 sample.
zk = g(xk , uk ) + vk (11)
E. Planar Pointcloud for SLAM
where (10) describes the state transition process, and the
A single laser mounted on the robot generates n planar
measurement equation as (11).
scans per second due to the rotating dynamics of the robot.
The magnetometer model derived in (6) can be re-written
Assuming a laser with sampling rate of N per seconds, and
using linear prediction. It is well known that for a real
the robot with an instantaneous angular velocity ωh , the
sinusoid with fundamental angular frequency ωf , a closed- w
output rate of the scan flidar is simply 2π , and the average
form equation based on the past two samples can be used to
size of each scan m and the angular resolution ∆θ is defined
predict the future sample yk+1
as
N · 2π
yk+1 = 2 · cos(wf Ts ) · yk − yk−1 (12) m= (20)
ωh
Compared to the EKF presented in [13], the proposed 2π
∆θ = (21)
EKF in this work incorporates the gyroscope measurements, m
modelled as a control input. This improves the transient Let pθ be the range measurements from the laser received
tracking performance of the filter during take off and descent. at yaw angle θ. The measurements are accumulated in a
Additionally, the gyroscope bias ωbias is also modelled in the point cloud p and is output when the robot completes a full
filter, to account for the saturated gyroscope measurement de- revolution. For convenience, the magnetic north i.e. θ = 0, is
scribed in (7) when kωiB k > ωgyro, limit . Then, (12) becomes selected as reference. At θ = 0, which is the zero-crossing of
the robot’s yaw, a complete scan P is produced. The average
yk+1 = 2 · cos((ωgyro + ωbias )Ts ) · yk − yk−1 (13) angular rate ω¯h is used to distribute the points evenly across
In the following, the EKF formulation is outlined. Firstly, the point cloud.
let the state vector xk and the control input uk be During hover flight, the angular resolution across is ap-
proximately constant. However, during climb or descent, the
T
xk = [ wbias,k yk yk−1 ] (14) angular velocity of the robot varies significantly. As a result,
uk = ωgyro (15) consecutive scans will have different angular resolution.
III. E XPERIMENTAL R ESULTS
Secondly, the state transition function can then defined as
In this section, the hardware implementation of the Flydar
x̂k+1 = f (x̂k , uk ) Mapping Unit is first presented. This is followed by the
 xk,1 
experimental investigations to evaluate the performance of
(16)
=  2 · cos((uk + xk,1 )Ts ) · xk,2 − xk,3  the proposed angular rate estimation method and the mapping
xk,2 approach.

8534
Fig. 4. The key components are the FMU are illustrated. The Intel Edison
runs the proposed angular rate estimation algorithm and a single laser is used
to generate a planar scan from the rotating locomotion of the rotorcraft.

Fig. 6. The estimated angular frequency ω̂h is shown tracking a staircase

function. Despite ωgyro saturating at 5.5Hz, the ω̂h tracks the ground truth
encoder frequency ωtruth with a low rms error of 0.0045Hz.

Motor U10 brushless dc (BLDC) motor. The motor shaft

connects to a US Digital E6 Optical Encoder with 10,000
cycles per revolution (CPR) to provide the ground truth
angular rate of the rotation. A first Teensy 3.5 microcontroller
(MC) interfaces and processes the encoder data and outputs
the measured angular frequency via serial to a laptop. The
laptop records and saves the measurements. A second Teensy
Fig. 5. The experimental setup to simulate the physical rotation of the robot
3.5 MC commands the rotation speed of the rig by sending a
is illustrated. The FMU is mounted on the rotating rig which is actuated by pulse-width modulated (PWM) signal to the electronic speed
a high-torque BLDC motor. controller (ESC), which directly controls the power input to
the BLDC motor. In this experiment, the PWM commanded
was 1170 − 1310µs at 10µs interval.
A. Hardware Implementation In Fig. 6, the estimated angular frequency using the
The Flying LiDAR (Flydar) is shown in Fig. 3. It uses proposed estimated are illustrated against the ground truth
the DJI Snail 2305 powertrain, a Pixracer R15 autopilot and the gyro measurement. It is observed that the estimated
with built-in IMU, and the Flydar Mapping Unit (FMU). angular frequency fˆh was able to track the ground truth ftruth
Optical markers are mounted on the craft for ground truth accurately despite the saturated gyro fgyro . Significantly, the
measurements using motion capture system. The craft weighs estimator was tested to about twice the gyro saturation limit.
500g and has a wingspan of 1m. The rms error was 0.0045Hz.
The FMU, as shown in Fig. 4, consists of two mounting C. Evaluation of EKF Estimation on Flydar Platform
plates. The first plate holds the Pixhawk 2.1 autopilot and
In this experiment, the robot was flown manually by a
a 3000 mAh Lithium Polymer (LiPo) battery that powers
human pilot. The throttle input by the pilot commands the
the entire FMU, as seen from the top view. The second
robot to track a desired angular frequency, fd . Significantly,
plate holds a RPLIDAR-A3 single-point laser module, with
fd ≈ 5.8Hz is higher than the gyro dynamics range of
a sampling rate of up to 16,000 samples/sec, at the bottom.
fgyro,limit = 5.5Hz and the gyro saturates. The Optitrack
The modules receive and output data via a ribbon cable,
Motion Capture System was used to determine the ground
which is accessed through the ribbon cable breakout board.
truth angular frequency of the robot’s rotation. The results
Significantly, the Intel Edison Companion Computer (IECC)
are shown in Fig. 7. It is observed that the estimated
runs the proposed angular rate estimation algorithm and
angular frequency fˆh was able to track the ground truth
outputs the estimated angular frequency to a Teensy 3.5
ftruth accurately despite the saturated gyro fgyro . The rms
MC (TMC) via serial connection. The TMC relays the
error at steady state and for the entire flight was 0.056Hz
rotation rate to the laser module which outputs a complete
and 0.051Hz respectively. Fig. 8 illustrates the magnetometer
omnidirection scan via the UART to USB adapter to the
measurement zk at a one-second window between 34 < t <
IECC.
35, verifying the sinusoidal mode of the sensor data. The
B. Evaluation of Angular Rate Estimation on FMU Rotating predicted EKF state x2 tracks zk reasonably, explaining the
Rig low rms error of the angular frequency estimate.

A FMU rotating rig was custom-designed and developed D. Evaluation of SLAM Performance using Flydar
to physically simulate the rapid rotating motion of the craft. In this experiment, the pilot commands the robot to travel
The rig consists of the FMU mounted on a high-torque T- within a bounded area. The ground truth robot pose was

8535
Fig. 7. The angular frequency ω̂h estimated online on the Flydar is shown Fig. 9. The estimated x and y position output from the Google Cartographer
against the ωgyro and OptiTrack ground truth ω̂h . The steady-state rms error SLAM in 2D generated from the Flydar planar scan is shown against the
is recorded at 0.056Hz. ground truth pose of the OptiTrack. The rms euclidean error during the
flight was 0.11m.

Fig. 8. A one-second snapshot of the magnetometer measurement zk and

the estimated states x2 and x3 are shown. The predicted state x2 tracks the Fig. 10. The absolute x− and y− error of the position estimate from the
sinusoidal magnetometer sensor data zk closely. SLAM is shown.

R EFERENCES
recorded using the OptiTrack Motion Capture system. The
output of the laser scan was also recorded on the onboard [1] S. Jameson, B. Satterfield, C. Bolden, H. Youngren, and N. Allen,
“Samarai nano air vehicle – a revolution in flight,” in Proc. Association
IECC. The robot pose was recovered using Google Cartogra- for Unmanned Vehicle Systems (AUVSI) International Unmanned
pher SLAM in 2D offline with the playback of the recorded System North America 2007), Washington, DC, USA, Aug. 2007, pp.
laser data. The estimated trajectory from the SLAM and the 290–294.
[2] K. Fregene and C. L. Bolden, “Dynamics and control of a biomimetic
ground truth trajectory from the OptiTrack are shown in Fig. single-wing nano air vehicle,” in Proceedings of the 2010 American
9. The rms euclidean error during the flight was recorded at Control Conference, June 2010, pp. 51–56.
0.11m. The absolute x− and y− position error shown in Fig. [3] A. Kellas, “The guided samara : design and development of a
controllable single-bladed autorotating vehicle,” Master’s thesis, Mas-
10. The hover flight assumption (i.e. ωx = ωy = 0) and the sachusetts Institute of Technology, Cambridge, 2007.
processing of the SLAM in 2D (i.e. φ = ψ = 0) might have [4] J. Houghton and W. Hoburg, “Fly-by-wire control of a monocopter,”
contributed to the position error. The map generated from in Experimental Projects II Tech. Rep. Massachusetts Institute of
Technology, 2008.
Flydar using Google Cartograhper in 2D is shown in Fig. [5] E. R. Ulrich, D. J. Pines, and J. S. Humbert, “From falling to flying:
11. the path to powered flight of a robotic samara nano air vehicle,”
Bioinspiration & Biomimetics, vol. 5, no. 4, p. 045009, nov 2010.
[6] J. E. Low, L. T. S. Win, D. S. B. Shaiful, C. H. Tan, G. S. Soh, and
IV. C ONCLUSION S. Foong, “Design and dynamic analysis of a transformable hovering
rotorcraft (thor),” in 2017 IEEE International Conference on Robotics
In this paper, a Flydar was demonstrated to perform SLAM and Automation (ICRA), May 2017, pp. 6389–6396.
operation. A EKF-based algorithm was proposed to estimate [7] D. Sufiyan, L. T. S. Win, S. K. H. Win, G. S. Soh, and S. Foong,
the high angular rate of the robot. Significantly, the algorithm “A reinforcement learning approach for control of a nature-inspired
aerial vehicle,” in 2019 International Conference on Robotics and
was demonstrated to estimate beyond the gyro saturation Automation (ICRA), May 2019, pp. 6030–6036.
limit successfully, up to a rms error of 0.0056Hz. The angular [8] S. K. Hla Win, T. H. Goh, J. E. Low, D. S. Bin Shaiful, L. T. Soe
rate was used to output a planar lidar scan from a single- Win, G. S. Soh, and S. Foong, “Direction controlled descent of samara
autorotating wings (saw) with n-wings * research supported by the
point laser, mounted on the rotating Flydar. Flydar achieved sutd-mit international design centre (idc) and by the temasek labora-
a localisation accuracy of 0.11m. tories defence innovation research programme (dirp) igdsp15020141.”

8536
Fig. 11. The final map output from the Google Cartographer SLAM is
shown.

in 2018 IEEE International Conference on Robotics and Automation

(ICRA), May 2018, pp. 6553–6559.
[9] S. K. H. Win, L. S. T. Win, D. Sufiyan, G. S. Soh, and S. Foong,
“Dynamics and control of a collaborative and separating descent of
samara autorotating wings,” IEEE Robotics and Automation Letters,
vol. 4, no. 3, pp. 3067–3074, July 2019.
[10] T. S. Lembono, J. E. Low, L. S. T. Win, S. Foong, and U. Tan,
“Orientation filter and angular rates estimation in monocopter using
accelerometers and magnetometer with the extended kalman filter,”
in 2017 IEEE International Conference on Robotics and Automation
(ICRA), May 2017, pp. 4537–4543.
[11] S. O. H. Madgwick, A. J. L. Harrison, and R. Vaidyanathan, “Estima-
tion of imu and marg orientation using a gradient descent algorithm,”
in 2011 IEEE International Conference on Rehabilitation Robotics,
June 2011, pp. 1–7.
[12] J. L. Marins, Xiaoping Yun, E. R. Bachmann, R. B. McGhee, and M. J.
Zyda, “An extended kalman filter for quaternion-based orientation
estimation using marg sensors,” in Proceedings 2001 IEEE/RSJ In-
ternational Conference on Intelligent Robots and Systems. Expanding
the Societal Role of Robotics in the the Next Millennium (Cat.
No.01CH37180), vol. 4, Oct 2001, pp. 2003–2011 vol.4.
[13] A. Routray, A. K. Pradhan, and K. P. Rao, “A novel kalman filter
for frequency estimation of distorted signals in power systems,” IEEE
Transactions on Instrumentation and Measurement, vol. 51, no. 3, pp.
469–479, June 2002.
[14] Z. Gang-bing, L. Yu, X. Jia-jia, and H. Guo-bing, “Frequency es-
timation based on discrete fourier transform and least squares,” in
2009 International Conference on Wireless Communications Signal
Processing, Nov 2009, pp. 1–4.
[15] P. Dash, D. Swain, A. Routray, and A. Liew, “An
adaptive neural network approach for the estimation of
power system frequency,” Electric Power Systems Research,
vol. 41, no. 3, pp. 203 – 210, 1997. [Online]. Available:
http://www.sciencedirect.com/science/article/pii/S0378779696011868

8537