Attitude Estimation Method Using Kalman Filter

for MARG Sensors and Its Application

Zeyang Dai

A DISSERTATION

SUBMITTED IN FULFILLMENT OF THE REQUIREMENTS

FOR THE DEGREE OF DOCTOR OF PHILOSOPHY
IN COMPUTER SCIENCE AND ENGINEERING

Graduate Department of Computer and Information Systems

The University of Aizu

2021
c Copyright by Zeyang Dai, 2021

All Rights Reserved.

Contents

Abstract x

Chapter 1 Introduction 1
1.1 Attitude Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1.1 Coordinate systems . . . . . . . . . . . . . . . . . . . . . . . . 2
1.1.2 Attitude representation . . . . . . . . . . . . . . . . . . . . . . 2
1.1.3 MARG sensors model . . . . . . . . . . . . . . . . . . . . . . 7
1.1.4 Wahba’s problem . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.1.5 Attitude kinematics . . . . . . . . . . . . . . . . . . . . . . . . 9
1.1.6 Dynamic filtering and estimation . . . . . . . . . . . . . . . . . 9
1.1.7 Issues in practical applications . . . . . . . . . . . . . . . . . . 10
1.2 Thesis Organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.3 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
1.4 Publications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

Chapter 2 Related Work 15

2.1 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2 Noise Identification . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

Chapter 3 Lightweight Extended Kalman Filter for MARG Sensors Atti-

tude Estimation 19
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.2 Orientation from Observation Vectors . . . . . . . . . . . . . . . . . . 21
3.3 Filter Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.4 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.4.1 Experimental environment and data collection . . . . . . . . . . 26
3.4.2 Attitude estimation evaluation . . . . . . . . . . . . . . . . . . 28
3.4.3 Robustness analysis . . . . . . . . . . . . . . . . . . . . . . . . 32
3.4.4 Comparison with representative kalman filters . . . . . . . . . . 33
3.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.6 Appendix:Derivation of Jacobian Matrix . . . . . . . . . . . . . . . . . 36

Chapter 4 Real-Time Attitude Estimation of Sigma-Point Kalman Filter

via Matrix Operation Accelerator 38
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
4.2 Sigma-Point Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . 39
4.2.1 Filter structure . . . . . . . . . . . . . . . . . . . . . . . . . . 39
4.2.2 Sigma-point approach . . . . . . . . . . . . . . . . . . . . . . . 41
4.3 System Architecture Overview . . . . . . . . . . . . . . . . . . . . . . 42

iv
 4.3.1 Filter design . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.3.2 Matrix operation accelerator . . . . . . . . . . . . . . . . . . . 43
4.4 Implementation and Evaluation . . . . . . . . . . . . . . . . . . . . . . 44
4.4.1 Performance metrics . . . . . . . . . . . . . . . . . . . . . . . 44
4.4.2 Experiment setup . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.4.3 Performance of proposed accelerator . . . . . . . . . . . . . . . 45
4.4.4 Performance of accelerated filters . . . . . . . . . . . . . . . . 46
4.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

Chapter 5 Wireless Time Synchronized Motion Trackers Using Kalman

Filtering to Multi-rate Sensor Fusion 48
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
5.2 Work Principles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
5.2.1 Motion tracker overview . . . . . . . . . . . . . . . . . . . . . 50
5.2.2 Motion capture system . . . . . . . . . . . . . . . . . . . . . . 51
5.2.3 Segment kinematics . . . . . . . . . . . . . . . . . . . . . . . . 53
5.3 Attitude Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.3.1 Filter design . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.3.2 Filter setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.4 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.4.1 Results of Allan variance . . . . . . . . . . . . . . . . . . . . . 58
5.4.2 Accuracy of the attitude estimation . . . . . . . . . . . . . . . . 59
5.4.3 Time synchronization assessment . . . . . . . . . . . . . . . . 60
5.4.4 Validation during walking . . . . . . . . . . . . . . . . . . . . 61
5.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

Chapter 6 Conclusion 65

Acknowledgment 67

References 75

v
List of Figures

Figure 1.1 NED Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . 2

Figure 1.2 Rotation with Respect to a Fixed Coordinate . . . . . . . . . . . . 2
Figure 1.3 Rotation from Frame A to Frame B, of Angle θ Around the Axis A r̂ 4
Figure 1.4 Thesis Structure and the Relationship of Three Main Chapters . . 11

Figure 2.1 Structure of Kalman Filter . . . . . . . . . . . . . . . . . . . . . 16

Figure 2.2 Sample Plot of Square-Root Allan Variance Analysis Results [1] . 17

Figure 3.1 Workflow Diagram of the Proposed LEKF . . . . . . . . . . . . . 24

Figure 3.2 Experimental Devices: WitMotion HWT905 and NanoPi Duo2 . . 26
Figure 3.3 Experimental Platform . . . . . . . . . . . . . . . . . . . . . . . 27
Figure 3.4 Unsynchronized and Synchronized Angular Rates . . . . . . . . . 28
Figure 3.5 Raw Data of Trial1 . . . . . . . . . . . . . . . . . . . . . . . . . 29
Figure 3.6 Consequent Movement of Trial1, Units: cm . . . . . . . . . . . . 29
Figure 3.7 Estimated Euler Angles of Trial1 . . . . . . . . . . . . . . . . . . 30
Figure 3.8 Raw data of Trial4 . . . . . . . . . . . . . . . . . . . . . . . . . 31
Figure 3.9 Estimated Euler Angles of Trial4 . . . . . . . . . . . . . . . . . . 31
Figure 3.10Norm of the Magnetic Field . . . . . . . . . . . . . . . . . . . . . 32
Figure 3.11Estimated Euler Angles of Clean and Polluted Data . . . . . . . . 33
Figure 3.12FFT Analysis Result of the Accelerometer‘s Axis-Z Values of Trial4 33
Figure 3.13Estimated Euler Angles of Compared Filters . . . . . . . . . . . . 35
Figure 3.14Consumed Time of the Proposed LEKF and Guo’s FKF . . . . . . 35

Figure 4.1 Framework of 9-axis Attitude and heading reference system (AHRS) 42
Figure 4.2 Block of the Matrix Operation Accelerator . . . . . . . . . . . . . 44
Figure 4.3 Architecture of the Experiment Platform . . . . . . . . . . . . . . 44

Figure 5.1 Overview of the Motion Tracker Prototype . . . . . . . . . . . . . 51

Figure 5.2 Block Diagram of the Motion Tracker . . . . . . . . . . . . . . . 52
Figure 5.3 Schematic of Segment Kinematics . . . . . . . . . . . . . . . . . 53
Figure 5.4 Allan Deviation of the Tested Motion Tracker . . . . . . . . . . . 58
Figure 5.5 TExperimental Device for Attitude Estimation Evaluation . . . . . 59
Figure 5.6 Vector Length Versus Calculated Time Offset . . . . . . . . . . . 60
Figure 5.7 Working Time Versus Time Offset . . . . . . . . . . . . . . . . . 61
Figure 5.8 Hip: Joint Angle between Waist and Thigh . . . . . . . . . . . . . 61
Figure 5.9 Knee: Joint Angle between Thigh and Calf . . . . . . . . . . . . . 62
Figure 5.10Ankle: Joint Angle between Calf and Foot . . . . . . . . . . . . . 62
Figure 5.11Motion Tracking during Walking . . . . . . . . . . . . . . . . . . 63

vi
List of Tables

Table 3.1 RMSE [in deg] of Attitude Estimation . . . . . . . . . . . . . . . 29

Table 3.2 RMSE [in deg] of representative Kalman filters . . . . . . . . . . 34

Table 4.1 Resource Utilization of Matrix Operation Accelerator . . . . . . . 45

Table 4.2 Calculation Cycles and Speedup of Matrix Operation . . . . . . . 46
Table 4.3 Execution Time of Square-Root Sigma-point Kalman filter (SPKF)
in One Iteration . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

Table 5.1 Major configurable parameters of the tracker . . . . . . . . . . . . 51

Table 5.2 Results of Noise Identification . . . . . . . . . . . . . . . . . . . 59
Table 5.3 Root mean square error (RMSE) [in deg] for Attitude Estimation . 60
Table 5.4 RMSE [in deg] of Joint Angles . . . . . . . . . . . . . . . . . . . 63

vii
List of Abbreviations

AHRS Attitude and heading reference system

CDKF Central difference Kalman filter
CF Complementary filter
CKF Cubature Kalman filter
DCM Direction cosine matrix
DMA Direct memory access
eCompass Electronic compass
EKF Extended Kalman filter
ENU East, North, Up coordinates
ESOQ EStimation of Optimal Quaternion
FCF Fast complementary filter
FKF Fast Kalman filter
FOAM Fast Optimal Attitude Matrix
FOMA Fast optimal matrix algorithm
FPGA Field Programmable Gate Arrays
GDA Gradient descent algorithm
GNA Gauss newton algorithm
IKF Indirect Kalman filter
INS Inertial navigation system
KF Kalman filter
LEKF Lightweight Extended Kalman Filter
LMA Levenberg-Marquadt algorithm
MARG Magnetic, angular rate and gravity sensor array
MEMS Micro-electromechanical Systems
MoCap Motion capture
NED North, East, Down coordinates
PL Programmable logic
PS Processing system
PSD Power spectral density
QUEST QUaternion ESTimation
RMSE Root mean square error
SD card Secure digital card
SDIO bus Secure digital input output bus
SPKF Sigma-point Kalman filter
SVD Singular Value Decomposition
TCP/IP Transmission Control Protocol/Internet Protocol
UAV Unmanned Aerial Vehicle

viii
UKF Unscented Kalman filter
VFPU Vector floating point unit
Wi-Fi Wireless Fidelity

ix
Abstract

Attitude estimation using magnetic, angular rate and gravity (MARG) sensor array
is an essential technique for pervasive applications such as controlling of the Unmanned
Aerial Vehicle (UAV) and motion capture. These applications employ attitude estima-
tion techniques to obtain the orientation of an object with respect to the Earth’s frame.
Although an abundance of attitude estimation algorithms has been proposed in the past
decades, still there are some challenges, including the trade-off between the accuracy
and the computational effort, accelerating of the advanced estimation algorithm, and
adapting for the emerging field of motion capture. To deal with them, this thesis ad-
dresses these problems in order.
At first, in order to reduce the computational effort and provide a reliable solution
for critical constrained resource applications, a quaternion based Lightweight Extended
Kalman Filter (LEKF) is proposed. In this filter, we devise a simplified measurement
model to process acceleration and magnetic field observations. With this model, on
one hand, it need a few computations to process observation vectors. On the other
hand, half parameters of Jacobian matrix are constant and others have common factors,
this further reduces the computational effort. Moreover, theoretically the accuracy is
maintained due to without any linearization of the nonlinear observation model. Ex-
perimental results show that the proposed filter provides better performance in terms of
either attitude estimation accuracy or computational time.
Further, a matrix operation accelerator is investigated to accelerate the sigma-point
Kalman filter (SPKF). Comparing with the typical extended Kalman filter (EKF), SPKF
can provide better precision and is more robust, but at the expense of higher com-
putational complexity. On the other hand, the designed accelerator provides a good
reusability as well. Experiments are conducted on Zynq-7020, and results indicate that
the proposed scheme can reduce about 50% computing time.
Finally, we studied the motion capture system and designed wireless time synchro-
nized motion trackers. Each tracker is composed of a microcontroller enabling Wireless
Fidelity (WiFi) connectivity and a MARG sensor array. With built-in WiFi connectiv-
ity, it’s easily to establish the motion capture system by connecting to a given wireless
router, which actually is an scalable asynchronous sensor network. Thus, time syn-
chronization of these trackers is first investigated and an easy-to-operate scheme is pro-
posed, which avoids the complicated hardware design and reduces the cost significantly.
What’s more, to take full advantage of the performance of the sensors and enable the
sensors working at an asynchronous measurement rate, the traditional solution EKF is
adapted for multi-rate sensor fusion. Last but not least, several experiments are car-
ried out to validate the feasibility of the proposed motion tracker for motion capture.
Experimental results fully prove the reliability of the method for time synchronization
and that the proposed motion tracker is feasible for motion capture and there is a good
consistency with the optical system.
Chapter 1

Introduction

1.1 Attitude Estimation

Attitude estimation, which is responsible for determining the orientation of the ob-
ject in three dimensional space, has become the fundamental component in most of
modern devices. For example, in controlling of Unmanned Aerial Vehicle (UAV), the
correct attitude information are crucial for feedback in the governing control system. In
smartphones, attitude information can be utilized for navigation, which has significantly
improved user experience in modern daily life. And in gaming industry, active gaming
platform is getting more and more popular, where the gaming devices gather the attitude
information of users, so as to expand the capability of this style of game, or help open
total virtual reality scenarios at the edge of the augmented reality. Attitude estimation
of the aforementioned applications is achieved with low-cost magnetic, angular rate and
gravity (MARG) sensor array with the recent advances in Micro-electromechanical Sys-
tems (MEMS), which normally consists of a 3-axis accelerometer, a 3-axis gyroscope,
and a 3-axis magnetometer.
In this section, we provide the knowledge review of attitude estimation using Mag-
netic, angular rate and gravity sensor array (MARG) sensors. The fundamental con-
cepts are presented in order, including the coordinate system, attitude representations,
the MARG sensors model, two fundamental methodologies for extracting attitude infor-
mation from MARG sensors readings, in which the first is known as Wahba’s problem,
also termed as deterministic attitude estimation of the accelerometer and magnetome-
ter, and the second is about the attitude kinematics which obtains the attitude through
integrating the gyroscope measurements. Then, the concept of dynamic filtering and
estimation is introduced, which is actually the sensor fusion technique, and aims at
eliminating the bottleneck of single sensor. Last but not least, issues in practical appli-
cations are discussed.

1
 CHAPTER 1. INTRODUCTION

1.1.1 Coordinate systems

Figure 1.1: NED Coordinates

For Attitude and heading reference system or local tangent plane coordinates, there
are two well-known coordinate systems; East, North, Up coordinates (ENU) and North,
East, Down coordinates (NED). The former is used in geography, while the later one
is specially used in aerospace. The local ENU coordinates are formed from a plane
tangent to the Earth’s surface fixed to a specific location and the east axis is labeled as
x, the north y and the up z by convention, while for NED coordinates the north axis is
labeled as x, the east y and the down z. It is easy to find that, ENU coordinates and NED
coordinates are similar except for swapping ’down’ for ’up’ and x for y. In this paper,
like most of other researchers, NED coordinate system is chosen as the benchmark and
implemented for all the designed applications in this thesis, as defined in Fig. 1.1.

1.1.2 Attitude representation

Figure 1.2: Rotation with Respect to a Fixed Coordinate

With respect to a reference frame, we have three frequently used ways to describe
the orientation of a rigid body, they are the equivalent form and include the Euler angles,
Direction cosine matrix (DCM) and quaternion respectively. In the following, we will

2
1.1. ATTITUDE ESTIMATION

discuss the three representations separately and describe the transformational relation
between them. The Euler angles are three angles introduced by Leonhard Euler, in
general they are termed as roll, pitch and yaw, and denoted as φ, θ and ψ or α, β and
γ in mathematical. According to Euler’s rotation theorem, the three angles can be used
to describe any rotation, as shown in Fig. 1.2 where φ is the angle between the x axis
and the N axis, θ for the angle between the z axis and the Z axis and ψ for the angle
between the N axis and the X axis. We assume the rotation is in x-convention. When
writing the rotation in terms of rotation matrices, the rotation is defined to be

R(φ, θ, ψ) = R(ψ)R(θ)R(φ) (1.1)

where
 
cos(ψ) sin(ψ) 0
 
R(ψ) = −sin(ψ) cos(ψ) 0 (1.2)
0 0 1
 
1 0 0
 
R(θ) = 0 cos(θ) sin(θ) (1.3)
0 −sin(θ) cos(θ)
 
cos(φ) sin(φ) 0
 
R(φ) = −sin(φ) cos(φ) 0 (1.4)
0 0 1

Henceforth, the final equation can be rewritten as

 
r11 r12 r13
 
R(φ, θ, ψ) = r21 r22 r23  (1.5)
r31 r32 r33

where
r11 = cos(ψ)cos(φ) − cos(θ)sin(φ)sin(ψ)
r12 = cos(ψ)sin(φ) + cos(θ)cos(φ)sin(ψ)
r13 = sin(ψ)sin(θ)
r21 = −sin(ψ)cos(φ) − cos(θ)sin(φ)cos(ψ)
r22 = −sin()sin(φ) + cos(θ)cos(φ)cos(ψ) (1.6)
r23 = cos(ψ)sin(θ)
r31 = sin(θ)sin(φ)
r32 = −sin(θ)cos(φ)
r33 = cos(θ)

3
 CHAPTER 1. INTRODUCTION

h i
The derived matrix is the so-called DCM. Let ω = ωx ωy ωz be the angular ve-
locity on axis x, y and z respectively of the rigid body expressed in the body frame B,
then, the rigid body kinematics equation is given by

Ṙ = RS(ω) (1.7)

where S(ω) is the skew symmetric matrix and given by

 
0 −ωz ωy
 
S(ω) =  ωz 0 −ωx  (1.8)
−ωy ωx 0

For the rotation from the sensor frame to the body frame, the kinematics is expressed

X'
A
B
Y

Z'

̂ 

Z
Y'

Figure 1.3: Rotation from Frame A to Frame B, of Angle θ Around the Axis A r̂

as
Ṙ = −S(ω)R (1.9)

A quaternion, introduced in [2–4], is a four dimensional complex number and can be

used to represent arbitrary orientation as well. For example, in Fig. 1.3, any orientation
of frame B relative to frame A can be achieved through a rotation of angle θ around an
axis A r̂ defined in frame A, we employ the quaternion BA q to express the rotation, which
is defined by
h i
B
Aq = w x y z
h i
= q0 q 1 q2 q3 (1.10)
h i
= cos( 2θ ) −rx sin( 2θ ) −ry sin( 2θ ) −rz sin( 2θ )

where rx , ry and rz are the components of the units vector A r̂ in x, y and z axes of
frame A respectively, while the elements, w, x, y, z and q0 , q1 , q2 q3 , are the frequently-
used different equivalent forms of the quaternion definition. Conventionally, all the

4
1.1. ATTITUDE ESTIMATION

quaternions are an unit length, this is to say, the quaternion is subject to

kqk2 = q02 + q12 + q22 + q32 = 1 (1.11)

With the quaternion of unit length, the frequently used quaternion arithmetic is defined
as the following. The conjugate of the quaternion is
h i
B ∗
Aq =A
B q = q 0 −q 1 , −q 2 , −q 3 (1.12)

where ∗ denotes the conjugate of the quaternion. The quaternion product of two quater-
nions, a and b, is defined by
h i h i
a ⊗ b = a0 a1 a2 a3 ⊗ b 0 b 1 b 2 b 3
 T
a0 b 0 − a1 b 1 − a2 b 2 − a3 b 3
  (1.13)
 a0 b 1 + a1 b 0 + a2 b 3 − a3 b 2 

= 
a b − a b + a b + a b 
 0 2 1 3 2 0 3 1

a0 b3 + a1 b2 − a2 b1 + a3 b0

where ⊗ denotes the Hamilton product. Note that the quaternion product is not com-
mutative, that is, a ⊗ b 6= b ⊗ a. With another quaternion C
B q to express the rotation
from frame B to frame C, the compounded orientation A q can be obtained by
C

C
Aq =B C
Aq ⊗ Bq (1.14)

For the rotation between two same vectors, A v and B v, which are described in frame A
and frame B respectively, we insert 0 to each vector as the first element and figure out
the rotation by
B ∗
B
v=B A
Aq ⊗ v ⊗ Aq (1.15)

A q also can
As mentioned before, these representation are equivalent. The orientation B
be represented as the rotation matrix defined by
 
2q02 − 1 + 2q12 2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 )
 
R(B 2
A q) = 2(q1 q2 − q0 q3 ) 2q0 − 1 + 2q2
2
2(q2 q3 + q0 q1 ) (1.16)
2(q1 q3 + q0 q2 ) 2(q2 q3 − q0 q1 ) 2q02 − 1 + 2q32

where the conversion is a homogeneous expression. The reverse mapping, from the ro-
tation matrix to the quaternion, though the Shepperds’s method [5] is usually considered

5
 CHAPTER 1. INTRODUCTION

as the solution, the revised framework [6] is recommended and defined by



 t0 if r22 > −r33 , r11 < −r22 , r11 > −r33




1 (r23 − r32 )/t0 if r22 < −r33 , r11 > r22 , r11 > r33
q0 = (1.17)
2 (r31 − r13 )/t0 if r22 > r33 , r11 < −r22 , r11 < −r33




(r − r )/t if r22 < r33 , r11 < −r22 , r11 < r33
12 21 0

√
where t0 = 1 + r11 + r22 + r33 ,


 (r23 − r32 )/t1 if r22 < −r33 , r11 > r22 , r11 > r33




1 t1 if r22 > −r33 , r11 < −r22 , r11 > −r33
q1 = (1.18)
2 (r12 + r21 )/t1 if r22 > r33 , r11 < −r22 , r11 < −r33




(r + r )/t if r22 < r33 , r11 < −r22 , r11 < r33
31 13 1

√
where t1 = 1 + r11 − r22 − r33 ,


 (r31 − r13 )/t2 if r22 < −r33 , r11 > r22 , r11 > r33




1 (r12 + r21 )/t2 if r22 > −r33 , r11 < −r22 , r11 > −r33
q2 = (1.19)
2 t2 if r22 > r33 , r11 < −r22 , r11 < −r33




(r + r )/t if r22 < r33 , r11 < −r22 , r11 < r33
23 32 2

√
where t2 = 1 − r11 + r22 − r33 ,


 (r12 − r21 )/t3 if r22 < −r33 , r11 > r22 , r11 > r33




1 (r31 + r13 )/t3 if r22 > −r33 , r11 < −r22 , r11 > −r33
q3 = (1.20)
2 (r23 + r32 )/t3 if r22 > r33 , r11 < −r22 , r11 < −r33




t if r22 < r33 , r11 < −r22 , r11 < r33
3

√
where t3 = 1 − r11 − r22 + r33 . This is the revised version and more accurate and
stable. The Euler angles can be extracted from B
A q by

ψ = atan2(2q1 q2 − 2q0 q3 , 2q02 + 2q22 − 1)

θ = −asin(2q1 q3 + 2q0 q2 ) (1.21)
φ = atan2(2q2 q3 − 2q0 q1 , 2q02 + 2q32 − 1)

where atan2 and asin are inverse the trigonometric functions.

6
1.1. ATTITUDE ESTIMATION

1.1.3 MARG sensors model

As mentioned above, each MARG sensor is composed of a 3-axis MEMS angular
rate gyro traid, a 3-axis MEMS accelerometer, and a 3-axis magnetometer. Regardless
of which sensor of them, there are many kinds of noise when in use, which include such
as the scale factor, misalignment, and bias. In order to mathematically correct these
errors, the sensor model is required so that we can conduct analysis.
For modern MARG sensors, the main sources of the sensor error include bias, scale
factor and non-orthogonality, henceforth, an unified sensor model [7] is used in this
paper, that is
yk = Sk Tk uk − bk + nk (1.22)

where k represents the sensor type, uk is the truth physical quantities in metric unit, and
yk is the sensor reads. Sk is the scale factor. Tk is the Gram–Schmidt orthogonalization
matrix for compensating the error of non-orthogonality. bk is the bias vector, normally
it’s the constant. nk is the vector of white noise caused by thermo-mechanical events
and stated as a zero mean process with a standard deviation equal to σk . For the 3-axis
sensor, yk , uk , bk and nk is 3 × 1 vectors, while Sk and Tk are 3 × 3 matrices. Although
the sources of error are almost same, they have different contributions to the overall
orientation accuracy for different sensors, we discuss them separately in the following.
A gyroscope is the sensor that measures the angular rate, the orientation for which
is obtained by integrating the measured angular rate over time. Evidently, constant bias
errors grow linearly with time and the bias has a significant effect on the accuracy of
the orientation. But not to worry, we can find the constant bias error of a gyro by taking
the average of the output over a long period of time while the device is not rotating, and
subtract it from subsequent measurements to eliminate this form of error. In contrast,
white noise has no effect on the integrated orientation due to it is a zero mean process.
Besides, the bias stability, not represented in the sensor model, generally it is very small
and not necessary to concern for the low-cost MARG, is the another often concerned
noise resource and tell us how stable the bias of a gyroscope is over a certain specified
period of time.
An accelerometer is responsible for measuring the acceleration. In modern devices,
the bias of the accelerometer is very small and has a minimal effect on the accuracy of
the orientation.
An magnetometer measures the strength and direction of the local magnetic field.
Different with the gyroscope and the accelerometer, the main sources of error are the
soft iron and hard iron due to its measurements are subjected to distortion. Soft iron
distortions are considered deflections or alterations in the existing magnetic field and
commonly caused by metals such as nickel and iron, while hard iron distortion caused
by the magnetic material or magnetized objects will cause a permanent bias in the sensor

7
 CHAPTER 1. INTRODUCTION

output. In actual use, the magnetometer is required to do the ellipse fitting to compen-
sate such two error for a given application scenario.

1.1.4 Wahba’s problem

Wahba’s problem [8], also known as static attitude determination, involves numeri-
cally determining the attitude taking advantage of the body vector observations without
considering its kinematics. For instance, the attitude determination using the measure-
ments of accelerometer and magnetometer. In this problem, simultaneously two sets of
N observed unit vectors V1 , ..., VN and W1 , ..., WN are respectively known in the refer-
ence coordinate system and the body frame, the attitude is merely regarded as a matrix
that transforms the reference vectors V1 , ..., VN in the reference frame to the vectors
V1 , ..., VN in the body frame and as a result, can be obtained by mathematical optimiza-
tion techniques. In this way, the rotation matrix, orthogonal matrix R, is numerically
found by minimizing
N
1X
L(R) = ai |Wi − RVi |2 (1.23)
2 i=1
PN
where the ai are non-negative weights. By normalizing the weights, that is, i=1 ai =
1, the loss function is rewritten in the straightforward form

N
X
L(R) = 1 − ai WiT RVi = 1 − tr(RB T ) (1.24)
i=1

where tr denotes the trace of the matrix and B is defined as

N
X
B= ai Wi ViT (1.25)
i=1

This equation converts the problem to maximum the term tr(RB T ) by finding the ap-
propriate R and attitude is found on an optimization basis. The found attitude normally
comes with a lower accuracy due to the discarding of the information of the system
dynamics.

Solutions to the Wahba’s least squares problem includes the earliest attitude re-
construction method TRAID [9], Singular Value Decomposition (SVD) method [8],
Fast Optimal Attitude Matrix (FOAM) [10], Q-Method [11], QUaternion ESTimation
(QUEST) [12], EStimation of Optimal Quaternion (ESOQ) [13–15], the recent consid-
eration [16] and so on. For more details of this problem readers are referred to [17].

8
1.1. ATTITUDE ESTIMATION

1.1.5 Attitude kinematics

As discussed before, gyroscope measures the angular rate of the rigid body in the
sensor frame. With an known initial condition, the attitude can be obtained by inte-
grating the readings of gyroscope over time. In this way, we have to derive the attitude
kinematics, which relates the attitude at time step k to the attitude at step k + 1. Substi-
tuting the formula 1.9 and representing in quaternion,

1
q̇ = [Ω×]q (1.26)
2

where  
0 −ωx −ωy −ωz
 
ωx 0 ω z −ω y 
[Ω×] = 
ω −ω

 (1.27)
 y z 0 ω x 
ωz ωy −ωx 0
Then integrating over time is
 
1
qk+1 = I4×4 + [Ω×] qk (1.28)
2

where k denotes the time step, while I4×4 is the 4×4 identity matrix. The state transition
formula takes the full system dynamics into account. However, when there are bias
errors that occurs in the gyroscope readings, this iterative formula would lead to the
significant accumulative error of the estimated angles with the increasing of time. As
so, we generally fuse the measurements of the gyroscope and observations from the
accelerometer and magnetometer together as the reliable solution, this is also called the
sensor fusion techniques and leads us to the next topic.

1.1.6 Dynamic filtering and estimation

Dynamic filtering and estimation is the sensor fusion technique that aims at elimi-
nating the bottleneck of each sensor, and generally called attitude estimation algorithms.
In the past decades, there are amounts of attitude estimation algorithms that have been
developed. Theses algorithms can be divided into two categories roughly; the Comple-
mentary filter (CF) and Kalman filter (KF) based methods. Comparing with KF based
methods, CF based methods obtain the accurate attitude by compensating for each other
in the frequency domain and have more efficient computations. Early thoughts of CF
based methods are raised around 1990 by Foxlin who first applied MARG sensors to
monitor the attitude motion of human’s head [18]. In recent, Madgwick computed
the attitude using Gradient descent algorithm (GDA) via the quaternion representation
in [19]. Particularly Wu proposed faster and more robust Fast complementary filter

9
 CHAPTER 1. INTRODUCTION

(FCF) than before. However, the typical drawback of CF is that the gain is constant and
always empirically given for one time and the inability to adapt the weight for another
quite different scenario, still can not be overcome completely. In contrast, KF based
methods are always taken as the common solution due to its guaranteed performance.
Therefore, this thesis mainly involves the theory of KF.
The KF is by far the most common one and statistically the optimal estimate for
linear system with independent white Gaussian noises. Especially only one iteration
is required for KF applications since the rate of convergence is higher than the angu-
lar speed in general. As a result of the guaranteed high performance, amounts of KF
based methods have been developed. In particular to adapt for nonlinear systems, such
methods like Extended Kalman filter (EKF) and the advanced SPKF, which includes
Unscented Kalman filter (UKF) [20], Central difference Kalman filter (CDKF) [21, 22]
and Cubature Kalman filter (CKF) [23], are developed in the later. For more discussion,
the introduction of KF is given in the Chapter 2, and the presentation and acceleration
of SPKF is explained in the Chapter 4.

1.1.7 Issues in practical applications

In recent, attitude estimation using MARG sensors becomes one of the promising
frontiers. On one hand, it has the characteristics of good wearability and low-cost. On
the other hand, it provides acceptable performance and not subject to the sites during
application. However, we are still facing challenges, and would like to discuss the
following three issues:

• For low-end devices, the resource are limited strictly, thus finding the lightweight
solution is absolutely imperative.

• For advanced Kalman filters based applications, how to achieve the performance
of being real-time has to be considered.

• In the real application of motion capture, where the distributed sensor nodes ob-
tain the orientation information of the object so as to reconstruct the motion tra-
jectory, as a result, time synchronization between the distributed sensor nodes
needs to be investigated. Besides, due to the sensors of the accelerometer, the
gyroscope, and the magnetometer works in an asynchronous measurement rate,
multi-rate sensor fusion also should be concerned.

1.2 Thesis Organization

The structure of this dissertation and the relationship of three main chapters are
illustrated in Fig. 1.4. This dissertation is organized in the following manner.

10
1.3. CONTRIBUTIONS

MARG based Motion Capture System

Chapter 1
Background

Chapter 4
Motion trackers and
built motion capture
system
Chapter 2
Lightweight Extended Kalman Fiter for MARG Sensors
Orientation Estimation

Chapter 4
Wireless Time Synchronized
Motion Trackers Using Kalman
Filtering to Multi-rate Sensor Fusion

Chapter 3
Real-Time Attitude Estimation of Sigma-Point Kalman
Filter via Matrix Operation Accelerator Chapter 2 & 3
Atiitude estimation
algorithm and
Kalman Filter
algorithm accelerator

Chapter 5 Roll, Pitch, Yaw

Conclusion

Figure 1.4: Thesis Structure and the Relationship of Three Main Chapters

First of all, research background is introduced in Chapter 1, which mainly intro-

duce the fundamental conceptions of attitude estimation such as coordinate systems,
attitude representations and so on. In addition, the thesis organization and the main
contributions are also explained in this chapter. Secondly, the related work is described
in Chapter 2. In order to reduce the computational effort, Chapter 3 describes a novel
Lightweight Extended Kalman Filter (LEKF) and the details of filter design is pre-
sented, particular the simplified measurement model is derived. For the experiment,
the proposed filter is evaluated from two aspects; the accuracy of attitude estimation
for different application scenario and the computing cost. Further, aiming at accelerat-
ing the advanced KF, namely SPKF, a matrix operation accelerator executing on Field
Programmable Gate Arrays (FPGA) is designed as the solution in Chapter 4. This ac-
celerator guarantees the good reusability and is assessed on Zynq-7020. Considering
the time synchronization in an asynchronous sensor network, in Chapter 5, we study the
motion capture system and propose a novel wireless time synchronized motion trackers.
We first introduce the work principles of the trackers, including the scheme of time syn-
chronization. Then the KF to multi-rate sensor fusion is explained, which enables the
sensor working at an asynchronous measurement rate and takes full advantage of the
performance of the sensors. The experiments of time synchronization assessment, the
accuracy evaluation of attitude estimation, the motion tracking during human walking
are conducted to validate the feasibility of the proposed motion tracker. At last, Chapter
6 gives the concluding remarks of this dissertation.

1.3 Contributions
This dissertation is a report about three years of my research on the topics of motion
capture theory and application using KF for MARG sensor array. We start the research
from two ways; one is the attitude estimation theory; two is the investigation of the
motion capture system using MARG sensor array. The results are described in Chapter

11
 CHAPTER 1. INTRODUCTION

3, 4 and 5. The main contributions of each chapter are summarized as follows.

(1) Chapter 3. Lightweight extended Kalman filter for MARG sensors orienta-
tion estimation
In modern devices, most of devices is equipped with a navigation system that in-
volves the attitude estimation using inertial sensors, and power consumption has be-
come the significant evaluation metrics for better cruising ability, in which the resource
is strictly limited. As a result, this arise a key problem of seeking a low power con-
sumption and accurate algorithm for attitude estimation. To address such problem, this
chapter,

• A simplified measurement model is derived from TRAID method. As a result of

the simplified measurement model, up to half of parameters of Jacobian matrix
are constant and only a few amount of computation is required for other parame-
ters of Jacobian, which are similar and have the common factors.

• A quaternion-based LEKF is constructed with the structure of EKF.

• The accuracy of the proposed filter for the drone application and human motion
capture is evaluated with the golden truth provided by the optical system, namely
Vicon.

• The computational cost is evaluated with comparison to two existing algorithms;

Indirect Kalman filter (IKF) and Fast Kalman filter (FKF).

(2) Chapter 4. Real-time attitude estimation of sigma-point Kalman filter via

matrix operation accelerator
With the increasing demands for the accuracy and robustness, SPKF is becoming
the promising solution. However, the higher accuracy and robustness are at the cost of
higher computational efforts. To deal with this, this chapter,

• The general structure of SPKF is summarized and the attitude estimation filter
based on SPKF is designed.

• The matrix operation accelerator is proposed on FPGA. This accelerator signif-

icantly improved the re-usability for matrix operation based algorithm and suc-
ceeds in accelerating SPKF.

• Evaluation of the performance of the designed accelerator is conducted on Zynq-

7020 from two aspects; performance of the proposed accelerator and the speedup
ratio for SPKF.

(3) Chapter 4. Wireless time synchronized motion trackers using kalman fil-
tering to multi-rate sensor fusion

12
1.4. PUBLICATIONS

For existing MARG sensor array based capture systems, time synchronization be-
tween trackers is normally ignored. Although some commercial devices such [24] pro-
vide accurate hardware solution, the hardware solution usually raise the cost signifi-
cantly and exceed the actual needs of the precision. therefore, in this paper,

• Wireless time synchronized motion trackers are presented. Time synchronization

for built motion capture system using the designed trackers are solved by the
consistency of the gyroscopes under the same condition. This method is actually
a software-based method and cost saving.

• A quaternion-based KF to multi-rate sensor fusion is proposed, which enables the

sensor working at an synchronous measurement rate and takes full advantage of
the performance of the sensors.

• To verify the feasibility of the proposed motion trackers for motion tracking,
firstly time synchronization is assessed, then the accuracy of the attitude esti-
mation is evaluated with the reference measurements provided by Vicon, at the
last the built motion capture system is validated during human walking and the
results are compared with results extracted from the optical system.

1.4 Publications
The following papers have been published or submitted to major journals and con-
ference.
(1) Major journal paper

1. Zeyang Dai, and Lei Jing. Lightweight Extended Kalman Filter for MARG Sen-
sors Attitude Estimation. IEEE Sensors Journal, 2021.
DOI: 10.1109/JSEN.2021.3072887.

2. Linjun Zhao, Chuanhua Su, Zeyang Dai, Huakun Huang, Shuxue Ding, Xinyi
Huang and Zhaoyang Han. (2019). Indoor device-free passive localization with
DCNN for location-based services. The Journal of Supercomputing, 1-18.
DOI: 10.1007/s11227-019-03110-2.

(2) Major conference paper

1. Zeyang Dai, and Lei Jing. Real-Time Attitude Estimation of Sigma-Point Kalman
Filter via Matrix Operation Accelerator. 2019 IEEE 13th International Sym-
posium on Embedded Multicore/Many-core Systems-on-Chip (MCSoC), Singa-
pore, Singapore, 2019, pp. 342-346.
DOI: 10.1109/MCSoC.2019.00055.

13
 CHAPTER 1. INTRODUCTION

2. Zeyang Dai, Chenghong Lu and Lei Jing. Time Drift Compensation Method on
Multiple Wireless Motion Capture Nodes. In 2020 13th International Conference
on Human System Interaction (HSI) (pp. 266-271). IEEE.
DOI: 10.1109/HSI49210.2020.9142648.

3. Lei Jing, Zeyang Dai and Yiming Zhou. Wearable Handwriting Recognition with
an Inertial Sensor on a Finger Nail. 2017 14th IAPR International Conference on
Document Analysis and Recognition (ICDAR), Kyoto, 2017, pp. 1330-1337.
DOI: 10.1109/ICDAR.2017.219.

4. Yiming Zhou, Zeyang Dai and Lei Jing. A Controlled Experiment Between Two
Methods on Ten-Digits Air Writing. 2016 IEEE International Conference on
Computer and Information Technology (CIT), Nadi, 2016, pp. 299-302.
DOI: 10.1109/CIT.2016.112.

14
Chapter 2

Related Work

In this section, two related works are described; first to introduce the basic theory
of KF, and second to introduce the method of noise identification for MARG sensors.

2.1 Kalman Filter

KF actually is a recursive solution to the discrete-data linear filtering problem and
was proposed in [25] in 1960 by R.E. Kalman. To date, there are many materials that
can be found to friendly introduce the general idea of KF, such as [26–28]. This section
will briefly introduce the theory of KF.
KF addresses the general problem of estimating the state xk ∈ Rn of a linear
discrete-time system, which is governed by the linear process model and the measure-
ment model, that is
xk = Ak xk−1 + Bµk + ωk
(2.1)
zk = Hk xk + υk
where the n × n matrix Ak relates the state at the time step k − 1 to the next state at step
k, the n × l matrix B is the control input model which is applied to the l × n control
vector µk , while the m × n matrix Hk is the observation model and the m × n vector
zk is the observation the the true state xk . The n × n vector ωk and m × n vector υk
are the process noise and measurement noise respectively, and they are assumed to be
independent and obey the Gaussian distribution with covariance Q and R respectively,
as the following probability distributions indicated

p(ω) ∼ N (0, Q)
(2.2)
p(υ) ∼ N (0, R)

With the symbol xˆ−k ∈ R to express the a priori state estimate at step k, and xˆk ∈ R
n n

to express the a posteriori state estimate at step k, the a priori and a posteriori errors

15
 CHAPTER 2. RELATED WORK

can be defined to be
e− −
k = xk − x̂k
(2.3)
ek = xk − x̂k
Then the a priori estimate error covariance is
h i
−T
Pk− =E e−
k ek (2.4)

and the a posteriori estimate error covariance is

h i
Pk = E ek ek T (2.5)

Considering
x̂k = x̂− −
k + Kk (zk − Hk x̂k ) (2.6)

where this formula is to compute an a posteriori state estimate x̂k as a linear combi-
nation of an a priori estimate x̂−
k and a weighted difference between an actual mea-
surement zk and a measurement prediction Hk x̂− −
k , and the difference (zk − Hk x̂k ) is
called the measurement innovation or the residual. The n × m matrix K is the gain
for minimizing the a posteriori error covariance. Substituting the above equations, the
resulting K is given by

Kk = Pk− HkT (Hk Pk− HkT + Rk )−1 (2.7)

Then we will get the well-known KF formulas and the summarized structure is graphi-
cally presented in Fig. 2.1.

Predict 
− − −1
= ( + )

−
̂  = ̂  +

− −
̂  = ̂  + ( − ̂  )

−
= +

−
= ( − )

Correct

Figure 2.1: Structure of Kalman Filter

As described above, KF is to deal with the problem of estimating the state xk ∈ Rn

of a linear discrete-time system, when the process model or the measurement model is

16
2.2. NOISE IDENTIFICATION

non linear, that is,

xk = f (xk−1 , µk ) + W ωk
(2.8)
zk = h(xk ) + V υk
This problem is solved by the well-known EKF [29,30], which linearizes the estimation
around the current estimate using the partial derivatives of the process and measurement
functions to compute estimates. Actually this method is akin to a Taylor series. The
structure of EKF is exactly same with KF, the only thing we have to do is figure out
the state transition matrix Ak , observation matrix Hk , the Jacobian matrix of partial
derivatives of f (xk−1 with respect to ω, the Jacobian matrix of partial derivatives of
h(xk ) with respect to υ, they are defined by

∂f  ∂h 
Ak =  , Hk =  (2.9)
∂x x̂k−1 ∂x x̂−k

and
∂f  ∂h 
W =  , V =  (2.10)
∂ω x̂k−1 ∂υ x̂−k
As for other advanced KFs, which aims at compensating the errors caused by the
large linearity of the system, are not introduced here.

2.2 Noise Identification

When designing the KF, the configuration using correct noise characteristics is con-
ductive to achieve the optimal performance of KF. Although the noise characteristics of
the sensor are specified in the corresponding product datasheet, it is necessary to learn
how to identify the noise. On one hand, sometime the specified parameters are not prac-
tical for low-cost devices. On the other hand, understanding the noise identification is
beneficial for us to tune the parameters of KF.

Figure 2.2: Sample Plot of Square-Root Allan Variance Analysis Results [1]

17
 CHAPTER 2. RELATED WORK

For MARG sensors, there are five basic noise terms [31], they are quantization
noise, the most concerned angle random walk, bias instability, rate random walk, and
rate ramp. For estimation of MARG sensors’ stochastic errors, several methods have
been devised, that is, adaptive Kalman filtering, Power spectral density (PSD), auto-
correlation function and Allan variance [32]. Among them, Allan variance [1, 33, 34]
is the simplest one, which is a method of representing the root mean square random-
drift errors as a function of averaging times. Assume that we have N measurements
y = {y1 , y2 , ..., yN } with sample time τ0 . The cluster time with length m is expressed
as τm = mτ0 . Thus, Allan variance is expressed as

M
X −1 h i2
1
σy2 τ = y j+1 − yj (2.11)
2(M − 1) j=1

where M denotes the number of cluster samples y k , and the average of cluster is defined
by
m
1 X
y k (m) = ym(k−1)+i 0 ≤ k ≤ M (2.12)
m i=1

where k indicates the kth cluster and the cluster time m is subject to m ≤ N/2. Then
we plot square-root of the results in log-log plot, it should look like the one shown in
Fig. 2.2. Different terms appear in different regions of τ . This allows us to identify
various random that exist in the measurements processes easily.

18
Chapter 3

Lightweight Extended Kalman Filter

for MARG Sensors Attitude
Estimation

3.1 Introduction
MARG sensors have been widely applied to our surrounding devices. These appli-
cations include the UAV [35, 36], human motion capture [37], wearable devices [38],
and so on. The MARG sensors are employed to compute the orientation of a rigid body
with respect to an inertial frame so as to precisely control the actuators or extract mo-
tion features. Normally, orientation parameters can be acquired from two approaches.
The first approach, related to the Inertial navigation system (INS), is to do integral of
angular rates from gyroscope over time if initial conditions are known. The second
approach is summarized as Wahba’s problem [8, 17] formed by at least two vector ob-
servations. As is known, in the case of low-cost MARG sensors, gyroscope measures
the angular velocity of a moving object. Accelerometer and magnetometer measure the
earth’s gravitational and magnetic fields respectively to provide an absolute reference
of orientation with the assumption that the measurements are not disturbed by any ex-
ternal noises. Their performance are always subject to their inherent drawbacks, for
example, drift error, external disturbance, and so on, that means we can not just rely on
any one to obtain accurate orientation estimation. For instance, the angular-rate inte-
gral will diverge very fast as time goes, and accelerometer’s output will be disturbed by
non-gravitational acceleration when the motion isn’t flat. There are too many uncertain-
ties inside almost every process of their sensing. Hence, a reliable attitude estimation
solution is to fuse them together using sensor fusion techniques.
Looking back on the past decades, amounts of approaches have been proposed for
sensor fusion. Roughly, we can classify them into two categories: complementary fil-

19
 CHAPTER 3. LIGHTWEIGHT EXTENDED KALMAN FILTER FOR MARG SENSORS
ATTITUDE ESTIMATION

ter [39–41] and Kalman filter [42–44] based methods. Complementary filters obtain a
more accurate orientation estimation by compensating for each sensor in the frequency
domain, while Kalman filters adopt a probabilistic determination of the state modeled as
a Gaussian distribution given the system model. Early thoughts of complementary filter
based solutions are raised around 1990 by Foxlin who first applied MARG sensors to
monitor the attitude motion of human’s head [18], the strategy of which is that combines
the accelerometer and magnetometer as the Electronic compass (eCompass) [45], the
problem of which can be effectively solved by optimization algorithms like GDA [39],
Gauss newton algorithm (GNA) [46] and Levenberg-Marquadt algorithm (LMA) [47]
or solutions to Wahba’s problem for example TRIAD [48, 49], Fast optimal matrix al-
gorithm (FOMA) [10], QUEST [50] and ESOQ [13]. Based on this, Mahony in 2008
studied the attitude representation on spectial orthogonal groups by different way [51].
Madgwick computed the attitude using the GDA via quaternion representation [39].
Especially FCF [40] proposed by Wu was proved to be faster and more robust than pre-
vious. Although all the above solutions are the constant gain complementary filters, the
typical drawback of which is that the gain is always empirically given for one time and
the inability to adapt the weight for another quite different scenario when their accu-
racy is affected, it has reflected the development trend where lightweight solutions with
maintained accuracy are preferred when seeking the balance between the performance
and the computation complexity.
The Kalman filter is by far the most common one and statistically the optimal es-
timator for linear system with independent white Gaussian noises, generally one itera-
tion is sufficient for convergence. As a result of the excellent performance, considering
that the systems are normally nonlinear, and to apply Kalman filter to nonlinear sys-
tems, adaption and implementation of Kalman filter for nonlinear systems are signif-
icantly investigated in the past decades. Adapted methods basically contain EKF [?],
UKF [20], CDKF [21, 22], and CKF [23], where EKF is the most popular solution, and
the later three are the so called sigma-point Kalman filter with higher computations and
advanced performance. Same with complementary filters, Kalman filter is also often
considered for attitude estimation. Sabatini presented his work to obtain accurate ori-
entation estimation using EKF in [43]. To reveal higher accuracy, Marina proposed the
attitude estimation algorithm based on UKF [35]. Although performance is guaranteed,
these methods are heavy on computations, and resource constrained devices could not
afford for it. Similar with the development trend of complementary filters, the prob-
lem of attitude estimation using Kalman filter are revisited recently. Valenti presented
a linear Kalman filter (LKF) [44] using the algebraic quaternion algorithm. As well,
Guo proposed the fast Kalman filter (Guo’s FKF) [52] with their previous contribu-
tions, which has been proved to be better than LKF. These attempts exactly made a
major breakthrough on reducing computational cost. However, existing solutions are

20
3.2. ORIENTATION FROM OBSERVATION VECTORS

still not perfect. For example, too many linear approximations could lower the accu-
racy, and there may be alternative methods to further reduce the computational cost,
and maintain the accuracy.
Therefore, what motivates us is to develop a novel lightweight Kalman estimator to
reduce computational cost, and maintain the accuracy so as to further reduce power dis-
sipation for MARG sensors attitude estimation. In this paper, a simplified measurement
model is systematically derived to establish the LEKF with employing the quaternion
kinematic equation as the process model. This proposed measurement model simplifies
the involved computations, and is proved to make a contribution on reducing compu-
tation cost. During the experiments, a commercial sensor for data collection, and an
optical system to provide reference measurements of orientation, namely Vicon, are
utilized to investigate the performance of the established Kalman filter. Evaluation for
different application scenarios is considered, which primarily include human motion
capture and the drone applications. Results indicate that the proposed filter provides re-
liable performance for both applications. Moreover, the comparison experiment shows
that the proposed filter provides better performance in terms of either attitude estimation
accuracy or computational time.
This chapter is organized as follows. Section II involves the attitude determination
from the accelerometer and magnetometer. Section III describes the design and imple-
mentation of the proposed filter. Experimental environment, experiments, and results
are presented in Section IV. Section V gives the concluding remarks.

3.2 Orientation from Observation Vectors

This section deals with the orientation determination from the observers with the
assumption that the observers are not influenced by any external disturbance. Given
normalized observation vectors Ab = (ax , ay , az )T , M b = (mx , my , mz )T obtained
from accelerometer and magnetometer respectively in the body frame, the orientation
determination in the North-East-Down (NED) frame can be modeled by

Ab = CAr ,
(3.1)
M b = CM r ,

where C is theDCM, and Ar = (0, 0, 1)T , M r = (mN , 0, mD )T denote the reference

vectors of earth gravitational and magnetic fields respectively, while mD and mN in [40]
are computed by
mD = ax mx + ay my + az mz ,
q (3.2)
mN = 1 − m2D .

21
 CHAPTER 3. LIGHTWEIGHT EXTENDED KALMAN FILTER FOR MARG SENSORS
ATTITUDE ESTIMATION

As was discussed earlier, orientation determination from a set of vector observations

is called Wahba’s problem, there are amounts of solutions to this problem. Although
methods such as QUEST, FOMA, and ESOQ guarantee the performance, these methods
greatly increase the computational cost, and are commonly implemented for more than
two observation vectors’ systems. In our case, the sampling rate generally is synchro-
nized with the gyroscope where only two vectors are involved, the earliest and simplest
solution, TRIAD method, is sufficient, and taken into consideration, and we review the
formula:
C = Mo MrT ,
Mo = (o1 |o2 |o3 ) , (3.3)
Mr = (r1 |r2 |r3 ) ,
where the observation vectors oi and the reference vectors ri are computed by

o1 = W1 ,
o2 = W2 ,
o3 = W1 × W2 ,
(3.4)
r1 = V 1 ,
r2 = V 2 ,
r3 = V1 × V2 .



Directly, we assign (W1 , V1 ) to the pair Ab , Ar , and (W2 , V2 ) to the pair M b , M r .
Through derivation, the final result C is given by
 
C1,1 C1,2 C1,3
 
C = C2,1 C2,2 C2,3 
C3,1 C3,2 C3,3
 m −m a ay mz −az my
 (3.5)
x
N m
D x
mN
ax
 
=  my −m
m
D ay az mx −ax mz
mN
ay  ,
N
mz −mD az
mN
ax my − ay mx az

On another hand, considering the representation of elements of DCM in the form of

22
3.2. ORIENTATION FROM OBSERVATION VECTORS

quaternion,
C1,1 = q02 + q12 − q22 − q32
C1,2 = 2(q1 q2 + q0 q3 )
C1,3 = 2(q1 q3 − q0 q2 )
C2,1 = 2(q1 q2 − q0 q3 )
C2,2 = q02 − q12 + q22 − q32 (3.6)
C2,3 = 2(q0 q1 + q2 q3 )
C3,1 = 2(q0 q2 + q1 q3 )
C3,2 = 2(q2 q3 − q0 q1 )
C3,3 = q02 − q12 − q22 + q32
and the transformation relation between the quaternion and the Euler angles,

φ = atan2(2(q0 q1 + q2 q3 ), q02 − q12 − q22 + q32 )

= atan2(C2,3 , C3,3 )
θ = asin(2(q0 q2 − q3 q1 ))
(3.7)
= asin(−C1,3 )
ψ = atan2(2(q0 q3 + q1 q2 ), q02 + q12 − q22 − q32 )
= atan2(C1,2 , C1,1 )

where φ is the roll, θ for pitch, and ψ for yaw, respectively. Taking the constraint of
the quaternion representing the orientation into consideration, we can obtain that using
only four elements of the DCM are sufficient to extract the Euler angles, thereinto C1,3
and C2,3 are for determining the pitch and roll angles, and C1,1 and C1,2 are responsible
for the yaw angle. Although C3,3 as well can play the same role as C1,1 , C1,1 is preferred
because of the simpler calculations, as the following reformulated measurement model
indicates.    
C1,3 ax
   
C2,3   ay 
zk =    =  , (3.8)
C   a 
 3,3   z 
ay mz −az my
C1,2 mN

Conspicuously, the derived measurement model involves a few computations, on the

one hand, it is a low dimensional system, and easy to obtain the results given the
measurements from the accelerometer and magnetometer, on the other hand, it is later
proved that other involved computations are greatly reduced, particularly the compu-
tation of the Jacobian matrix of this model. What’s more, the proposed measurement
model is nonlinear, the good performance should be guaranteed in theory.

23
 CHAPTER 3. LIGHTWEIGHT EXTENDED KALMAN FILTER FOR MARG SENSORS
ATTITUDE ESTIMATION

Predict Steps
Correct the covariance

State prediction

Gyroscope Correct the estimated state

Accelerometer

Magnetometer

Covariance prediction

Kalman gain

Correct Steps

Figure 3.1: Workflow Diagram of the Proposed LEKF

3.3 Filter Design

This section investigates the establishment of the proposed LEKF, and we firstly
assume that the bias error of gyroscope are constant, and has been cancelled before
used. In order to formulate the system model, we employ the discretized quaternion
kinematic equation as the process mode, which is written as
 
∆T
qk = I4×4 + [Ω×] qk−1 , (3.9)
2
 T
where qk = q0,k q1,k q2,k q3,k defines the process state in quaternion form
at time k, I4×4 is the 4 × 4 identity matrix, and ∆T denotes the sampling interval
between epoch k and k − 1, while [Ω×] is determined with the gyroscope output
ω = (ωx , ωy , ωz )T by
 
0 −ωx −ωy −ωz
 
ωx 0 ωz −ωy 

[Ω×] =  . (3.10)
ω −ω 0 ω 
 y z x 

ωz ωy −ωx 0

By integrating with the derived measurement model, the Kalman filter model is formu-
lated by  
∆T
qk = I4×4 + [Ω×] qk−1 + wk ,
2 (3.11)
zk = Hk qk + υk ,

24
3.3. FILTER DESIGN

where wk and υk denote the progress noise and observation noise respectively, while
Hk is the observation matrice, we write down the derivation by
 
2q1,k q3,k + 2q2,k q0,k
 
 2q2,k q3,k − 2q1,k q0,k 

∂ 2 
2 2 2 
q
 0,k − q 1,k − q 2,k + q 3,k 
2q1,k q2,k − 2q3,k q0,k
Hk =
∂qk (3.12)
 
−q2,k q3,k −q0,k q1,k
 
 q1,k q0,k q3,k q2,k 
= 2
q
.
 0,k −q1,k −q2,k q3,k 

q3,k q2,k q1,k q0,k

As is known, the process model and measurement model can not be applied to
the covariance directly. Instead, the matrices of the Jacobian are computed, which are
actually the partial derivatives for both models. For the process model, the Jacobian
Ξk−1 is given by  
q1,k−1 q2,k−1 q3,k−1
 
−q0,k−1 q3,k−1 −q2,k−1 
Ξk−1 =  −q
.
 (3.13)
 3,k−1 −q 0,k−1 q 1,k−1 

q2,k−1 −q1,k−1 −q0,k−1

while the Jacobian J for the measurement model is derived by

∂zk
J= n o
∂ (Ab )T , (Mb )T
 ∂C1,3 ∂C1,3 ∂C1,3 ∂C1,3 ∂C1,3

∂C1,3
∂ax
 ∂C2,3
∂ay
∂C2,3
∂az
∂C2,3
∂mx
∂C2,3
∂my
∂C2,3
∂mz
∂C2,3 
(3.14)
 ∂a 
=  x ∂ay ∂az ∂mx ∂my ∂mz 
 ∂C3,3 ∂C3,3 ∂C3,3 ∂C3,3 ∂C3,3 ∂C3,3 
 ∂ax ∂ay ∂az ∂mx ∂my ∂mz 
∂C1,2 ∂C1,2 ∂C1,2 ∂C1,2 ∂C1,2 ∂C1,2
∂ax ∂ay ∂az ∂mx ∂my ∂mz

The detailed results are listed in the Appendix A. Henceforth, we can easily handle
the covariance Σwk for the process noise, and Συk for the observation noise, which are
respectively computed by
 2
∆T
Σwk = Ξk−1 Σgyro ΞTk−1 , (3.15)
2

and
Συk = J · diag [Σacc , Σmag ] · JT , (3.16)

where Σgyro = σg2 · I3×3 is the covariance of gyroscope, Σacc = σa2 · I3×3 for accelerom-

25
 CHAPTER 3. LIGHTWEIGHT EXTENDED KALMAN FILTER FOR MARG SENSORS
ATTITUDE ESTIMATION

eter, and Σmag = σm 2

· I3×3 for magnetometer, in general they can be obtained from the
device specification directly.
At this point, we have completely described the derivation of the proposed LEKF.
With the predict measurement z− k which is computed by

z− −
k = Hk qk (3.17)

the workflow is summarized in Fig. 3.1. Considering the constraints of the quaternion
representing the orientation and to ensure the filer working correctly, another thing that
should be noted, is that the last resulting quaternion must be normalized in every iter-
ation of Kalman filtering. Intuitively, LEKF is a lightweight Kalman filter, due to the
simplified measurement model, and that its involved computation is reduced, particu-
larly more than half elements of the Jacobian J are constant, and other parameters have
plenty of common factors.

Figure 3.2: Experimental Devices: WitMotion HWT905 and NanoPi Duo2

3.4 Experiments and Results

In this section, we conduct several experiments to investigate the performance of the
proposed LEKF. The detailed description about experimental setup and data collection
is given in the first subsection.

3.4.1 Experimental environment and data collection

The experimental data for testing the filters is collected with a commercial sen-
sor, namely WitMotion HWT905. HWT905 is a 10-axis orientation sensor with tem-
perature compensation which contains 16 bit 3-axis gyroscopes, accelerometers and

26
3.4. EXPERIMENTS AND RESULTS

magnetometers, and interfaced with a compact computer called NanoPi Duo2. The
hardware is shown in Fig. 3.2. In the following experiments, the raw data is collected
via the UART to USB data wire at 200Hz. With Zhang’s contribution in [7], his pro-
posed calibration method is employed to cancel the bias errors, including both of the
accelerometer and the gyroscope, while the scale factor error, as the equation 5 indi-
cates in [7], is not considerable in our case. Academically, the scale factor error also
should be concerned for the calibration, yet it’s experimentally proved to be extremely
small [53,54], and have few influence comparing with the bias errors, especially for the
sensor fusion of short time periods, and modern advanced integrated sensors. Regrad-
ing on the magnetometer, the frequently-used ellipsoid fitting algorithm is applied to
calibrate the magnetometer [55]. Through analyzing the obtained calibration parame-
ters, we found that the drone has a significant magnetic disturbance to the axis-z of the
magnetometer, including both of the soft- and hard-iron interference, it is predictable
that the angle of yaw could be subject to greater errors than other two of Euler angles.
For the filter setup, the sensor noise characteristics are obtained directly from the offi-
cial datasheet of HWT905, that is, Σgyro = 0.0025·I3×3 (◦ /s)2 , Σacc = 0.0001·I3×3 g 2 ,
and Σmag = 0.0004 · I3×3 uT 2 .

Figure 3.3: Experimental Platform

In order to provide the reference measurements, an optical motion capture system

called Vicon is utilized, which consists of 8 infrared cameras, and is able to track the
position of optical markers continually. To successfully extract the orientation parame-
ters, three optical markers are mounted on the top of the drone, while the experimental
devices are fixed on the bottom the drone, as shown in Fig. 3.3. The position of the
attached markers is measured with sampling rate of 200 Hz, and then we extract the
reference orientation parameters by post-processing. However, due to that both sys-
tems for data collection have different coordinate systems and clock resources, we have
to take two steps to align the coordinate systems and clocks of the two different sys-
tems. Firstly, we keep the drone stationary, obtain the corresponding orientation using

27
 CHAPTER 3. LIGHTWEIGHT EXTENDED KALMAN FILTER FOR MARG SENSORS
ATTITUDE ESTIMATION

an advanced Kalman filter, cubature Kalman filter [23], and then figure out the offset
between the two coordinate systems. Secondly, for each experimental data, we perform
a cross-correlation of both angular rates of the two systems to figure out the time drift,
as indicated in Fig. 3.4, where both angular rates have been aligned to the same coor-
dinate system. In theory, the error of time synchronization is less than one sampling
interval.

1 Vicon
MARG
rad/s

1
36.5 36.505 36.51 36.515 36.52
1 Vicon
MARG
rad/s

1
36.5 36.505 36.51 36.515 36.52
Time(s)

Figure 3.4: Unsynchronized and Synchronized Angular Rates

For the experiments, evaluation for different application scenarios is considered,

which primarily includes human motion capture and the drone application. Both are
the most common application scenarios almost covering all implementations in daily
life, human motion capture applications usually behave very stochastically, thus haven’t
fixed characteristics in frequency domain, and demand more requirements on response
accuracy, while the drone applications go along with relative regular high-frequency
vibration and the magnetic disturbance, it might be the acid test of the robustness. For
the assessment, no matter which application scenario, the performance evaluation will
be reflected by the accuracy. In the case of human motion capture, the data is acquired
from the motion randomly generated by moving the drone manually. For the drone
application, the data is recorded during the real flight. All the experimental data are
post-processed using python 3.7 on an Ubuntu 18.04 server with a CPU of 4-core i7-
h iT
7700K. q0 = 1 0 0 0 and P0 = 0.001 · I4×4 are set as the initial conditions for
the proposed filter.

3.4.2 Attitude estimation evaluation

Table. 3.1 shows the results of all the trials, where the upper three reflects the abso-
lute accuracy for human motion capture, while the below three are the evaluation results
of the drone during the real flight. For each trial, the attitude is calculated using LEKF
proposed in the previous section, and the root-mean-square error (RMSE) is computed
to measure the deference between the estimated Euler angles and the reference provided

28
3.4. EXPERIMENTS AND RESULTS

Table 3.1: RMSE [in deg] of Attitude Estimation

Trials Roll Pitch Yaw

1 1.28 1.60 3.01
2 1.16 0.95 2.87
3 0.82 0.90 2.82
4 1.27 1.05 6.37
5 1.13 0.94 4.04
6 1.48 2.01 8.41

2
Magnetometer (Gauss) Gyroscope (rad/s) Accelerometer (G)

X
1 Y
Z
0
1
2 0.0 20.0 40.0 60.0 80.0
X
1 Y
0 Z
1
2
0.0 20.0 40.0 60.0 80.0
1 X
Y
0 Z
1
0.0 20.0 40.0 60.0 80.0
Time(s)

Figure 3.5: Raw Data of Trial1

200
175
150
125 Z
100
75
50
25
65
60
55
30 40 50 Y
50 60 45
X 70 80 40

Figure 3.6: Consequent Movement of Trial1, Units: cm

by the optical system. Directly, although the movement range between different trials
is different that leads to the fluctuating evaluation results, we can see that the estimated
roll and pitch has relative stable performance for both application scenarios, whereas

29
 CHAPTER 3. LIGHTWEIGHT EXTENDED KALMAN FILTER FOR MARG SENSORS
ATTITUDE ESTIMATION

the estimated yaw performs relatively poorly for human motion capture, and dramati-
cally under the condition of drone applications. We would explain the results separately
in detail in the following.
For more details about the attitude estimation for human motion capture applica-
tions, we show the raw data of the Trial1 in Fig. 3.5, the corresponding estimated Euler
angles in Fig. 3.7, and the consequent moving trajectory in Fig. 3.6. Especially, Fig.
3.6 intuitively reveals the process of the movement, in which we pick up the sensor from
the red point, then stochastically wave the sensor in the air, and finally put the sensor
back to the blue position. In this case, the estimated Euler angles coincide well with the
reference, except that the estimated yaw has a large initial error. Through our analysis,
as stated above, the large initial error is caused by the significant magnetic distortion of
the drone, even though the magnetometer’s readings have been calibrated using ellipse
fitting algorithm, it cannot be overcome completely. As well, the iron house, in which
the optical system is assembled, has some effects on the magnetometer. This is why the
estimated yaw performs a bit worse than the other two. In contrary, on the other hand,
this proves that the proposed LEKF has a good convergence performance.

20 Reference
Estimated
Roll (deg)

0
20
20 0.0 20.0 40.0 60.0 80.0
Reference
Estimated
Pitch (deg)

20
0.0 20.0 40.0 60.0 80.0
25 Reference
Estimated
Yaw (deg)

0
25
50
0.0 20.0 40.0 60.0 80.0
Time(s)

Figure 3.7: Estimated Euler Angles of Trial1

Similarly, in order to analyze the details about the attitude estimation for drone
applications, Fig. 3.5 and Fig. 3.7 depict the raw data and corresponding estimated
Euler angles of Trial4, respectively. For the large spikes of the raw data in the tail. it’s
attributed to the sudden landing of the drone. In this case, it can be recognized easily
that there is drastic vibration on the axis-z of the accelerometer, which is certainly
resulted by the flight of the drone. Even so, both of the roll and pitch still do the
estimation very well. However, the estimated yaw diverges from the reference greatly.
The reason is that the running motors of the drone, which always work with powerful

30
3.4. EXPERIMENTS AND RESULTS

pulse current, further disturbs the surrounding magnetic field, it’s very hard to deal with
such dynamic noises. By contrast, the estimated roll and pitch angles are immune to the
magnetic disturbance.

Magnetometer (Gauss) Gyroscope (rad/s) Accelerometer (G)

X
1 Y
Z
0
1
2 0.0 20.0 40.0 60.0 80.0
X
1 Y
0 Z
1
2
0.0 20.0 40.0 60.0 80.0
1 X
Y
0 Z
1
0.0 20.0 40.0 60.0 80.0
Time(s)

Figure 3.8: Raw data of Trial4

20 Reference
Estimated
Roll (deg)

0
20
20 0.0 20.0 40.0 60.0 80.0
Reference
Estimated
Pitch (deg)

20
0.0 20.0 40.0 60.0 80.0
25 Reference
Estimated
Yaw (deg)

0
25
50
0.0 20.0 40.0 60.0 80.0
Time(s)

Figure 3.9: Estimated Euler Angles of Trial4

Evidently, through the evaluation results, the proposed LEKF is shown to coincide
with the reference very well for both application scenarios. Although the performance
of the estimated yaw depends on the environmental factor of the magnetic field strongly,
the estimated roll and pitch are proved to be immune to the magnetic disturbance.

31
 CHAPTER 3. LIGHTWEIGHT EXTENDED KALMAN FILTER FOR MARG SENSORS
ATTITUDE ESTIMATION

3.4.3 Robustness analysis

To verify the robustness of the proposed LEKF, the simulation experiments have
been carried out. Normally, attitude estimation using MARG sensors has three factors
of influence to the performance, that is, large external acceleration, magnetic distur-
bance, and high-frequency vibration of the drone applications. Generally, large exter-
nal acceleration violates the assumption that the gravity is not distorted, and usually is
solved with the basic idea that decrease the confidence interval of the accelerometer.
However, in this article, the proposed LEKF does not involve the robust mechanism of
large external accelerations, readers are referred to [?] for more details.

650
Without Pollution
600 Polluted Slightly
Polluted Strongly
Amplititude (mGauss)

550

500

450

400
Start of the pollution End of the pollution
350
0.0 20.0 40.0 60.0 80.0 100.0
Time(s)

Figure 3.10: Norm of the Magnetic Field

When operating under the condition of the distorted magnetic field, the magnetome-
ter’s readings will be distorted heavily. To a certain degree, although the experiment of
attitude estimation evaluation has proved the fact that the estimated roll and pitch are
immune to the magnetic disturbance, to clearly learn the fact, a simulation is conducted
where we distort the magnetometer’s outputs manually. We collect a static data that is
considered as a clean set of data, all the estimated Euler angles of which should stay
in a straight line, and then manually pollute this data with both of soft- and hard-iron
disturbance, as drawn in Fig. 3.10. Fig. 3.11 shows the estimated Euler angles of
clean and polluted data for a comparison. Apparently, for the slight magnetic distor-
tion (lower than 50 mGauss), basically we can say that the estimated roll and pitch are
immune to the magnetic distortion. However, for the strong magnetic distortion (more
than 50 mGauss), it not only affects the yaw angle but also the roll and pitch. In this
case, a robust mechanism such as detecting the magnetic distortion with the norms of
the magnetometer’s readings can be leveraged to make all the Euler angles immune to
magnetic disturbance.
Apart from the usual applications, attitude estimation on the drone has a high de-
mand on the response in the presence of the vibration. As mentioned above, definitely
there exists vibrations in all the trials of the drone, the Fast Fourier Transformation
(FFT) analysis result, as shown in Fig. 3.12, also confirms it as fact, where an approxi-

32
3.4. EXPERIMENTS AND RESULTS

0.0
Without Pollution
0.5 Polluted Slightly

Roll (deg)
1.0 Polluted Strongly

1.5

2.70 0.0 20.0 40.0 60.0 80.0 100.0

Without Pollution
2.75 Polluted Slightly

Pitch (deg)
2.80 Polluted Strongly
2.85
2.90
5 0.0 20.0 40.0 60.0 80.0 100.0
Without Pollution
0 Polluted Slightly
Yaw (deg) Polluted Strongly
5
10
0.0 20.0 40.0 60.0 80.0 100.0
Time(s)

Figure 3.11: Estimated Euler Angles of Clean and Polluted Data

0.010

0.008

0.006
Amplititude

0.004

0.002

0.000
0 20 40 60 80 100
Frequency (Hz)

Figure 3.12: FFT Analysis Result of the Accelerometer‘s Axis-Z Values of Trial4

mate 90Hz vibration on the axis-z of the accelerometer’s outputs can be identified eas-
ily. We can see that, although the proposed LEKF does not have any robust mechanism
about the vibration, it’s experimentally proved to have a good anti-vibration ability.

3.4.4 Comparison with representative kalman filters

In this subsection, we also assess the results from the representative Kalman filter,
that is, the recent Guo’s FKF [52], and the basic UKF which defines the system model
as  
∆T
qk = I4×4 + [Ω×] qk−1 + wk ,
2
h iT
zk = Ab M b (3.18)
h iT
= R(q−1
k ) A r
M r + υk∗ .

where the superscript of υk∗ is utilized to distinguish with the proposed. Experimental
conditions of which are the exact same as the LEKF, for instance the covariance of the
filter, and the runtime environment. The experiment compares aspects of the precision
of attitude estimation, and the computational cost.

33
 CHAPTER 3. LIGHTWEIGHT EXTENDED KALMAN FILTER FOR MARG SENSORS
ATTITUDE ESTIMATION

Table 3.2: RMSE [in deg] of representative Kalman filters

RMSE
Trials Roll Pitch Yaw Method
1.60 1.68 3.22 Guo’s FKF
1
1.31 1.62 3.32 UKF
1.48 1.01 3.23 Guo’s FKF
2
1.17 0.98 2.49 UKF
1.20 0.95 3.22 Guo’s FKF
3
0.81 0.91 2.50 UKF
1.72 1.15 5.44 Guo’s FKF
4
1.23 1.03 5.87 UKF
1.25 0.92 2.44 Guo’s FKF
5
1.15 0.97 4.09 UKF
1.97 2.04 7.96 Guo’s FKF
6
1.41 1.95 5.87 UKF

The overall statistical values of the attitude estimation accuracy for all trials are
shown in Table. 3.2, and Fig. 3.13 draws the estimation details of Trial1. Combin-
ing with Table. 3.1, we can find out that the accuracy of the proposed LEKF and the
basic UKF has a tiny difference on the roll and pitch, on one hand, this is because
the linearized system model limits the performance, so that the UKF cannot play to its
strength, particularly for large nonlinear systems, on the other hand, this means that the
proposed LEKF can provide excellent performance for such applications, as same as
the UKF, although the EKF has first-order approximated the calculation progress. Also,
we can see that, the proposed LEKF outperforms Guo’s FKF on the estimation of roll
and pitch, regardless of which application scenarios. Regarding the yaw angle, there
are lots of uncertainties caused by the magnetic distortion, but in general, the proposed
LEKF is second only to the UKF for human motion capture, it as well demonstrates
that the UKF does well in challenging environment.
Computational cost is another important indicator of algorithm besides the accuracy.
Here, we compare the consumed time of the proposed LEKF and Guo’s FKF, whereas
the UKF is not considered, due to UKF is an advanced Kalman filter, and normally
takes much more computing time than EKF. The evaluation result is plotted in Fig.
3.14. Distinctly, the proposed LEKF is slightly faster than Guo’s FKF.

34
3.5. SUMMARY

20
Reference
Proposed

Roll (deg)
0 Guo's FKF
UKF
20
10 0.0
Reference

Pitch (deg)
0 Proposed
Guo's FKF
10 UKF

0.0
20 Reference
Yaw (deg)

Proposed
0 Guo's FKF
UKF
20
0.0
Time(s)

Figure 3.13: Estimated Euler Angles of Compared Filters

Proposed
0.24 Guo's FKF
Comsumed Time( 10 3 s)

0.22

0.20

0.18

0.16

0 2500 5000 7500 10000 12500 15000

Samples

Figure 3.14: Consumed Time of the Proposed LEKF and Guo’s FKF

3.5 Summary
Attitude estimation for MARG sensors is revisited in this work. With employing the
quaternion kinematic equation as the process mode, we derive a simplified measurement
model to establish the proposed LEKF. Our analysis prove that the proposed measure-
ment model is conductive to reduce computation effort, this measurement model works
efficiently, and simplifies the involved computation, especially the computation of its
Jacobian where more than half elements are constant, and others have plenty of com-
mon factors.
Performance of the proposed LEKF are assessed using a commercial sensor for col-
lecting raw MARG sensors’ data and an optical system called Vicon to provide refer-
ence measurements. Evaluation for different application scenarios is considered, which
primarily includes human motion capture and the drone application. Results indicate
that the proposed LEKF performs very well, and is experimentally proved to have a

35
 CHAPTER 3. LIGHTWEIGHT EXTENDED KALMAN FILTER FOR MARG SENSORS
ATTITUDE ESTIMATION

good anti-vibration. What’s more, the estimated roll and pitch are immune to the rel-
atively small magnetic distortion. Finally, the comparison experiment shows that the
proposed LEKF can provide good performance, as same as the basic UKF, but it con-
sumes slightly less time than Guo’s FKF.

3.6 Appendix:Derivation of Jacobian Matrix

J1,1 = 1, J1,2 = 0, J1,3 = 0, J1,4 = 0,

J1,5 = 0, J1,6 = 0, J2,1 = 0, J2,2 = 1,
J2,3 = 0, J2,4 = 0, J2,5 = 0, J2,6 = 0,
J3,1 = 0, J3,2 = 0, J3,3 = 1, J3,4 = 0,
J3,5 = 0, J3,6 = 0,
− ∂m N
∂ax
(ay mz − az my )
J4,1 = ,
m2N
∂mN
mz mN − ∂ay
(ay mz − az my )
J4,2 = ,
m2N
−my mN − ∂m N
∂az
(ay mz − az my )
J4,3 = ,
m2N
− ∂m
∂mx
N
(ay mz − az my )
J4,4 = ,
m2N
−az mN − ∂m
∂my
N
(ay mz − az my )
J4,5 = ,
m2N
∂mN
ay mN − ∂mz
(ay mz − az my )
J4,6 = .
m2N

where

∂mD 1 ∂mD 1
= mx , = my ,
∂ax 2mD ∂ay 2mD
∂mD 1 ∂mD 1
= mz , = ax ,
∂az 2mD ∂mx 2mD
∂mD 1 ∂mD 1
= ay , = az ,
∂my 2mD ∂mz 2mD
∂mN mD ∂mD ∂mN mD ∂mD
=− , =− ,
∂ax mN ∂ax ∂ay mN ∂ay
∂mN mD ∂mD ∂mN mD ∂mD
=− , =− ,
∂az mN ∂az ∂mx mN ∂mx

36
3.6. APPENDIX:DERIVATION OF JACOBIAN MATRIX

∂mN mD ∂mD ∂mN mD ∂mD

=− , =− .
∂my mN ∂my ∂mz mN ∂mz

37
Chapter 4

Real-Time Attitude Estimation of

Sigma-Point Kalman Filter via Matrix
Operation Accelerator

4.1 Introduction
Low-cost MARG sensor array have been increasingly used to obtain precise attitude
information. For example, accurate knowledge of attitude is essential for flight control
in UAVs and mobile robotics navigation. Gyroscope measures the angular velocity of
a moving object. Accelerometer and magnetometer measure the earth’s gravitational
and magnetic fields respectively to provide an absolute reference of orientation. As
we known, these inexpensive sensors suffer from less accuracy and noisy due to white
Gaussian noise and environmental effects like temperature or mechanical vibration. A
lot of adaptive fusion techniques [39, 40, 42] have been studied in the past. In general
we employ the well-known EKF [43, 52, 56] as the common solution.
However, the increasing demands for accuracy and robustness make us face greater
challenges. The express companies are trying to use drones instead of people to de-
livery. More and more robots are being used for many tasks which are considered too
dangerous for people to do. The harsh environment will lead to the highly nonlinear
system. The EKF may not provide enough accuracy because of the limited first order
accuracy of the nonlinear functions. The SPKF [57–59] is potentially better solution.
The core of SPKF is the sigma-point approach, which propagates the sigma points
through the true non-linearity, without approximation, and then a weighted mean and
covariance is taken. This results in approximations that are accurate to the third order
(Taylor series expansion) for nonlinear Gaussian inputs and at least the second-order for
non-Gaussian inputs [60]. In addition, SPKF is free of the burdensome analytic deriva-
tion or Jacobians. Totally there are three kinds of SPKF, UKF [20], CDKF [21, 22]

38
4.2. SIGMA-POINT KALMAN FILTER

and CKF [23]. Generally we use square-root filters because ill-conditioned covariance
matrix may cause numerical instability. The only drawback is their higher computa-
tion cost than EKF. Typical embedded microprocessor can not afford for the real-time
implementation.
To speed up such filters, many approaches using FPGA [61–64] have been proposed
since matrix operations of these algorithms have high spatial and temporal parallelism.
These method can be roughly divided into two categories. One is to implement the
accelerator as the external co-processor. The other is to embed the accelerator in a soft-
core processor and use custom instructions for accessing. More or less, methods are
effective for acceleration compared with software implementation. However, it is easy
to become obsessed with the acceleration and forget the re-usability. This will result in
the waste of time for redesign, particularly when the design is quite complex.
Therefore, what motivates us is the re-usability. In this paper, an IP core called
matrix operation accelerator is proposed, which not only can be reused to accelerate
such matrix operation based filters, but also saves silicon. What’s more, we implement
the system on Zynq-7020 (XC7Z020-1CLG484) for experimental verification.
The organization of this chapter is as follows. First, we briefly introduce the algo-
rithm of SPKF and sigma-point approaches in section 4.2. Then Section 4.3 describes
the system architecture, including the filter design and the proposed matrix operation
accelerator. The implementation of the system and evaluation about the result are pre-
sented in Section 4.4.2. Finally the work is concluded in Section 4.5.

4.2 Sigma-Point Kalman Filter

In this section, we briefly introduce SPKF, including the structure and methods to
produce sigma points.

4.2.1 Filter structure

As with EKF, SPKF involves the estimation of the state of a discrete-time nonlinear
dynamic system,
xk = F(xk−1 , uk ) + vk
(4.1)
yk = H(xk ) + wk

where xk denotes the hidden state of the system, uk is the control input and yk repre-
sents the observed measurement at time k. The state transition function F and observa-
tion function H are both nonlinear. The process noise vk and the observation noise wk
are assumed to obey white Gaussian distribution, N (0, Qk ) and N (0, Rk ) respectively.
Furthermore, except the sigma-point approach, the framework of EKF and SPKF are

39
CHAPTER 4. REAL-TIME ATTITUDE ESTIMATION OF SIGMA-POINT KALMAN FILTER VIA
MATRIX OPERATION ACCELERATOR

the exact same. Consequently we can summarize the structure of SPKF (UKF, CDKF
and CKF) as,
Algorithm 1 Square-root sigma-point Kalman filter Structure

Initialize with
  
x̂0 = E [x0 ] S0 = chol E (x0 − x̂0 )(x0 − x̂0 )T (4.2)

For k ∈ {1, ..., ∞},

Sigma-point calculation

χk−1 = sigma-function(x̂k−1 ) (4.3)

Time update

χk|k−1 = F(χk−1 , uk−1 )


P2L ω (m) χ I
i=0 i i,k|k−1 ,
x̂− =
k
P2L ω (m) χ II
i=1 i i,k|k−1 ,
q 
− (c) −
Sk = qr ωi (χ1:2L,k|k−1 − x̂k ), SQ,k−1
 n o
cholupdate S− , χ0,k − x̂− , ω (c) , I (4.4)
k k 0
S− k =
S− , II
k

Yk|k−1 = H(χk|k−1 )

P2L ω (m) Y I
i i,k|k−1 ,
ŷk = Pi=0 (m)
−
 2L ω Y II
i=1 i i,k|k−1 ,

Measurement update
q 
(c) −
Sŷk = qr ωi (Y1:2L,k|k−1 − ŷk ), SR,k−1
 n o
cholupdate Sŷ , Y0,k|k−1 − ŷ− , ω (c) , I
k k 0
Sŷk =
S , II
ŷk
2L
X h ih iT
(c) − −
Pxk yk = ωi χi,k|k−1 − x̂k Yi,k|k−1 − ŷk
i=0 (4.5)
K= (Pxk yk /ST
ŷk )/Sŷk

x̂k = x̂− −
k + K(yk − yk )
q n o
ξ= (c)
ωi (χ1:2L,k|k−1 − x̂− −
k ) − K(Y1:2L,k|k−1 − ŷk )

cholupdate S− , KS , −1 , I
k ŷk
Sk =
qr {ξ, S II
R,k−1 } ,

40
4.2. SIGMA-POINT KALMAN FILTER

where I for UKF and CDKF, II for CKF

As mentioned above, we use square-root filters to improve the numerical stability

because all the covariance matrices are guaranteed to stay positive semi-definite. The
square-root and non square-root filters are algebraically equivalent.

4.2.2 Sigma-point approach

The sigma-point approach is the core of SPKF. They are all related through making
use of a technique called weighted statistical linear regressions to calculate the optimal
terms in the Kalman update step. The different sigma-point approaches define different
functions to produce sigma points.
In UKF, a set of sigma points is produced by the unscented transformation, which
is formulated as,


x̂k−1 , i=0

 √
χi,k−1 = x̂k−1 + L + λ(Sk )i , i = 1, ..., L (4.6)

 √

x̂
k−1 − L + λ(Sk )i , i = L + 1, ..., 2L

where L is the dimension of x and (Sk )i indicates the ith column of the matrix Sk .
Then we calculate the utilized weights by

(m) λ (c) λ
ω0 = ω0 = + (1 − α2 + β)
L+λ L+λ
1 (4.7)
(m) (c)
ωi = ωi = i = 1, ..., 2L
2(L + λ)

where λ = α2 (L + k) − L is a scaling parameter. k is the secondary scaling parameter.

α determines the spread of sigma points around xk and is usually set to a small positive
value (0 ≤ α ≤ 1). β incorporate prior knowledge of the distribution of xk . For
Gaussian distributions, β = 2 and k = 0 is optimal [65].
The CDKF expands the nonlinear function by using Stirling’s polynomial interpo-
lation formula. Similar to UKF, the (2L + 1) sigma points and weights are produced
by 

 x̂ , i=0

 k−1
χi,k−1 = x̂k−1 + h(Sk )i , i = 1, ..., L (4.8)



x̂ − h(S ) , i = L + 1, ..., 2L
k−1 k i

and
(m) h2 − L (m) 1
ω0 = ωi =
h 2h (4.9)
2
(c) 1 (m) h −1
ωi = 2 ωi = i = 1, ..., 2L
4h 4h2
41
CHAPTER 4. REAL-TIME ATTITUDE ESTIMATION OF SIGMA-POINT KALMAN FILTER VIA
MATRIX OPERATION ACCELERATOR

Comparing with UKF, it’s obvious that the CDKF uses only a single parameter, which
is easy to tune from a practical perspective. Another advantage over UKF is the CDKF
is more adaptive, they have the same performance though [22] [66].
The CKF employs a third-degree spherical-radical cubature rule to compute Gaussian-
weighted integrals numerically with the best performance of them. The sigma-point
approach is described by
 √
x̂ L(Sk )i , i = 1, ..., L
+
k−1
χi,k−1 = √
x̂
k−1 − L(Sk )i , i = L + 1, ..., 2L (4.10)
(m) (c) 1
ωi = ωi = , i = 1, ..., 2L
2L

Since CKF only requires 2L cubature points, CKF has better computational speed the-
oretically. It’s more convenient to be implemented without free parameters to tune.

4.3 System Architecture Overview

Overview of the system architecture is given in this section. At first, we deal with
the filter design, and then, we describe the details of the proposed matrix operation
accelerator.

4.3.1 Filter design

Figure 4.1: Framework of 9-axis AHRS

The general framework of 9-axis AHRS is shown in Fig. 4.1. We assume that biases
of gyroscope, accelerometer and magnetometer are constant and cancelled before used.
Moreover, quaternion is chosen as the attitude representation for solving gimbal lock
and the computation simplicity. With the sensed angular rate ω = (ωx , ωy , ωz )T , we can

42
4.3. SYSTEM ARCHITECTURE OVERVIEW

define the process model using the discretized quaternion kinematic equation,
 
∆t
qk = I4×4 + [Ω×] qk−1 (4.11)
2

where ∆t denotes the sampling interval and

 
0 −ωx −ωy −ωz
 
ωx 0 ωz −ω y 
[Ω×] = 
ω −ω
 (4.12)
 y z 0 ωx  
ωz ωy −ωx 0

With normalized observed vectors Ab = (ax , ay , az )T , Mb = (mx , my , mz )T from ac-

celerometer and magnetometer in body frame b, we formulate the measurement model
by, h iT h iT
yk = Ab Mb = Rot(qk ) Ar Mr (4.13)

where r indicates the reference frame and Rot(qk ) is the rotation matrix converted
from qk , while Ar and Mr in [40] are given by
h iT
Ar = 0 0 1
h iT
Mr = mN 0 mD
(4.14)
mD = ax mx + ay my + az mz
q
mN = 1 − m2D

4.3.2 Matrix operation accelerator

Through analyzing the structure of SPKF, apparently it’s a matrix operation based
algorithm. These matrix operations include such like matrix inversion, matrix multipli-
cation and qr decomposition and etc. Furthermore, the matrix operations are sequen-
tial, it’s not necessary to synthese the whole algorithm to digital circuit, which is a very
costly waste of silicon. As so, we design the matrix operation accelerator instead. It
not only saves silicon but also is reusable to adapt for different matrix operation based
algorithms. Fig. 4.2 shows the block of the matrix operation accelerator.
This block is able to manipulate the memory directly and works like Direct memory
access (DMA). There are several registers in control unit, which is used to configure the
matrix operation unit. Once we configure these registers by accessing the interface ctrl,
this accelerator will automatically finish the matrix operation and move the resulting
matrix to the destination memory area. Then, it will trig irq_done signal to inform
other processing unit.

43
CHAPTER 4. REAL-TIME ATTITUDE ESTIMATION OF SIGMA-POINT KALMAN FILTER VIA
MATRIX OPERATION ACCELERATOR

Figure 4.2: Block of the Matrix Operation Accelerator

4.4 Implementation and Evaluation

In this section, we realize the matrix operation accelerator and evaluate the imple-
mentation on Zynq-7020 (XC7Z020-1CLG484). We first introduce the performance
metrics. Then we discuss the experiment setup and evaluation results.

4.4.1 Performance metrics

We introduce one metric called speedup of the execution to evaluate the perfor-
mance of the accelerator. The speedup of the execution is used to evaluate the improve-
ment of the execution. Adapting the formula of Amdahl’s law [67] to our system, the
speedup of the execution can be obtained by

Speedup = tsw /thw (4.15)

where tsw is the execution time of the software implementation and thw is the execution
time of the accelerated implementation.

Figure 4.3: Architecture of the Experiment Platform

44
4.4. IMPLEMENTATION AND EVALUATION

4.4.2 Experiment setup

The experiment is conducted on Zynq-7020 (XC7Z020-1CLG484), which is a mixed
CPU-FPGA platform. Fig. 4.3 shows the whole architecture adapting for our evalua-
tion. This device combines Programmable logic (PL) with Processing system (PS). PS
is a dual-core ARM Cortex-A9 processing system and each of them is equipped with a
single and double precision Vector floating point unit (VFPU).
In this experiment, the accelerator is programmed in C and synthesized using Xil-
inx’s Vivado Design Suite for C-based implementation. To meet the requirement of
precise, all operations implement double floating point arithmetic. There are two ad-
vanced extensible interface (AXI) between ARM and the accelerator. One is 32-bit
general purpose AXI master port for configuring the accelerator, playing a role of ctrl.
The other is 64-bit AXI accelerator coherency port (ACP) slave port enabling coherent
accesses from PL to CPU memory space, as the mem_bus. Once configured, the ac-
celerator will automatically manipulate the memory and finish data computation. For
the evaluation, we configure the accelerator running at 100MHz and use single ARM
Cortex-A9 core with enabled VFPU configured at 667MHz for performance compari-
son. And we set the dimension of all matrices to 14 according to the requirement of the
implementation.

4.4.3 Performance of proposed accelerator

Totally six different matrix operations are synthesized into the PL. Table 4.1 shows
the synthesis results, including the number of used resource and the utilization of PL.
As seen from the table, only qr decomposition and cholupdate use a little more re-
sources than other operations, that is because this two operations involve more complex
computation. The last term of this table shows the total used resource of the accelerator.

Table 4.1: Resource Utilization of Matrix Operation Accelerator

Operation Name Number of resource utilization (%)

BRAM DSP FF LUT
Add/Sub 2(0.7) 3(1.4) 805(7.6) 1348(2.5)
Dot 0(0) 28(12.7) 5253(4.9) 4584(8.6)
Invert 4(1.4) 39(17.7) 3091(2.9) 4326(8.1)
QR 4(1.4) 28(12.7) 11652(11.0) 15238(28.6)
Cholupdate 0(0) 25(11.4) 10956(10.3) 13743(25.8)
Total 24(8.6) 67(30.5) 28549(26.8) 33554(63.1)

Table 4.2 shows the latency of the accelerator and speedup of the execution. The
speedup indicates the acceleration factor defined as the ratio of execution time of the
software implementation to the execution time with the accelerator. From the table, the

45
CHAPTER 4. REAL-TIME ATTITUDE ESTIMATION OF SIGMA-POINT KALMAN FILTER VIA
MATRIX OPERATION ACCELERATOR

matrix addition, subtraction and cholupdate is slower than the software implementation
when executing with the accelerator. The first two is because of the low computational
complexity. cholupdate is due to the sequential order of the calculations which can not
be paralleled.

Table 4.2: Calculation Cycles and Speedup of Matrix Operation

Operation Name Latency Interval Speedup

Min Max Min Max tsw /thw
Add/Sub 205 205 205 205 0.75
Dot 1406 1406 1406 1406 5.48
Invert 1124 9552 1124 9552 3.80
QR 3041 23971 3041 23971 1.65
Cholupdate 2482 2482 2482 2482 0.78

4.4.4 Performance of accelerated filters

In this part, we do the performance comparison of unaccelerated and accelerated
SPKF. The test data is stored in the Secure digital card (SD card) and accessed by the
Secure digital input output bus (SDIO bus). From the Table 4.3, it’s obvious that the
accelerator improves approximately 2 times the processing speed of SPKF.

Table 4.3: Execution Time of Square-Root SPKF in One Iteration

Filter software implementation (msec) with accelerator (msec) speedup

UKF 2.22 1.32 168%
CDKF 2.34 1.25 187%
CKF 2.47 1.10 225%
Average 2.34 1.22 192%

4.5 Summary
In this chapter, a matrix operation accelerator is proposed to accelerate SPKF for
real-time orientation estimation. These advanced filters have higher performance in
terms of accuracy and more robust than EKF. It’s quite essential for modern applica-
tions particularly in hash environment. The accelerator is programmed in C and synthe-
sized using Xilinx’s Vivado Design Suite. We conduct the experiment on Zynq-7020
(XC7Z020-1CLG484). The experimental results show that the accelerator is efficient
of the computation and saves silicon. With this, on one hand, it not only improves
the performance but also offloads the heavy computation from the CPU. On the other
hand, it’s reusable to speedup other matrix operation based algorithms, which will save

46
4.5. SUMMARY

time for redesign. Although some of computation of SPKF is executed in software and
can not be solved as the matrix operation, the proposed scheme is still a considerable
solution to the problem of algorithm acceleration.

47
Chapter 5

Wireless Time Synchronized Motion

Trackers Using Kalman Filtering to
Multi-rate Sensor Fusion

5.1 Introduction
As discussed before, Motion capture (MoCap) technology has become increasingly
popular and can be found everywhere in daily life. For example, in areas such as film
and game industry, medical science and gesture recognition. In the film and game
industry, the designers generally combine MoCap with virtual reality technology to
make objects extremely vivid and lifelike [68]. Well-known film "Avatar" and game
"Pokemon Go" have acquired a good reputation. In the filed of medical science, it’s
proved that MoCap is a good way of the monitoring of patients’ condition and help
them to obtain more efficient rehabilitation therapies [69, 70]. For gesture recognition,
it’s exactly an old research topic and improves users experience of human-computer
interaction [71, 72]. Anyway, all the applications need obtain the positions, speeds,
attitudes of the object by employing a certain MoCap technology.
As so far, there are many MoCap techniques that have been developed and only
two of them become the promising frontier, namely, optical sensor based and MARG
sensors based methods. Solutions such as mechanical based [73] and magnetic based
[74–76] are being retired due to either the inconvenience for wearing or the low per-
formance in terms of accuracy, while vision based method [77] is more susceptible to
background, like illumination, occlusion and other environmental factors. Optical sen-
sor based systems like Vicon [78] succeed in doing this and could be the first successful
application of MoCap, which enable users to move freely and have an excellent perfor-
mance on accuracy. Usually the users are required to wear infrared reflective markers
and the system determines the motion of the markers from the images recorded by the

48
5.1. INTRODUCTION

surrounding cameras. On the other hand, MARG based systems [79–81] extract the
information by integrating the measurements of the gyroscope with a given initial con-
dition or deriving from the two observed vectors referring to the global frame. However,
optical based methods are only available in structured environments and quite expen-
sive, it would be preferred for film shooting and other applications that require high
accuracy. In comparison, MARG based systems are a very promising frontier because
of its low price, low power consumption and small size, what’s more, they can be used
almost anywhere. In consequence, we focus on MARG based techniques for motion
capture in this paper.
As we known, due to the bottleneck of each component of MARG, the MARG
based systems have to employ the optimal sensor fusion algorithm to extract the accu-
rate attitude information. These algorithms can generally be divided into two categories
in rough; complementary filter CF based approaches and extended Kalman filter (EKF)
based approaches respectively. Comparing with EKF based approaches, CF based ap-
proaches obtain the attitude by compensating for each other in the frequency domain.
During the recent years, several advanced CF based methods have been developed.
In [39], Madgwick proposed a computationally efficient orientation algorithm using
the gradient descent algorithm. In 2016, Wu proposed a novel fast quaternion-based
estimator to further reduce the computing complexity in [40]. Although CF has more
efficient computations, a typical drawback is that the gain is constant and always empir-
ically given for one time and the inability to adapt the weight for another quite different
scenario. In contrast, EKF is always taken as the common solution due to the guaran-
teed high performance. In 2005, Trawny proposed an IKF [42] to solve the problem of
attitude estimation. This approach performs well on robustness and still provides a ref-
erence of the idea for the modern applications. To reduce the computational complexity,
Valenti presented a linear Kalman filter using the algebraic quaternion algorithm in [44],
and Guo established a more computational measurement model in his proposed FKF re-
spectively. These work proved that EKF is adaptive and has a stable performance on
accuracy. Therefore, we employ EKF as the solution of attitude estimation as well.
Until now, many motion capture systems have already been developed [24, 79, 81,
82], especially the famous commercial motion capture system Xsens MVN [24] has
been wildly used in the practical applications. Commercial systems, although being
easy-to-use and providing an excellent performance in terms of accuracy, strictly limit
the scalability of the sensor network because the sensor network is established using
their self-defined protocol. Moreover, the time synchronization of the asynchronous
network often has been ignored. Even the company Xsens handled the problem by the
hardware solution in the next generation product [82] and achieved high accuracy of
time synchronization, the hardware method is quite expensive and exceeds the actual
need. For the prototypes developed by the research center, they normally focused on re-

49
 CHAPTER 5. WIRELESS TIME SYNCHRONIZED MOTION TRACKERS USING KALMAN
FILTERING TO MULTI-RATE SENSOR FUSION

ducing the limitations such as power consumption, size and weight, it is rate to consider
the scalability and time synchronization of the sensor network of the motion capture
system.
Therefore, the novel wireless time synchronized motion tracker is proposed in this
chapter. The presentation will be described in three aspects: the working principles of
the motion tracker, the design of the attitude estimation algorithm and the validation of
the motion tracker.

5.2 Work Principles

In this section, the working principles of the proposed motion tracker are introduced
in detail, including the architecture of the tracker, the establishment of the motion cap-
ture system and segment kinematics. As well as the facing problems, time synchroniza-
tion of trackers in an asynchronous sensor network are investigated.

5.2.1 Motion tracker overview

Fig. 5.1 shows the prototype of the motion tracker, which is a small embedded sys-
tem with a package size of 47mm(W)×30mm(D)×13mm(H) (excluding the mounting
ear) and a weight of 26g. It is powered using a LiPo battery and lasts for more than
2h at least in the real test. Fig. 5.2 shows the diagram of the motion tracker. Each
tracker is composed of a microcontroller that has a built-in Wireless Fidelity (Wi-Fi)
connectivity and a low-cost inertial and magnetic measurement unit that consists of 3-
axis accelerometer, 3-axis gyroscope and 3-axis magnetometer. Gyroscope senses the
angular rate in three axis and provides an estimate of the orientation when integrated
with previous orientation state over time. Accelerometer and magnetometer measure
strength and direction of the reference field, gravity and magnetic field respectively.
In the context of sensor fusion, a quaternion-based EKF to multi-rate sensor fusion is
proposed as the reliable solution and eliminates the bottleneck of each single sensor.
This fusion algorithm enables the sensors working at an asynchronous measurement
rate and takes full advantage of the performance of all the sensors. In the course of
the tracker work, the microcontroller acquires the measurement from the sensing ele-
ments, process the data using our proposed EKF, and transfer the results out through the
wireless network. Besides, the Micro-USB interface driven by the terminal program is
provided to configure the working parameters of the tracker, such as the sampling rate,
the measurement rage and so on. The major configurable parameters are list in Table.
5.1.
For the program residing in the microcontroller, totally three tasks are created and
running on a mini real-time kernel FreeRTOS. The first task is designed to acquire the

50
5.2. WORK PRINCIPLES

(a) Hardware Top View (b) Hardware Bottom View

(c) Tracker Inside View (d) Tracker Prototype

Figure 5.1: Overview of the Motion Tracker Prototype

Table 5.1: Major configurable parameters of the tracker

Available Configurations Units

Full-Scale Range
Gyroscope ±250, ±500, ±1000, ±2000 ◦
/s
Accelerometer ±2, ±4, ±8, ±16 g
Magnetometer ±4800 µT
Others
Low Pass Filter 5, 10, 20, 41, 92, 184 Hz
Output Rate 0 ∼ 100 Hz

measurement from the motion sensor and estimate the attitude using our proposed EKF.
Further, the processed data is delivered to the second task, which actually is a client
program using Transmission Control Protocol/Internet Protocol (TCP/IP) and transfers
the results out. Last but not least, the third task runs a terminal program for human
computer interaction to configure the parameters of the motion tracker. These tasks
ensure that the motion sensing will be completed successfully.
On the whole, the proposed motion tracker is compact and easy in implementation.

5.2.2 Motion capture system

Except the motion trackers, we need a wireless router and a computer that is able to
run a server program of TCP/IP to establish the motion capture system. Evidently, the
scalability of the sensor network just depends on the performance of the router and the
computer. In other word, the scalability is not limited by the data transmission protocol

51
 CHAPTER 5. WIRELESS TIME SYNCHRONIZED MOTION TRACKERS USING KALMAN
FILTERING TO MULTI-RATE SENSOR FUSION

I2C
CC3200
MPU9250
With built-in Wi-Fi Antenna
connectivity
3.3V

UART
Regulator Micro-USB
USB to UART
4.2V
5V

Battery Charger

Figure 5.2: Block Diagram of the Motion Tracker

of the motion tracker and it doesn’t take much effort to remove or add a tracker node.
However, one coin has two sides. The built sensor network is asynchronous. This is to
say, the measurements are misaligned from different trackers, and we have to figure out
a solution for the time synchronization before conducting any motion tracking activities.
Note that the gyroscope will produce the same results when placed in the same
coordinate system, this property can be utilized to figure out the problem of time syn-
chronization. We assume that all the sensors hold the same characteristics particularly
the sampling interval. In the beginning of conducting the motion capture, we attach all
the required trackers to the same rigid segment and perform some random acts for a
while. We choose one of them as the benchmark, find other trackers to the benchmark
alignment and rotate all the angular rate to the coordinate system of the benchmark.
Theoretically all the results should be the almost same. With the symbol ji q to express
the rotation from the frame of the tracker i to the frame of the benchmark tracker j,
such rotation operation can be formulated by

j
ω = ji q ⊗ i ω ⊗ ji q∗ , i 6= j (5.1)

where ⊗ denotes the quaternion product and ∗ is the conjugate of the quaternion [3],
while i ω indicates the angular rate of the tracker i. In this way, choosing any one of
them as the benchmark, we can perform a cross-correlation to figure out the time offset.
In theory, the time synchronization error is lower than one sampling interval of the
gyroscope. The higher sampling rate is conducive to improve the time synchronization
accuracy.

52
5.2. WORK PRINCIPLES

Although the method for time synchronization adds the complexity of the usage in
some way, it is acceptable and easy to operate. In particular, this approach reduces the
cost and avoids the complicated hardware design.

5.2.3 Segment kinematics

Each subject body contains body segments which are linked by the joints. When
the trackers are attached to the subject’s body segments, segment kinematics actually
is the translation from the tracker kinematics to body segment kinematics using a bio-
mechanical model. In this way, the translation will be a two-step process. The first step
is to align the sensor with the segment, in other word, the system calibration. Usually,
similar to other researchers, the well-known T-pose and N-pose are considered as the
solution. This asks the subject to stand like that for a while, the translation from the
tracker to the body segment B S q will be determined by

B ∗
G
Bq =G
Sq ⊗ Sq (5.2)

where GB q is the known orientation of each segment in T-pose or P-pose. In fact, dif-
ferent applications require different calibration methods, more detailed information is
provided in [24, 83].

+1
+1
.
. i+1 +1
i

.
x
Figure 5.3: Schematic of Segment Kinematics

The second step is to figure out the way to update the position of the joint. Fig. 5.3
shows the simplified schematic of segment kinematics in the global two-dimensional
coordinate system. Evidently, the position G pi+1 is determined by the translation of
the position G pi and the rotation G
B qi+1 . B qi and B qi+1 are the known orientations and
G G

53
 CHAPTER 5. WIRELESS TIME SYNCHRONIZED MOTION TRACKERS USING KALMAN
FILTERING TO MULTI-RATE SENSOR FUSION

provided by the tracker i and i + 1 respectively. With the known position G pi and the
segment length S li+1 , the position G pi+1 for the segment i + 1 can be computed by

G ∗
G
pi+1 = G pi + G S
B qi+1 ⊗ li+1 ⊗ B qi+1 (5.3)

where we assume the position p0 of the joint origin is known. And the joint angle i+1
i q
in quaternion between the segment i and i + 1 can be extracted by

G ∗
i+1
i q =G
B qi ⊗ B qi+1 (5.4)

5.3 Attitude Estimation

We investigate the attitude estimation algorithm in this section. First of all, the
filter design is explained, which actually is based on the idea of the construction of the
extended Kalman filter. Following this, the filter setup is discussed, including the initial
condition configuration and the noise characteristic identification of MARG.

5.3.1 Filter design

h iT
In this filter, quaternion q = w, x, y, z is employed as the representation of the
orientation of the sensor, which effectively avoids the traditional problem of gimbal lock
and has higher computational efficiency than other representations like Euler angles and
rotation matrix. Gyroscope measures the angular rate ω = (ωx , ωy , ωz ), with an initial
known orientation, the attitude at time k can be computed by integrating over time:
 
∆T
G
S qk = I4×4 + [Ω×] GS qk−1 (5.5)
2

where I4×4 is the 4 × 4 identity matrix, ∆T denotes the sampling interval of the gyro-
scope and  
0 −ωx −ωy −ωz
 
ωx 0 ωz −ω y 
[Ω×] = ω −ω
 (5.6)
 y z 0 ωx  
ωz ωy −ωx 0

This quaternion kinematic equation is employed as the process model while G S qk is

viewed as the hidden state of the system. For the normalised observations in the sensor
frame S, S A = (ax , ay , az ), S M = (mx , my , mz ), from the accelerometer and magne-
tometer respectively, we model the observation process as
h iT h iT
yk = S
A S
M = R(G ∗
S qk ) ·
G
A G
M (5.7)

54
5.3. ATTITUDE ESTIMATION

S qk ) is the rotation matrix extracted from S qk using the equation 5.8,

where R(G ∗ G ∗

 
w 2 + x2 − y 2 − z 2 2xy − 2wz 2wy + 2xz
 
R(q) =  2wy + 2xy w 2 − x2 + y 2 − z 2 2yz − 2wx  (5.8)
2 2 2 2
2xz − 2wy 2yz + 2wx w −x −y +z

while G A and G M are the reference vectors and in [40] given by

h i
G
A= 0 0 1
h i
G
M = mN 0 mD
(5.9)
mD = ax mx + ay my + az mz
q
mN = 1 − m2D

Henceforth, the discrete-time nonlinear system can be rewritten as

 
∆T
qk = I4×4 + [Ω×] qk−1 + wk ,
2 (5.10)
h iT
yk = R(q∗k ) · G A G M + υk

where we use qk instead of G S qk for simplicity, while wk and υk are the process noise
and observation noise and assumed to obey white Gaussian distribution. Combing with
the structure of the extended Kalman filter, the covariance Qk is computed by
 2
∆T
Qk = Ξk−1 Σgyro ΞTk−1 , (5.11)
2

with the given covariance matrix Σgyro = σg2 I3×3 where σg denotes the deviation of
angular rate. The Ξk−1 is computed by
 
q1,k−1 q2,k−1 q3,k−1
 
−q0,k−1 q3,k−1 −q2,k−1 
Ξk−1 =
−q
.
 (5.12)
 3,k−1 −q 0,k−1 q 1,k−1 

q2,k−1 −q1,k−1 −q0,k−1

In contrast, the covariance Rk for the observation noise is simply equal to the diagonal
matrix of the covariance of the accelerometer and magnetometer.

Rk = diag(R1,k , R2,k ) (5.13)

55
 CHAPTER 5. WIRELESS TIME SYNCHRONIZED MOTION TRACKERS USING KALMAN
FILTERING TO MULTI-RATE SENSOR FUSION

where R1,k = σa2 I3×3 and R2,k = σm 2

I3×3 . As well, the observation matrices Hk is
defined to be the following Jacobians
!
H1,k
Hk = (5.14)
H2,k )

where  
−q−
y,k q −
z,k −q −
w,k q −
x,k
 − 
H1,k = 2  q−
x,k q −
w,k q −
z,k q y,k  (5.15)
− − −
qw,k −qx,k −qy,k q− z,k

and H2,k is defined by the formula

 
h0,0 h0,1 h0,2 h0,3
 
H2,k = 2 h1,0 h1,1 h1,2 h1,3  (5.16)
h2,0 h2,1 h2,2 h2,3

where
h0,0 = q− −
w,k mN − qy,k mD h0,1 = q− −
x,k mN + qz,k mD

h0,2 = q− −
y,k mN − qw,k mD h0,3 = −q− −
z,k mN − qx,k mD

h1,0 = −q− −
z,k mN + qx,k mD h1,1 = q− −
y,k mN + qw,k mD
(5.17)
h1,2 = q− −
x,k mN + qz,k mD h1,3 = q− −
w,k mN + qy,k mD

h2,0 = q− −
y,k mN + qw,k mD h2,1 = q− −
z,k mN − qx,k mD

h2,2 = q− −
w,k mN − qy,k mD h2,3 = q− −
x,k mN + qz,k mD

In general, the maximum fusion rate of the Kalman filter is limited by the lowest
sampling rate of the utilized sensors. For example, in modern sensors, gyroscope and
accelerometer have the same high sampling rate usually, while magnetometer outputs
the measurement in relative low rate. Such limitation is a waste of the performance
of the sensor that can senses the signal in high sampling rate, which degrades over-
all performance of the application. Thus, the designed filter adopts a multi-rate filter
architecture. With this architecture, this filter allows the sensors to work at an asyn-
chronous sample rate and takes full advantage of the performance of all the sensors.
The workflow of this filter is summarized as the following:

(1) Predict the state if the gyroscope is available: q−
k = I4×4 +
∆T
2
[Ω×] qk−1 .

(2) Covariance prediction: P−
k = Fk Pk−1 Fk + Qk with Fk = I4×4 +
T ∆T
2
[Ω×] .

(3) If the accelerometer and magnetometer are both available, execute the missions
of this block and move to step (5), if only accelerometer available, move to step
(4).

56
5.3. ATTITUDE ESTIMATION

h iT
i Observations: yk = S
A S
M .
ii Kalman gain: K = P− − T
k Hk (Hk Pk Hk + Rk ) .
T −1

iii Correct the state: qk = q− −

k + K(yk − yk ).

iv Update the covariance: Pk = (I4×4 − K)P−

k.

(4) Correct the quaternion with the readings from the accelerometer:
h iT
i Observations: yk = S A .
ii Kalman gain: K = P− − T
k H1,k (H1,k Pk H1,k + R1,k ) .
T −1

iii Correct the state: qk = q− −

k + K(yk − yk ).

iv Update the covariance: Pk = (I4×4 − K)P−

k.

(5) Update the state and covariance for the next turn and move to step (1): qk−1 = qk ,
Pk−1 = Pk

5.3.2 Filter setup

When deploying the designed filter to the tracker, the initial conditions are set to
h iT
q0 = 1 0 0 0 and P0 = 0.001 · I4×4 . When powering up, the tracker always
requires several minutes for warming up before conducting any motion capture activ-
ities. Also, to obtain better dynamic performance, the fusion of the gyroscope and
accelerometer are running at 200 Hz, while the magnetometer is fused at the rate of
100 Hz because of the limitation of the maximum sampling rate of the magnetometer.
The output rate of the estimated attitude is subjected to the settings of the timer of the
microcontroller.
In addition, correct configurations of the sensor noise characteristics are conduc-
tive to achieve optimum performance of the filter. Normally, the standard approach of
characterization of noise for MARG sensors is Allan variance and its square-root, Al-
lan deviation, which is introduced in [84]. It is defined as a function of inter sample
distance τ , where xn denotes the time series of phase observations with length N and
spaced by the measurement interval τ :

N
X −2
1
2
σ (τ ) = (xn+2 − 2xn+1 + xn )2 (5.18)
2(N − 2)τ 2 n=1

Several types of noise can be identified by fitting the computed Allan deviation and the
cluster time τ . In this paper, only the white noises, σg for the gyroscope, σa for the
accelerometer and σm for the magnetometer, are considered, which can be recognized
by observing the value at τ = 1 in the log-log plot of Allan deviation and the cluster
time τ . The analysis results will be described in the experiment section.

57
 CHAPTER 5. WIRELESS TIME SYNCHRONIZED MOTION TRACKERS USING KALMAN
FILTERING TO MULTI-RATE SENSOR FUSION

5.4 Experiments and Results

In this section, several experiments are conducted and the results are discussed.
Results of Allan variance are shown in the first subsection. The second subsection
evaluates the performance of the attitude estimation of the motion tracker by comparing
with the reference measurement provided by the optical system, namely Vicon. Thirdly,
the assessment of time synchronization of the motion trackers is carried out. At last,
with the synchronized motion trackers, the built motion capture system is validated
during human walking. All the experiments are carried out with 100 Hz sampling rate,
the measurement range of ±2 g for accelerometer and ±2000 ◦ /s for gyroscope and the
low pass filter is set to 98 Hz.

5.4.1 Results of Allan variance

To obtain the Allan variance, the tested tracker is placed on the foam board, which
plays the role of a filter to damp the vibrations from the surrounding environment.
Moreover, all the metal objects that could cause the distortion of the magnetic field are
cleared. We record the measurements over 4 hours.

Accelerometer (m/s2 )
Gyroscope (rad/s)
Magnetometer (Gauss)
10−2
σ(τ )

10−3

10−4

10−2 10−1 100 101 102 103 104

τ /s

Figure 5.4: Allan Deviation of the Tested Motion Tracker

Fig. 5.4 shows the analysis results of the Allan variances of the three sensors. Ac-
cording to this graph, the white noise of the three sensors can be easily identified and
Table. 5.2 lists the results. The results of noise identification will be utilized to setup
the attitude estimation filter.

58
5.4. EXPERIMENTS AND RESULTS

Table 5.2: Results of Noise Identification

Gyroscope Accelerometer Magnetometer

σ 0.000165 rad/s 0.007034 m/s2 0.000825 Gauss

5.4.2 Accuracy of the attitude estimation

In this section, we evaluate the performance of the attitude estimation with Vicon,
which is an optical system that consists of eight infrared cameras and is able to track
the position of optical markers continually. When attaching three non co-linear markers
to the rigid body, the orientation of which can be extracted from the three measured
positions and viewed as the reference orientation. Fig. 5.5 shows the experimental
device. We first keep the device stationary and align the two coordinate system of the
sensor and Vicon using cubature Kalman filter [23]. Further, the experimental data is
acquired from the random motion generated by moving the tacker manually. Similar
with the method to synchronize the trackers, time synchronization between the tracker
and Vicon is done by locating the maximum value of the correlation rate of the angular
rate from the two system.

Figure 5.5: TExperimental Device for Attitude Estimation Evaluation

Table. 5.3 shows the RMSE of three trials. Although the magnetometer is calibrated
using the ellipsoid fitting algorithm [55], the angle of yaw still behaves badly, there are
two probable factors that cause this; the first is taht the iron house, the experiment room
of Vicon, could disturb the magnetic field, and the second is that the magnetometer
could suffer from low performance. In practical, the magnetometer can be discarded by
increasing σm to infinity when it performs poorly. In contrast, angles of pitch and roll
have relative reliable performance.

59
 CHAPTER 5. WIRELESS TIME SYNCHRONIZED MOTION TRACKERS USING KALMAN
FILTERING TO MULTI-RATE SENSOR FUSION

Table 5.3: RMSE [in deg] for Attitude Estimation

Trials Roll Pitch Yaw

0 1.52 2.03 14.30
1 1.38 0.64 14.12
2 1.23 1.96 14.89

5.4.3 Time synchronization assessment

As mentioned before, time synchronization of the trackers is done by the consis-
tency of the gyroscopes under the same condition. Although there is a positive cor-
relation between the accuracy of time synchronization and the sampling rate of the
gyroscope, and the error of time synchronization should be less than or equal to one
sampling interval of the gyroscope theoretically, it’s still essential to investigate the
feasibility and the stability over time.
In this experiment, four trackers are mounted on the wooden rigid segment. We
choose the tracker 4 as the benchmark and collect the static data to find other track-
ers to the tracker 4 alignment. Then we move the experimental device randomly and
the progress lasts for one hour approximately. For data analysis, the vector length for
performing cross-correlation is investigated in order to obtain the accurate time offset,
which also determines how long for time synchronization. On the other hand, the sta-
bility of time synchronization over time is analyzed by comparing the calculated time
offset at different times.

Tracker2 - Tracker1
40 Tracker3 - Tracker1
Tracker4 - Tracker1

20
Time Offset (s)

−20

−40

5000 20000 35000 50000 65000 80000 95000

Vector Length

Figure 5.6: Vector Length Versus Calculated Time Offset

The analysis results are shown in Fig. 5.6 and Fig. 5.7. From Fig. 5.6, we can
conclude that the offset is stable when the vector length is larger than 40000. This is to
say, we need more than 6 minutes data for time synchronization at least. Although it
adds the complexity of the implementation of devices in some way, it may be acceptable
for application without strict time limitations. Fig. 5.7 shows that the time offset is
changed with the increasing of working time, this probably because the clock precision
of sensors is different. Even so, the changing of time offset can be ignored for short
time monitoring.

60
5.4. EXPERIMENTS AND RESULTS

1
Tracker1 - Tracker4
Tracker2 - Tracker4
0
Tracker3 - Tracker4

−1

Time Offset (s)

−2

−3

−4

−5
0.0 1000.0 2000.0 3000.0 4000.0
Time (s)

Figure 5.7: Working Time Versus Time Offset

5.4.4 Validation during walking

With synchronized trackers, the feasibility of the built motion capture system is
validated during walking in this experiment. The trackers are fixed on human body, as
Fig. 5.11 suggests, they are distributed on waist, thigh, calf and instep respectively. In
the same way, this experiment is conducted with Vicon and all the trackers are equipped
with three optical markers. We extract the joint angle from both of trackers’ data and
Vicon’s data and employ RMSE as the evaluation metrics. And, the magnetometer is
discarded by increasing σm due to the magnetic disturbance.

Vicon
Tracker
Roll (deg)

−50

0.0 10.0 20.0 30.0 40.0

50
Vicon
25 Tracker
Pitch (deg)

−25

0.0 10.0 20.0 30.0 40.0

50 Vicon
Tracker
Yaw (deg)

25

−25

0.0 10.0 20.0 30.0 40.0

Time (s)

Figure 5.8: Hip: Joint Angle between Waist and Thigh

Fig. 5.8, Fig. 5.9 and Fig. 5.10 depicts the analyzed results where the joint angle
is represented using euler angle for intuition. Evidently, there is a good consistency
between the tackers and Vicon, except the angle of yaw performed below other two.
In Fig. 5.10, as a result of our analysis, the anomaly in the beginning is caused by the

61
 CHAPTER 5. WIRELESS TIME SYNCHRONIZED MOTION TRACKERS USING KALMAN
FILTERING TO MULTI-RATE SENSOR FUSION

Vicon
100
Tracker

Roll (deg)
50

0
0.0 10.0 20.0 30.0 40.0

40 Vicon
Tracker
Pitch (deg)

20

−20
0.0 10.0 20.0 30.0 40.0
80
Vicon
60 Tracker
Yaw (deg)

40

20

0
0.0 10.0 20.0 30.0 40.0
Time (s)

Figure 5.9: Knee: Joint Angle between Thigh and Calf

50
Vicon
Tracker
Roll (deg)

−50

−100
0.0 10.0 20.0 30.0 40.0

−20 Vicon
Tracker
Pitch (deg)

−40

−60

0.0 10.0 20.0 30.0 40.0

Vicon
Tracker
Yaw (deg)

100

50

0.0 10.0 20.0 30.0 40.0

Time (s)

Figure 5.10: Ankle: Joint Angle between Calf and Foot

sudden lifting of the foot and this part is excluded when calculating RMSE. Table. 5.4
shows the calculated RMSE for two trials.
In this experiment, we investigate the feasibility of the built motion capture system
by evaluating the accuracy of estimated joint angles. Although the angle of yaw of
the trackers is without correction of the magnetometer and its inaccuracy could affect
the accuracy of the extracted joint angle, the experimental results still reflect the good
consistency between the proposed trackers and the optical system.

62
5.4. EXPERIMENTS AND RESULTS

(a) View from Back (b) View from Right

(c) Walking (d) Walking

Figure 5.11: Motion Tracking during Walking

Table 5.4: RMSE [in deg] of Joint Angles

RMSE
Trials Joint Angle Roll Pitch Yaw
Hip 7.99 2.68 9.18
0 Knee 0.72 2.80 5.63
Ankle 2.94 1.22 2.67
Hip 7.79 3.90 10.25
1 Knee 1.14 2.37 6.25
Ankle 2.52 2.21 5.09

63
 CHAPTER 5. WIRELESS TIME SYNCHRONIZED MOTION TRACKERS USING KALMAN
FILTERING TO MULTI-RATE SENSOR FUSION

5.5 Summary
In this chapter, novel wireless time synchronized motion trackers are presented.
First of all, we introduced the work principles of the proposed motion tracker. This part
explained the architecture of the motion tracker, the workflow of the residing program
and the establishment of motion capture system. The tracker is compact and easy to use.
In particular, the scalability of the sensor network is not limited and a low-cost and easy-
to-operate method is proposed for time synchronization of the trackers. And, segment
kinematics are briefly introduced, which includes the step for system calibration and
the update of the position of the joint. Then, quaternion-based EKF to multi-rate sensor
fusion is designed for the attitude estimation, this filter enables the sensors working at
an asynchronous measurement rate and takes full advantage of the performance of the
sensors. Moreover, the setup of the filter is described particularly the Allan variance is
introduced and utilized in order to achieve optimum performance.
With the proposed trackers, several experiments are carried out. At first, the white
noise of the sensors are identified using Allan variance and utilized to setup the pa-
rameters of the filter. Then the accuracy of the attitude estimation is assessed with the
reference measurements provided by Vicon. Results indicate that angles of pitch and
roll provides reliable performance, while the angle of yaw suffers from the magnetic
disturbance or the low performance of the magnetometer. Further, the feasibility and
stability of time synchronization is investigated. Results shows that, it needs more 6
minutes for time synchronization and is acceptable for application without strict time
limitations, and time synchronization is stable within a short time. Finally, to verify the
feasibility of the trackers for motion capture, we validate the established motion capture
system during human walking. Although the accuracy of the joint angle suffers from
the inaccuracy of the angle of yaw, the experiment results still reflect that there is a good
consistency between the proposed trackers and the optical system.

64
Chapter 6

Conclusion

In this chapter, we conclude the dissertation by summarizing the conclusions of

Chapter 3, 4 and 5.
In Chapter 3, we derive a simplified measurement model from TRIAD method and
establish the LEKF for MARG sensors attitude estimation. In this filter, the quaternion
kinematic equation is simply adopted as the process model, and the simplified mea-
surement model is utilized to process acceleration and magnetic field observations. In
consequence of the simplified observation model, up to half of parameters of Jacobian
matrix are constant and only a few amount of computation is necessary for other param-
eters of Jacobian, which are similar and have the common factors. This significantly
reduces the computational efforts.
Experiments are carried out with a commercial sensor for data collection and an op-
tical system to provide the reference angles. The performance of the proposed filter for
human motion capture and UAV applications are evaluated separately. Results indicate
that, for both application scenarios, the proposed filter performs well, particularly the
evaluation during the flight on an UAV shows that the proposed filter is robust. On the
other hand, the comparisons between the proposed filter and the representative filters
prove that the proposed filter is faster and its accuracy is improved in some way.
In Chapter 4, a matrix operation accelerator is proposed to accelerate SPKF for real-
time orientation estimation. These advanced filters have higher performance in terms
of accuracy and more robust than EKF. It’s quite essential for modern applications
particularly in hash environment. The accelerator is programmed in C and synthe-
sized using Xilinx’s Vivado Design Suite. We conduct the experiment on Zynq-7020
(XC7Z020-1CLG484). The experimental results show that the accelerator is efficient
of the computation and saves silicon. With this, on one hand, it not only improves
the performance but also offloads the heavy computation from the CPU. On the other
hand, it’s reusable to speedup other matrix operation based algorithms, which will save
time for redesign. Although some of computation of SPKF is executed in software and
can not be solved as the matrix operation, the proposed scheme is still a considerable

65
 CHAPTER 6. CONCLUSION

solution to the problem of algorithm acceleration.

In Chapter 5, novel wireless time synchronized motion trackers are presented. First
of all, we introduced the work principles of the proposed motion tracker. This part
explained the architecture of the motion tracker, the workflow of the residing program
and the establishment of motion capture system. The tracker is compact and easy to use.
In particular, the scalability of the sensor network is not limited and a low-cost and easy-
to-operate method is proposed for time synchronization of the trackers. And, segment
kinematics are briefly introduced, which includes the step for system calibration and
the update of the position of the joint. Then, quaternion-based EKF to multi-rate sensor
fusion is designed for the attitude estimation, this filter enables the sensors working at
an asynchronous measurement rate and takes full advantage of the performance of the
sensors. Moreover, the setup of the filter is described particularly the Allan variance is
introduced and utilized in order to achieve optimum performance.
With the proposed trackers, several experiments are carried out. At first, the white
noise of the sensors are identified using Allan variance and utilized to setup the pa-
rameters of the filter. Then the accuracy of the attitude estimation is assessed with the
reference measurements provided by Vicon. Results indicate that angles of pitch and
roll provides reliable performance, while the angle of yaw suffers from the magnetic
disturbance or the low performance of the magnetometer. Further, the feasibility and
stability of time synchronization is investigated. Results shows that, it needs more 6
minutes for time synchronization and is acceptable for application without strict time
limitations, and time synchronization is stable within a short time. Finally, to verify the
feasibility of the trackers for motion capture, we validate the established motion capture
system during human walking. Although the accuracy of the joint angle suffers from
the inaccuracy of the angle of yaw, the experiment results still reflect that there is a good
consistency between the proposed trackers and the optical system.

66
Acknowledgment

I wish to express my sincere appreciation to my supervisor, Prof. Lei Jing, for the
patient guidance, encouragement and advice he has provided throughout my time as his
student. I have been extremely lucky to have a supervisor who cared so much about my
life and work, who responded to my questions and queries so promptly and modestly,
and who found the opportunity of the internship for me and accompanied me for almost
five years.
I also would like to thank the constructive comments and suggestions from the re-
view committee members; Prof. Xiang Li, Prof. Kazuyoshi Mori and Prof. Keitaro
Naruse. Their professional opinions have significantly improved my dissertation. As
well, I sincerely thanks their time and attention on my research topic. Through the
defending of the dissertation, I learned a lot.
The assistance from laboratory secretary, Mrs. Hoshi; Dr. Huakun Huang, Mr.
Chenghong Lu and all other members of our laboratory is truly appreciated. Mrs. Hoshi
often helped me borrow the experiment room and cared my daily life in the Lab. Dr.
Huang always shared experience and considerations on research with me. And Mr. Lu
usually assisted me to complete the experiments. Without their support, this research
could not have reached its goal.
Finally, I wish to acknowledge the support and great love of my family, my girl-
friend, Huicong Yu; my father, Xinzhong Dai; and my grandfather, Jiayun Dai. They
kept me going on and this work would not have been possible without their input.
Sincerely thanks you all. Best wishes to you!

67
References

[1] I. Board, “Ieee standard specification format guide and test procedure for single-
axis interferometric fiber optic gyros,” IEEE Std, pp. 952–1997, 1998.

[2] Y.-B. Jia, “Quaternions and rotations,” Com S, vol. 477, no. 577, p. 15, 2008.

[3] B. Kenwright, “A beginners guide to dual-quaternions: what they are, how they
work, and how to use them for 3d character hierarchies,” 2012.

[4] H. Harrison, “Quaternions and rotation sequences: a primer with applications

to orbits, aerospace and virtual reality, jb kuipers, princeton university press, 41
william street, princeton, nj 08540, usa. 1999. 372pp. illustrated.£ 35.00. isbn 0-
691-05872-5.” The Aeronautical Journal, vol. 103, no. 1021, pp. 175–175, 1999.

[5] S. W. Shepperd, “Quaternion from rotation matrix,” Journal of Guidance and Con-
trol, vol. 1, no. 3, pp. 223–224, 1978.

[6] S. Sarabandi and F. Thomas, “Accurate computation of quaternions from rota-

tion matrices,” in International Symposium on Advances in Robot Kinematics.
Springer, 2018, pp. 39–46.

[7] Z.-Q. Zhang, “Two-step calibration methods for miniature inertial and magnetic
sensor units,” IEEE Transactions on Industrial Electronics, vol. 62, no. 6, pp.
3714–3723, 2014.

[8] G. Wahba, “A least squares estimate of satellite attitude,” SIAM review, vol. 7,
no. 3, pp. 409–409, 1965.

[9] J. R. Wertz, Spacecraft attitude determination and control. Springer Science &
Business Media, 2012, vol. 73.

[10] F. L. Markley, “Attitude determination using vector observations: A fast optimal

matrix algorithm,” 1993.

[11] J. Keat, “Analysis of least-squares attitude determination routine doaop,” Techni-

cal Report CSC/TM-77/6034, Comp. Sc. Corp, Tech. Rep., 1977.

68
REFERENCES

[12] M. D. Shuster and S. D. Oh, “Three-axis attitude determination from vector ob-
servations,” Journal of guidance and Control, vol. 4, no. 1, pp. 70–77, 1981.

[13] D. Mortari, “Esoq: A closed-form solution to the wahba problem,” Journal of the
Astronautical Sciences, vol. 45, no. 2, pp. 195–204, 1997.

[14] ——, “Second estimator of the optimal quaternion,” Journal of Guidance, Con-
trol, and Dynamics, vol. 23, no. 5, pp. 885–888, 2000.

[15] F. L. Markley and D. Mortari, “Quaternion attitude estimation using vector ob-
servations.” Journal of the Astronautical Sciences, vol. 48, no. 2, pp. 359–380,
2000.

[16] J. Wu, Z. Zhou, B. Gao, R. Li, Y. Cheng, and H. Fourati, “Fast linear quaternion
attitude estimator using vector observations,” IEEE Transactions on Automation
Science and Engineering, vol. 15, no. 1, pp. 307–319, 2017.

[17] F. L. Markley and J. L. Crassidis, Fundamentals of spacecraft attitude determina-

tion and control. Springer, 2014, vol. 33.

[18] E. Foxlin, “Inertial head-tracker sensor fusion by a complementary separate-bias

kalman filter,” 1996.

[19] S. Madgwick, “An efficient orientation filter for inertial and inertial/magnetic sen-
sor arrays,” Report x-io and University of Bristol (UK), vol. 25, pp. 113–118,
2010.

[20] S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,”

Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, 2004.

[21] M. NøRgaard, N. K. Poulsen, and O. Ravn, “New developments in state estimation

for nonlinear systems,” Automatica, vol. 36, no. 11, pp. 1627–1638, 2000.

[22] K. Ito, “Gaussian filter for nonlinear filtering problems,” in Proceedings of the
39th IEEE Conference on Decision and Control (Cat. No. 00CH37187), vol. 2.
IEEE, 2000, pp. 1218–1223.

[23] I. Arasaratnam and S. Haykin, “Cubature kalman filters,” IEEE Transactions on

automatic control, vol. 54, no. 6, pp. 1254–1269, 2009.

[24] D. Roetenberg, H. Luinge, and P. Slycke, “Xsens mvn: Full 6dof human motion
tracking using miniature inertial sensors,” Xsens Motion Technologies BV, Tech.
Rep, vol. 1, 2009.

[25] R. E. Kalman, “A new approach to linear filtering and prediction problems,” 1960.

69
 REFERENCES

[26] G. Welch, G. Bishop et al., “An introduction to the kalman filter,” 1995.

[27] G. Bishop, G. Welch et al., “An introduction to the kalman filter,” Proc of SIG-
GRAPH, Course, vol. 8, no. 27599-23175, p. 41, 2001.

[28] S. M. Kay, Fundamentals of statistical signal processing. Prentice Hall PTR,

1993.

[29] M. I. Ribeiro, “Kalman and extended kalman filters: Concept, derivation and prop-
erties,” Institute for Systems and Robotics, vol. 43, 2004.

[30] G. A. Terejanu et al., “Extended kalman filter tutorial,” University at Buffalo,

2008.

[31] A. Hussen and I. Jleta, “Low cost inertial sensors modeling using allan variance,”
International Journal of Computer, Electrical, Automation, Control and Informa-
tion Engineering, vol. 9, no. 5, pp. 1237–1242, 2015.

[32] M. Matejček and M. Šostronek, “Computation and evaluation allan variance re-
sults,” in 2016 New Trends in Signal Processing (NTSP). IEEE, 2016, pp. 1–9.

[33] R. J. Vaccaro and A. S. Zaki, “Statistical modeling of rate gyros,” IEEE Transac-
tions on Instrumentation and Measurement, vol. 61, no. 3, pp. 673–684, 2011.

[34] B. IEEE, “Ieee standard specification format guide and test procedure for single-
axis interferometric fiber optic gyros,” 1998.

[35] H. G. De Marina, F. J. Pereda, J. M. Giron-Sierra, and F. Espinosa, “Uav atti-

tude estimation using unscented kalman filter and triad,” IEEE Transactions on
Industrial Electronics, vol. 59, no. 11, pp. 4465–4474, 2011.

[36] H. G. de Marina, F. Espinosa, and C. Santos, “Adaptive uav attitude estimation

employing unscented kalman filter, foam and low-cost mems sensors,” Sensors,
vol. 12, no. 7, pp. 9566–9585, 2012.

[37] G. Bellusci, F. Dijkstra, and P. Slycke, “Xsens mtw: Miniature wireless inertial
motion tracker for highly accurate 3d kinematic applications,” Xsens Technologies,
2013.

[38] X. Meng, Z.-Q. Zhang, J.-K. Wu, W.-C. Wong, and H. Yu, “Self-contained pedes-
trian tracking during normal walking using an inertial/magnetic sensor module,”
IEEE Transactions on Biomedical Engineering, vol. 61, no. 3, pp. 892–899, 2013.

[39] S. O. Madgwick, A. J. Harrison, and R. Vaidyanathan, “Estimation of imu and

marg orientation using a gradient descent algorithm,” in 2011 IEEE international
conference on rehabilitation robotics. IEEE, 2011, pp. 1–7.

70
REFERENCES

[40] J. Wu, Z. Zhou, J. Chen, H. Fourati, and R. Li, “Fast complementary filter for
attitude estimation using low-cost marg sensors,” IEEE Sensors Journal, vol. 16,
no. 18, pp. 6997–7007, 2016.

[41] J. Wu, Z. Zhou, H. Fourati, R. Li, and M. Liu, “Generalized linear quaternion com-
plementary filter for attitude estimation from multisensor observations: An opti-
mization approach,” IEEE Transactions on Automation Science and Engineering,
2019.

[42] N. Trawny and S. I. Roumeliotis, “Indirect kalman filter for 3d attitude estima-
tion,” University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep, vol. 2, p.
2005, 2005.

[43] A. M. Sabatini, “Quaternion-based extended kalman filter for determining orien-

tation by inertial and magnetic sensing,” IEEE Transactions on Biomedical Engi-
neering, vol. 53, no. 7, pp. 1346–1356, 2006.

[44] R. G. Valenti, I. Dryanovski, and J. Xiao, “A linear kalman filter for marg orienta-
tion estimation using the algebraic quaternion algorithm,” IEEE Transactions on
Instrumentation and Measurement, vol. 65, no. 2, pp. 467–481, 2015.

[45] W. Li and J. Wang, “Magnetic sensors for navigation applications: an overview,”

The Journal of navigation, vol. 67, no. 2, pp. 263–275, 2014.

[46] J. L. Marins, X. 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 International Conference on Intelligent
Robots and Systems. Expanding the Societal Role of Robotics in the the Next Mil-
lennium (Cat. No. 01CH37180), vol. 4. IEEE, 2001, pp. 2003–2011.

[47] H. Fourati, N. Manamanni, L. Afilal, and Y. Handrich, “Complementary ob-

server for body segments motion capturing by inertial and magnetic sensors,”
IEEE/ASME transactions on Mechatronics, vol. 19, no. 1, pp. 149–157, 2012.

[48] H. D. Black, “A passive system for determining the attitude of a satellite,” AIAA
journal, vol. 2, no. 7, pp. 1350–1351, 1964.

[49] ——, “Early development of transit, the navy navigation satellite system,” Journal
of Guidance, Control, and Dynamics, vol. 13, no. 4, pp. 577–585, 1990.

[50] W.-Y. Loh and Y.-S. Shih, “Split selection methods for classification trees,” Sta-
tistica sinica, pp. 815–840, 1997.

71
 REFERENCES

[51] M. Euston, P. Coote, R. Mahony, J. Kim, and T. Hamel, “A complementary fil-

ter for attitude estimation of a fixed-wing uav,” in 2008 IEEE/RSJ International
Conference on Intelligent Robots and Systems. IEEE, 2008, pp. 340–345.

[52] S. Guo, J. Wu, Z. Wang, and J. Qian, “Novel marg-sensor orientation estimation
algorithm using fast kalman filter,” Journal of Sensors, vol. 2017, 2017.

[53] O. Särkkä, T. Nieminen, S. Suuriniemi, and L. Kettunen, “A multi-position cali-

bration method for consumer-grade accelerometers, gyroscopes, and magnetome-
ters to field conditions,” IEEE Sensors Journal, vol. 17, no. 11, pp. 3470–3481,
2017.

[54] Z. Yang, S. Yan, and B. Li, “Hybrid calibration method for three-axis gradiome-
ter,” IEEE Magnetics Letters, vol. 8, pp. 1–5, 2017.

[55] J. Fang, H. Sun, J. Cao, X. Zhang, and Y. Tao, “A novel calibration method of
magnetic compass based on ellipsoid fitting,” IEEE Transactions on Instrumenta-
tion and Measurement, vol. 60, no. 6, pp. 2053–2061, 2011.

[56] R. G. Valenti, I. Dryanovski, and J. Xiao, “A linear kalman filter for marg orienta-
tion estimation using the algebraic quaternion algorithm,” IEEE Transactions on
Instrumentation and Measurement, vol. 65, no. 2, pp. 467–481, 2016.

[57] J.-m. Duan, D. Liu, H.-x. Yu, and H. Shi, “An improved fastslam algorithm for
autonomous vehicle based on the strong tracking square root central difference
kalman filter,” in 2015 IEEE 18th International Conference on Intelligent Trans-
portation Systems. IEEE, 2015, pp. 693–698.

[58] D. Liu, J. Duan, and H. Shi, “A strong tracking square root central difference
fastslam for unmanned intelligent vehicle with adaptive partial systematic resam-
pling,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 11,
pp. 3110–3120, 2016.

[59] H. ANKIŞHAN, F. Ari, E. Ö. TARTAN, and A. G. PAKFİLİZ, “Square root cen-

tral difference-based fastslam approach improved by differential evolution,” Turk-
ish Journal of Electrical Engineering & Computer Sciences, vol. 24, no. 3, pp.
994–1013, 2016.

[60] S. J. Julier and J. K. Uhlmann, “New extension of the kalman filter to nonlinear
systems,” in Signal processing, sensor fusion, and target recognition VI, vol. 3068.
International Society for Optics and Photonics, 1997, pp. 182–194.

[61] V. Bonato, R. Peron, D. F. Wolf, J. A. de Holanda, E. Marques, and J. M. Cardoso,

“An fpga implementation for a kalman filter with application to mobile robotics,”

72
REFERENCES

in 2007 International Symposium on Industrial Embedded Systems. IEEE, 2007,

pp. 148–155.

[62] D. T. Tertei, J. Piat, and M. Devy, “Fpga design and implementation of a matrix
multiplier based accelerator for 3d ekf slam,” in 2014 International Conference on
ReConFigurable Computing and FPGAs (ReConFig14). IEEE, 2014, pp. 1–6.

[63] J. Soh and X. Wu, “An fpga-based unscented kalman filter for system-on-chip ap-
plications,” IEEE Transactions on Circuits and Systems II: Express Briefs, vol. 64,
no. 4, pp. 447–451, 2017.

[64] S. Mie, Y. Okuyama, Y. Sato, Y. Chan, N. K. Dang, and B. A. Abderazek,

“Real-time uav attitude heading reference system using extended kalman filter for
programmable soc,” in 2017 IEEE 11th International Symposium on Embedded
Multicore/Many-core Systems-on-Chip (MCSoC). IEEE, 2017, pp. 136–142.

[65] E. A. Wan and R. Van Der Merwe, “The unscented kalman filter,” Kalman filtering
and neural networks, pp. 221–280, 2001.

[66] R. Van Der Merwe et al., “Sigma-point kalman filters for probabilistic inference
in dynamic state-space models,” Ph.D. dissertation, OGI School of Science &
Engineering at OHSU, 2004.

[67] K. Papadimitriou, A. Dollas, and S. Hauck, “Performance of partial reconfigura-

tion in fpga systems: A survey and a cost model,” ACM Transactions on Recon-
figurable Technology and Systems (TRETS), vol. 4, no. 4, p. 36, 2011.

[68] C. Bregler, “Motion capture technology for entertainment [in the spotlight],” IEEE
Signal Processing Magazine, vol. 24, no. 6, pp. 160–158, 2007.

[69] D. Šimšík, J. Karchňák, B. Jobbágy, and A. Galajdová, “Design of inertial module

for rehabilitation device,” in 2013 IEEE 11th International Symposium on Applied
Machine Intelligence and Informatics (SAMI). IEEE, 2013, pp. 33–36.

[70] O. M. Giggins, K. T. Sweeney, and B. Caulfield, “Rehabilitation exercise assess-

ment using inertial sensors: a cross-sectional analytical study,” Journal of neuro-
engineering and rehabilitation, vol. 11, no. 1, p. 158, 2014.

[71] L. Jing, Y. Zhou, Z. Cheng, and T. Huang, “Magic ring: A finger-worn device for
multiple appliances control using static finger gestures,” Sensors, vol. 12, no. 5,
pp. 5775–5790, 2012.

[72] Y.-L. Hsu, C.-L. Chu, Y.-J. Tsai, and J.-S. Wang, “An inertial pen with dynamic
time warping recognizer for handwriting and gesture recognition,” IEEE Sensors
Journal, vol. 15, no. 1, pp. 154–163, 2014.

73
 REFERENCES

[73] X. Gu, Y. Zhang, W. Sun, Y. Bian, D. Zhou, and P. O. Kristensson, “Dexmo: An

inexpensive and lightweight mechanical exoskeleton for motion capture and force
feedback in vr,” in Proceedings of the 2016 CHI Conference on Human Factors in
Computing Systems. ACM, 2016, pp. 1991–1995.

[74] J. F. O’Brien, R. E. Bodenheimer Jr, G. J. Brostow, and J. K. Hodgins, “Automatic

joint parameter estimation from magnetic motion capture data,” Georgia Institute
of Technology, Tech. Rep., 1999.

[75] S. Yabukami, H. Kikuchi, M. Yamaguchi, K. Arai, K. Takahashi, A. Itagaki, and

N. Wako, “Motion capture system of magnetic markers using three-axial magnetic
field sensor,” IEEE transactions on magnetics, vol. 36, no. 5, pp. 3646–3648,
2000.

[76] D. Vlasic, R. Adelsberger, G. Vannucci, J. Barnwell, M. Gross, W. Matusik, and

J. Popović, “Practical motion capture in everyday surroundings,” in ACM transac-
tions on graphics (TOG), vol. 26, no. 3. Acm, 2007, p. 35.

[77] T. B. Moeslund and E. Granum, “A survey of computer vision-based human mo-

tion capture,” Computer vision and image understanding, vol. 81, no. 3, pp. 231–
268, 2001.

[78] P. Merriaux, Y. Dupuis, R. Boutteau, P. Vasseur, and X. Savatier, “A study of vicon

system positioning performance,” Sensors, vol. 17, no. 7, p. 1591, 2017.

[79] B. Fang, F. Sun, H. Liu, and D. Guo, “Development of a wearable device for
motion capturing based on magnetic and inertial measurement units,” Scientific
Programming, vol. 2017, 2017.

[80] A. Szcz˛esna, P. Skurowski, E. Lach, P. Pruszowski, D. P˛eszor, M. Paszkuta,

J. Słupik, K. Lebek, M. Janiak, A. Polański et al., “Inertial motion capture cos-
tume design study,” Sensors, vol. 17, no. 3, p. 612, 2017.

[81] P.-z. Chen, J. Li, M. Luo, and N.-h. Zhu, “Real-time human motion capture driven
by a wireless sensor network,” International Journal of Computer Games Tech-
nology, vol. 2015, p. 4, 2015.

[82] M. Paulich, M. Schepers, N. Rudigkeit, and G. Bellusci, “Xsens mtw awinda:

Miniature wireless inertial-magnetic motion tracker for highly accurate 3d kine-
matic applications,” Xsens: Enschede, The Netherlands, 2018.

[83] A. Filippeschi, N. Schmitz, M. Miezal, G. Bleser, E. Ruffaldi, and D. Stricker,

“Survey of motion tracking methods based on inertial sensors: A focus on upper
limb human motion,” Sensors, vol. 17, no. 6, p. 1257, 2017.

74
REFERENCES

[84] D. W. Allan, “Statistics of atomic frequency standards,” Proceedings of the IEEE,

vol. 54, no. 2, pp. 221–230, 1966.

75