Using Inertial Sensors For Position and Orientation Estimation
Using Inertial Sensors For Position and Orientation Estimation
E-mail: [email protected]
†
Xsens Technologies B.V., Enschede, the Netherlands
E-mail: [email protected]
‡
Department of Information Technology, Uppsala University, Sweden
E-mail: [email protected]
Abstract
In recent years, microelectromechanical system (MEMS) inertial sensors (3D accelerometers and
3D gyroscopes) have become widely available due to their small size and low cost. Inertial sensor
measurements are obtained at high sampling rates and can be integrated to obtain position and
orientation information. These estimates are accurate on a short time scale, but suffer from inte-
gration drift over longer time scales. To overcome this issue, inertial sensors are typically combined
with additional sensors and models. In this tutorial we focus on the signal processing aspects of
position and orientation estimation using inertial sensors. We discuss different modeling choices and
a selected number of important algorithms. The algorithms include optimization-based smoothing
and filtering as well as computationally cheaper extended Kalman filter and complementary filter
implementations. The quality of their estimates is illustrated using both experimental and simulated
data.
1 At the moment of publication Manon Kok worked as a Research Associate at the University of Cambridge, UK. A
major part of the work has been done while she was a PhD student at Linköping University, Sweden.
Contents
1 Introduction 3
1.1 Background and motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Using inertial sensors for position and orientation estimation . . . . . . . . . . . . . . . . 6
1.3 Tutorial content and its outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2 Inertial Sensors 9
2.1 Coordinate frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Angular velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3 Specific force . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.4 Sensor errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3 Probabilistic Models 14
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.2 Parametrizing orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2.1 Rotation matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2.2 Rotation vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2.3 Euler angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.2.4 Unit quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.3 Probabilistic orientation modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.3.1 Linearization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.3.2 Alternative methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.4 Measurement models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.4.1 Gyroscope measurement models . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.4.2 Accelerometer measurement models . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.4.3 Modeling additional information . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.5 Choosing the state and modeling its dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.6 Models for the prior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.7 Resulting probabilistic models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.7.1 Pose estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.7.2 Orientation estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
1
5 Calibration 59
5.1 Maximum a posteriori calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.2 Maximum likelihood calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.3 Orientation estimation with an unknown gyroscope bias . . . . . . . . . . . . . . . . . . . 61
5.4 Identifiability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
7 Concluding Remarks 70
A Orientation Parametrizations 75
A.1 Quaternion algebra . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
A.2 Conversions between different parametrizations . . . . . . . . . . . . . . . . . . . . . . . . 76
B Pose Estimation 78
B.1 Smoothing in an optimization framework . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
B.2 Filtering in an optimization framework . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
B.3 Extended Kalman filter with quaternion states . . . . . . . . . . . . . . . . . . . . . . . . 79
B.4 Extended Kalman filter with orientation deviation states . . . . . . . . . . . . . . . . . . . 79
2
Chapter 1
Introduction
In this tutorial, we discuss the topic of position and orientation estimation using inertial sensors. We
consider two separate problem formulations. The first is estimation of orientation only, while the other is
the combined estimation of both position and orientation. The latter is sometimes called pose estimation.
We start by providing a brief background and motivation in §1.1 and explain what inertial sensors are
and give a few concrete examples of relevant application areas of pose estimation using inertial sensors.
In §1.2, we subsequently discuss how inertial sensors can be used to provide position and orientation
information. Finally, in §1.3 we provide an overview of the contents of this tutorial as well as an outline
of subsequent chapters.
3
(a) Left bottom: an Xsens MTx IMU [156]. Left top: a
Trivisio Colibri Wireless IMU [148]. Right: a Samsung
Galaxy S4 mini smartphone.
creativecommons.org/licenses/by/2.0).
2 ‘WiiMote with MotionPlus’ by Asmodai available at https://commons.wikimedia.org/wiki/File:WiiMote_with_
4
(a) Back pain therapy using serious gaming. IMUs (b) Actor Seth MacFarlane wearing 17 IMUs to capture his
are placed on the chest-bone and on the pelvis to es- motion and animate the teddy bear Ted. The IMUs are placed
timate the movement of the upper body and pelvis. on different body segments and provide information about the
This movement is used to control a robot in the relative position and orientation of each of these segments.
game and promotes movements to reduce back pain.
Figure 1.2: Examples illustrating the use of multiple IMUs placed on the human body to estimate its
pose. Courtesy of Xsens Technologies.
(a) Inertial sensors are used in combination with GNSS mea- (b) Due to their small size and low weight, IMUs
surements to estimate the position of the cars in a challenge on can be used to estimate the orientation for control
cooperative and autonomous driving. of an unmanned helicopter.
Figure 1.3: Examples illustrating the use of a single IMU placed on a moving object to estimate its pose.
Courtesy of Xsens Technologies.
5
angular
Z
velocity orientation
external
specific ZZ
force remove acceleration position
rotate
gravity
Figure 1.4: Schematic illustration of dead-reckoning, where the accelerometer measurements (external
specific force) and the gyroscope measurements (angular velocity) are integrated to position and orien-
tation.
20
Integrated signal
10
−10
−20
0 20 40 60 80 100
Sample [#]
Figure 1.5: Integration of a white noise signal yt ∼ N (0, 1) for 50 noise realizations.
Example 1.1 (Integration drift) Let us first focus on the general case of measuring a quantity that
is constant and equal to zero. The integrated and double integrated signals are therefore also equal to
zero. However, let us now assume that we measure this quantity using a non-perfect sensor. In case our
measurements are corrupted by a constant bias, integration of these measurements will lead to a signal
which grows linearly with time. Double integration leads to a signal that instead grows quadratically with
time. If the sensor instead measures a zero-mean white noise signal, the expected value of the integrated
measurements would be zero, but the variance would grow with time. This is illustrated in Figure 1.5 for
the integration of a signal yt = et with et ∼ N (0, 1). Hence, integration drift is both due to integration
of a constant bias and due to integration of noise.
To illustrate integration drift using experimental data, a stationary data set is collected with a Sony
Xperia Z5 Compact smartphone using the app described in [57]. The smartphone contains accelerometers
and gyroscopes produced by Invensense [64]. We integrate the inertial measurements to obtain position
6
3
Orientation [◦ ]
2
0
0 2 4 6 8 10
Time [s]
(a) Integrated orientation for the position in x- (blue), y- (green) and z-
direction (red).
5
Position [m]
0 2 4 6 8 10
Time [s]
(b) Integrated position for rotation around the x-axis (blue), the y-axis
(green) and the z-axis (red).
Figure 1.6: Position and orientation estimates based on dead-reckoning of the inertial sensors only. The
data is collected with a Sony Xperia Z5 Compact smartphone that is lying stationary on a table.
and orientation estimates. Since the smartphone is kept stationary during the data collection, we expect
the position and orientation to remain the same. However, the orientation estimates drift a few degrees
over 10 seconds as shown in Figure 1.6(a). Note that the integration drift is not the same for all
axes. This is mainly due to a different sensor bias in the different axes. This will be studied further
in Example 2.3, where the same data set is used to study the sensor characteristics. As shown in
Figure 1.6(b), the position drifts several meters over 10s. The reason for this is two-fold. First, the
accelerometer measurements need to be integrated twice. Second, the orientation estimates need to be used
to subtract the gravity and any errors in this will result in leakage of gravity into the other components.
From the example above, it can be concluded that errors in the measurements have a large impact on
the quality of the estimated position and orientation using inertial sensors only. This is particularly the
case for position, which relies both on double integration of the acceleration and on accurate orientation
estimates to subtract the earth’s gravity. Because of this, inertial sensors need to be supplemented with
other sensors and other models to obtain accurate position and orientation estimates.
Inertial sensors provide pose estimates at high sampling rates which are accurate on a short time scale
but drift over longer time scales. They are therefore very suitable for being combined with sensors with
a lower sampling rate but with information that does not drift over time. For pose estimation, inertial
sensors are often combined with measurements from for instance a global navigation satellite system
(GNSS) [71, 147, 59], an ultrawideband (UWB) system [74, 132, 110, 27, 30] or cameras [26, 61, 79, 94].
For orientation estimation, they are often used in combination with magnetometers, which measure the
direction of the magnetic field [124, 120].
This tutorial aims at giving an introduction on how to use inertial sensors for position and orientation
estimation, but also on how to combine them with additional information. These additional sensors are
not the focus of this paper but simple models will be used for magnetometers and sensors providing
position information to illustrate the combined use of these sensors.
7
to be characterized. This is the topic of Chapter 2. Note that throughout the tutorial, we will focus
on MEMS inertial sensors and consider both data from standalone IMUs and from smartphones. This
implies that we do not focus on for instance mechanical or optical gyroscopes and on mechanical or
solid-state accelerometers [147]. These sensors may have characteristics that are quite different from the
MEMS inertial sensors considered here.
Based on the analysis of the sensors in Chapter 2 and on additional analysis of the application at
hand, models can be constructed. This is the topic of Chapter 3, where we will also discuss different
parametrizations of orientation. This will highlight the challenges in parametrizing and estimating
orientations and show that the orientation estimation problem is inherently nonlinear. Furthermore, we
will present two models that can be used for position and orientation estimation. The first is a model for
pose estimation using inertial measurements in combination with position measurements. The second is
a model for orientation estimation, using inertial and magnetometer measurements.
In Chapter 4, different algorithms for position and orientation estimation will be introduced. The
general structure of the algorithms will be discussed, after which explicit algorithms for orientation
estimation using inertial and magnetometer measurements are given. We will also discuss how the
algorithms can be extended to pose estimation when position measurements are available. Some general
characteristics of the two estimation problems will be given and the quality of the estimates from the
different algorithms will be analyzed. Which algorithm is most suitable for which application depends
strongly on the computational power that is available, the accuracy that is required and the characteristics
of the problem at hand.
In Chapter 4, we assume that the sensors are properly calibrated. However, calibration of the sensors
is important to for instance estimate the inertial sensor biases. Furthermore, calibration is specifically
of concern when combining inertial data with other sensors. In these cases, it is important that the
inertial sensor axes and the axes of the additional sensors are aligned. Sensor calibration is the topic of
Chapter 5. As an illustrative example, we will consider the estimation of an unknown gyroscope bias.
Our focus in this tutorial is to present models and algorithms that can be used for position and
orientation estimation using inertial measurements. Because of this, we assume fairly simple models
for the additional information — the magnetometer and position measurements. In Chapter 6, we will
discuss how the algorithms from Chapter 4 can be used in more complex settings. For example, we
will consider the cases of more complex position information in the form of images from a camera, the
presence of non-Gaussian measurement noise and the availability of additional information that can be
exploited. The information provided by the inertial sensors remains one of the main building blocks
of algorithms that can be used for these cases. Adaptations of the algorithms presented in Chapter 4
can therefore be used to also solve these more complex scenarios. We will end this tutorial with some
concluding remarks in Chapter 7.
8
Chapter 2
Inertial Sensors
To combine inertial measurements with additional sensors and models for position and orientation esti-
mation, it is important to accurately describe the quantities measured by the inertial sensors as well as
to characterize the typical sensor errors. This will be the topic of this chapter. It will serve as a basis
for the probabilistic models discussed in Chapter 3.
As discussed in Chapter 1, accelerometers and gyroscopes measure the specific force and the angular
velocity, respectively. In §2.2 and §2.3, we will discuss these quantities in more detail. To enable a
discussion about this, in §2.1 a number of coordinate frames and the transformations between them will
be discussed. We assume that we have 3D accelerometers and 3D gyroscopes, i.e. that the sensors have
three sensitive axes along which these physical quantities are measured. They are measured in terms of
an output voltage which is converted to a physical measurement based on calibration values obtained
in the factory. Even though the sensors are typically calibrated in the factory, (possibly time-varying)
errors can still remain. In §2.4, the most commonly occurring sensor errors are discussed.
The body frame b is the coordinate frame of the moving IMU. Its origin is located in the center of
the accelerometer triad and it is aligned to the casing. All the inertial measurements are resolved
in this frame.
The navigation frame n is a local geographic frame in which we want to navigate. In other words, we
are interested in the position and orientation of the b-frame with respect to this frame. For most
applications it is defined stationary with respect to the earth. However, in cases when the sensor
is expected to move over large distances, it is customary to move and rotate the n-frame along
the surface of the earth. The first definition is used throughout this tutorial, unless mentioned
explicitly.
The inertial frame i is a stationary frame. The IMU measures linear acceleration and angular velocity
with respect to this frame. Its origin is located at the center of the earth and its axes are aligned
with respect to the stars.
The earth frame e coincides with the i-frame, but rotates with the earth. That is, it has its origin at
the center of the earth and axes which are fixed with respect to the earth.
The n, i and e coordinate frames are illustrated in Figure 2.1. We use a superscript to indicate in which
coordinate frame a vector is expressed. Vectors can be rotated from one coordinate frame to another
using a rotation matrix. We use a double superscript to indicate from which coordinate frame to which
coordinate frame the rotation is defined. An illustration is given in Example 2.1.
Example 2.1 (Rotation of vectors to different coordinate frames) Consider a vector x expressed
in the body frame b. We denote this vector by xb . The rotation matrix Rnb rotates a vector from the
body frame b to the navigation frame n. Conversely, the rotation from navigation frame n to body frame
9
zi, ze
xn
zn
n
y
ye
xi
yi
xe
Figure 2.1: An illustration of three of the coordinate frames discussed in §2.1: the n-frame at a certain
location on the earth, the e-frame rotating with the earth and the i-frame.
b is denoted Rbn = (Rnb )T . Hence, the vector x expressed in the body frame (xb ) and expressed in the
navigation frame (xn ) are related according to
where Rbn is the rotation matrix from the navigation frame to the body frame. The earth rate, i.e.
the angular velocity of the earth frame with respect to the inertial frame is denoted by ωie . The earth
rotates around its own z-axis in 23.9345 hours with respect to the stars [101]. Hence, the earth rate is
approximately 7.29 · 10−5 rad/s.
In case the navigation frame is not defined stationary with respect to the earth, the angular velocity
ωen , i.e. the transport rate is non-zero. The angular velocity required for navigation purposes — in which
we are interested when determining the orientation of the body frame with respect to the navigation
frame — is denoted by ωnb .
where g denotes the gravity vector and anii denotes the linear acceleration of the sensor expressed in the
navigation frame, which is
The subscripts on the linear acceleration a are used to indicate in which frame the differentiation is
performed. For navigation purposes, we are interested in the position of the sensor in the navigation
frame pn and its derivatives as performed in the navigation frame
d n n d n n
dt p n = vn , dt v n = ann . (2.5)
A relation between aii and ann can be derived by using the relation between two rotating coordinate
frames. Given a vector x in a coordinate frame u,
d u d uv v uv d v u u
dt x u = dt R x u = R dt x v + ωuv × x , (2.6)
10
u
where ωuv is the angular velocity of the v-frame with respect to the u-frame, expressed in the u-frame. For
a derivation of this relation in the context of inertial navigation, see [59, 147]. For a general introduction,
see any textbook on dynamics, e.g. [92, 96].
Using the fact that
pi = Rie pe , (2.7)
where we have made use of (2.5), (2.6), and the fact that the angular velocity of the earth is constant,
d i
i.e. dt ωie = 0. Using the relation between the earth frame and the navigation frame
where nne is the distance from the origin of the earth coordinate frame to the origin of the navigation
coordinate frame, expressions similar to (2.8) can be derived. Note that in general it can not be assumed
d
that dt ωen = 0. Inserting the obtained expressions into (2.8), it is possible to derive the relation between
aii and ann . Instead of deriving these relations, we will assume that the navigation frame is fixed to the
earth frame, and hence Ren and nene are constant and
vee = dt
d e d en n
R p e = Ren dt d n
p n = vne ,
p e = dt (2.10a)
e d e d e e
aee = ve = vn = ann .
dt e dt n
(2.10b)
This is a reasonable assumption as long as the sensor does not travel over significant distances as compared
to the size of the earth and it will be one of the model assumptions that we will use in this tutorial.
More on the modeling choices will be discussed in Chapter 3.
Inserting (2.10) into (2.8) and rotating the result, it is possible to express anii in terms of annn as
Example 2.2 (Magnitude of centrifugal and Coriolis acceleration) The centrifugal acceleration
depends on the location on the earth. It is possible to get a feeling for its magnitude by considering the
property of the cross product stating that
n n
kωie × ωie × pn k2 ≤ kωie
n n
k2 kωie k2 kpn k2 . (2.12)
Since the magnitude of ωie is approximately 7.29 · 10−5 rad/s and the average radius of the earth is
6371 km [101], the magnitude of the centrifugal acceleration is less than or equal to 3.39 · 10−2 m/s2 .
The Coriolis acceleration depends on the speed of the sensor. Let us consider a person walking at a
speed of 5 km/h. In that case the magnitude of the Coriolis acceleration is approximately 2.03 · 10−4 m/s2 .
For a car traveling at 120 km/h, the magnitude of the Coriolis acceleration is instead 4.86 · 10−3 m/s2 .
11
0.02 10
ya,t [m/s2 ]
yω,t [rad/s]
0 5
−0.02 0
0 2 4 6 8 10 0 2 4 6 8 10
Time [s] Time [s]
(a) Gyroscope measurements yω,t which we ex- (b) Accelerometer measurements ya,t
pect to consist only of the earth’s angular veloc- which we expect to consist of the gravity
ity. vector, the centrifugal acceleration and
the Coriolis acceleration.
Figure 2.2: Inertial measurements for 10 seconds of stationary data. As can be seen, the measurements
are corrupted by noise and have a bias.
Count
Count
Count
−0.01 0 0.01 −0.01 0 0.01 −0.01 0 0.01
(yω,t )x (yω,t )y (yω,t )z
Figure 2.3: Histogram (blue) of the gyroscope measurements for 10 seconds of data from a stationary
sensor and a Gaussian fit (red) to the data. As can be seen, the measurement noise looks quite Gaussian.
Example 2.3 (Inertial sensor measurements and their errors) In Figures 2.2–2.4, gyroscope and
accelerometer measurements are displayed for around 10 seconds of stationary data collected with a Sony
Xperia Z5 Compact smartphone. Since the smartphone is stationary, the gyroscope is expected to only
measure the earth’s angular velocity. However, as can be seen in Figure 2.2(a), the gyroscope measure-
ments are corrupted by noise. As shown in Figure 2.3, this noise can be seen to be quite Gaussian.
Furthermore, the measurements can be seen to be biased.
During the stationary period, we would expect the accelerometer to measure the gravity, the centrifugal
acceleration and the Coriolis acceleration. Note that again the measurements are corrupted by noise,
which can be seen to be quite Gaussian in Figure 2.4. The x- and y-components of the accelerometer
measurements are not zero-mean. This can be due to the fact that the table on which the smartphone lies
is not completely flat, implying that part of the gravity vector is measured in these components. It can
also reflect a sensor bias. The z-component is actually larger than expected which indicates the presence
of an accelerometer bias at least in this axis.
Note that from the above discussion it can be concluded that it is more straightforward to determine
the gyroscope bias than it is to determine the accelerometer bias. To be able to estimate the gyroscope
bias, it is sufficient to leave the sensor stationary. For the accelerometer, however, it is in that case
difficult to distinguish between a bias and a table that is not completely flat. For more information about
accelerometer or general inertial sensor calibration, see [145, 106, 108].
The gyroscope in the smartphone is automatically recalibrated during stationary time periods. The
measurements shown in Figure 2.2(a) have not been corrected for this (so-called uncalibrated or raw
data). The reason why the gyroscope is calibrated during stationary periods, is because its bias is slowly
time-varying. As an example, let us consider a data set of 55 minutes. The gyroscope bias during the
T
first minute of the data set was 35.67 56.22 0.30 · 10−4 rad/s, while the gyroscope bias during the
T
last minute of the data set was 37.01 53.17 −1.57 · 10−4 rad/s.
The performance of IMUs is often specified in terms of their Allan variance [63, 33, 2]. The Allan
variance gives information about the sensor errors for stationary conditions, i.e. in a stable climate
without exciting the system. It studies the effect of averaging measurements for different cluster times
12
Count
Count
Count
0.03 0.08 0.13 0.04 0.09 0.14 9.8 9.85 9.9
(ya,t )x (ya,t )y (ya,t )z
Figure 2.4: Histogram (blue) of the accelerometer measurements for 10 seconds of data from a stationary
sensor and a Gaussian fit (red) to the data. As can be seen, the measurement noise looks quite Gaussian.
Note the different scales on the horizontal axis.
103 10−2
σA (Tc ) [m/s2 ]
σA (Tc ) [◦ /h]
102
10−3
101
10−2 10−1 100 101 102 103 10−2 10−1 100 101 102 103
Tc [s] Tc [s]
Figure 2.5: Left: Allan deviation for two gyroscopes. Right: Allan deviation for two accelerometers.
Reproduced with permission from [153].
Tc . Typically, the Allan standard deviation σA (Tc ) is plotted against the cluster time Tc as illustrated
in Figure 2.5. This figure shows the characteristic behavior of the Allan variance for inertial sensors.
To study it more in detail, we will discuss two components of the Allan variance that are typically of
concern for inertial sensors: the white noise and the bias instability.
Assume, as in Example 1.1, that we have a white noise signal with standard deviation σ. A longer
averaging time would for this signal lead to values closer to zero. The contribution to the Allan standard
deviation from the white noise component is given by σA (Tc ) = √σn where n is the number of samples
averaged over. This corresponds to a line with slope −1/2 in a log–log plot. For instance in the Allan
deviation for the gyroscope in Figure 2.5, the lines can be seen to have a slope of −1/2 until around
10−20 s, which indicates that the white noise is the dominating source of error for these short integration
times.
A constant bias does not have any effect on the Allan variance diagram. However, in case the bias
changes, longer averaging times will no longer be beneficial. Hence, the Allan variance diagrams in
Figure 2.5 show a deviation from the slope −1/2 for longer averaging times.
The Allan variance is a useful tool to study and compare the noise characteristics of inertial sensors.
However, it only considers stationary conditions. In dynamic conditions, a large number of other error
sources potentially come into play, see e.g. [147, 154]. These are for instance related to the fact that the
sensors sample at discrete times. Hence, to capture high-frequency signals, high sampling frequencies
are desired [128, 129]. Furthermore, large dynamics can lead to erroneous or saturated measurements.
Other errors that are not included are for instance changes in the sensitivity of the axes due to changes
in temperature. We should therefore never just rely on the Allan variance when deciding which sensor
to use in a particular application.
13
Chapter 3
Probabilistic Models
Pose estimation is about estimating the position and orientation of the body frame b in the navigation
frame n. This problem is illustrated in Figure 3.1, where the position and orientation of the body changes
from time t1 to time t2 . In this chapter, we will introduce the concept of probabilistic models and discuss
different modeling choices when using inertial sensors for pose estimation.
The subject of probabilistic modeling is introduced in §3.1. Most complexity in pose estimation lies
in the nonlinear nature of the orientation and the fact that orientation can be parametrized in different
ways. How to parametrize the orientation is therefore a crucial modeling choice in any pose estimation
algorithm. Because of this, we will discuss different parametrizations for the orientation in §3.2 and in
§3.3 we will discuss how these different parametrizations can be used in probabilistic modeling.
Our probabilistic models consist of three main components. First, in §3.4, we introduce models
describing the knowledge about the pose that can be inferred from the measurements. Second, in §3.5,
we model how the sensor pose changes over time. Finally, in §3.6, models of the initial pose are introduced.
The chapter will conclude with a discussion on the resulting probabilistic models in §3.7. The models
that will be used in the position and orientation estimation algorithms in Chapter 4 will be introduced
in this section.
3.1 Introduction
Probabilistic models constitute the foundation of the estimation algorithms in Chapter 4. In this section
we will introduce the concept of probabilistic modeling and the notation that is used in building our
models. Models are used to describe the information about the dynamics and the available measurements.
These models are subsequently used in combination with the measurements to infer some knowledge.
The knowledge that we are interested in is the pose of the sensor and we use information about the
sensor dynamics and the available measurements (amongst others, inertial measurements). A simplified
case where probabilistic modeling is used to estimate the position of a sensor is given in Example 3.1.
Example 3.1 (Probabilistic modeling) Let us estimate the 2D position pt of a sensor at time t from
t2
t1
b
Figure 3.1: An illustration of the pose estimation problem. We want to express the position and orien-
tation of the moving body frame b at times t1 and t2 with respect to the navigation frame n.
14
two position measurements
T T
yt1 = 0 0 , yt2 = 2 0 .
T
A straightforward suggestion for an estimate of the position would be p̂t = 1 0 . Let us now assume
that we know the accuracy of the sensors and represent this in terms of the following probabilistic models
We will not go into details about how this estimate is derived. Instead, we would like to point out two
differences between this position estimate and our initial suggestion p̂t . First, based on the knowledge
of the accuracy of the sensors, it is sensible to trust the measurement from the first sensor more than
the measurement from the second sensor. Our improved position estimate is therefore closer to the
measurement of the first sensor than our initial suggestion p̂t . Furthermore, based on the accuracy of
the sensors, it is possible to derive the accuracy of our estimate.
Now consider the case where we are also interested in estimating the position pt+1 . Knowledge that
the sensor is worn by a human or placed in a car, would give us information about how far the sensor
can travel from time t to time t + 1. If the sensor would be placed in for instance a train, the motion
would even be constrained to be along the tracks. Incorporating this information about the dynamics of
the sensor will improve the estimate of pt+1 .
We split the knowledge that we want to infer into the unknown time-varying states xt for t = 1, . . . , N ,
or equivalently x1:N , and the unknown constant parameters θ. We denote the measurements by yk for
k = 1, . . . , K. The times k at which these measurements are obtained do not necessarily correspond
with the times t at which the states are defined. It is also not necessary for all sensors to sample at
the same frequency. As discussed in §2.4, the inertial sensors are typically sampled at fairly high rates
to capture high-frequency dynamics. In stand-alone, wired IMUs, all sensors typically have the same,
constant sampling frequency. Specifically in the case of wireless sensors and smartphones, however, the
sampling frequencies can vary both over sensors and over time. In the remainder, we assume that the
times t at which the states are defined coincide with the times k at which the gyroscopes sample. Hence,
we denote the gyroscope measurements yω,t with t = 1, . . . N . For notational convenience, we will also
use the subscript t for the measurements from other sensors. Note that these are not required to actually
sample at each time t for t = 1, . . . N . For instance, magnetometers in smartphones often sample either
at equal or at half the sampling frequencies of the inertial sensors, while position aiding sensors like for
instance GNSS or UWB typically sample at much lower sampling frequencies.
Our aim is now to infer information about the states x1:N and the parameters θ using the measure-
ments y1:N and the probabilistic models. This can be expressed in terms of a conditional probability
distribution
where p(a | b) denotes the conditional probability of a given b. In the pose estimation problem, we are
interested in obtaining point estimates which we denote x̂1:N and θ̂. It is typically also highly relevant
to know how certain we are about these estimates. This is often expressed in terms of a covariance.
When the distribution (3.1) is Gaussian, the distribution is completely described in terms of its mean
and covariance.
In (3.1) we assume that all measurements y1:N are used to obtain the posterior distribution of x1:N
and θ. This is referred to as smoothing. Although it makes sense to use all available information to
obtain the best estimates, a downside of smoothing is that we need to wait until all measurements are
collected before the pose can be computed. Because of this, in many applications, we are also interested
in filtering. In filtering we estimate xt using all measurements up to and including time t. One way of
dealing with constant parameters in filtering is to treat them as slowly time-varying. In this case, they
15
x1 x2 xN −1 xN
y1 y2 yN −1 yN
can be considered to be included in the time-varying states xt . The filtering problem can be expressed
in terms of the conditional probability distribution
p(xt | y1:t ). (3.2)
We have now introduced smoothing, where the states x1:N are estimated simultaneously, and filtering,
where at each time instance the state xt is estimated. There is a large range of intermediate methods,
where a batch of states xt−L1 :t+L2 , with L1 and L2 being positive integers, is estimated using the
measurements y1:t . This is related to fixed-lag smoothing and moving horizon estimation [66, 113].
The topic of how to estimate the conditional probability distributions for position and orientation
estimation will be introduced in Chapter 4. We will now instead take a closer look at these distributions
and their different components. A fundamental assumption here is that we assume that our models
possess the Markov property, implying that all information up to the current time t is contained in the
state xt . This is illustrated in Figure 3.2 in terms of a probabilistic graphical model [12]. The state xt+1
can be seen to depend on xt and to result in the measurements yt+1 . It is conditionally independent of
x1:t−1 given the state xt . Using Bayes’ rule and the Markov property, the conditional distributions (3.1)
and (3.2) can be decomposed as
N
Y N
Y
p(x1:N , θ | y1:N ) ∝ p(θ)p(x1 | θ) p(xt | xt−1 , θ) p(yt | xt , θ), (3.3a)
t=2 t=1
p(xt | y1:t ) ∝ p(yt | xt )p(xt | y1:t−1 ). (3.3b)
The predictive distribution p(xt | y1:t−1 ) can be computed by marginalizing out the previous state xt−1
as
Z
p(xt | y1:t−1 ) = p(xt | xt−1 )p(xt−1 | y1:t−1 ) dxt−1 . (3.4)
In (3.3), p(θ) and p(x1 | θ) encode our prior information of θ and the knowledge of the state x1 given θ,
respectively. The dynamics are modeled in terms of p(xt+1 | xt , θ) and p(xt+1 | xt ). The distributions
p(yt | xt , θ) and p(yt | xt ) model the information given by the measurements about the state and the
parameters.
The dynamics of the state can be modeled in terms of a nonlinear function ft ( · ) as
xt+1 = ft (xt , wt ). (3.5)
The uncertainty of the dynamic model is modeled in terms of wt , which is often referred to as the process
noise. The model (3.5) provides information about the distribution p(xt+1 | xt ). More explicitly, if wt is
Gaussian additive noise with wt ∼ N (0, Q), then
p(xt+1 | xt ) ∼ N (xt+1 ; ft (xt ), Q), (3.6)
where we use the notation N (xt+1 ; ft (xt ), Q) to explain that the random variable xt+1 is normal dis-
tributed with mean ft (xt ) and covariance Q.
The information given by the measurements about the state xt can be modeled as
yt = ht (xt , et ), (3.7)
where ht ( · ) is a possibly nonlinear function and et is the measurement noise. The measurement
model (3.7) provides information about the distribution p(yt | xt ). The combination of (3.5), (3.7)
and a model of the prior p(x1 ) is referred to as a state space model [69] which is widely used in a large
number of fields.
16
x x
α x?
v v α
u
Figure 3.3: Left: clockwise rotation α of the vector x to the vector x? . Right: counterclockwise rotation
α of the coordinate frame v to the coordinate frame u.
The properties (3.8) provide an interpretation of the name special orthogonal group SO(3). All orthog-
onal matrices of dimension 3 × 3 have the property RRT = RT R = I3 and are part of the orthogonal
group O(3). The notion special in SO(3) specifies that only matrices with det R = 1 are considered
rotations.
Consider two coordinate frames denoted u and v. As was illustrated in Example 2.1, a vector x
expressed in the v-frame can be rotated to the u-frame as
xu = Ruv xv , (3.9a)
A rotation matrix is a unique description of the orientation. It has 9 components which depend on each
other as defined in (3.8).
Example 3.2 (Rotation of a coordinate frame and rotation of a vector) Consider the 2D ex-
ample in Figure 3.3, where on the left, a vector x is rotated clockwise by an angle α to x? . This is
equivalent to (on the right) rotating the coordinate frame v counterclockwise by an angle α. Note that
xv? = xu .
17
n
xk
x⊥
α
x
x?
Figure 3.4: Clockwise rotation of a vector x by an angle α around the unit vector n. The rotated vector
is denoted by x? . The vector x is decomposed in a component xk that is parallel to the axis n, and a
component x⊥ that is orthogonal to it.
In Figure 3.4, a vector x is rotated an angle α around the unit vector n. We denote the rotated
vector by x? . Suppose that x as expressed in the coordinate frame v is known (and denoted xv ) and
that we want to express xv? in terms of xv , α and n. It can first be recognized that the vector x can
be decomposed into a component parallel to the axis n, denoted xk , and a component orthogonal to it,
denoted x⊥ , as
where
Denoting the rotated coordinate frame the u-frame and using the equivalence between xv? and xu as
shown in Example 3.2, this implies that
This equation is commonly referred to as the rotation formula or Euler’s formula [135]. Note that the
combination of n and α, or η = nα, is denoted as the rotation vector or the axis-angle parameterization.
To show the equivalence between (3.13) and the rotation matrix parametrization, we will rewrite (3.13).
Here, we make use of the fact that a cross product can be written as a matrix vector product. Given
vectors u and v we have,
0 −u3 u2
u × v = [u×]v = −[v×]u, [u×] , u3 0 −u1 , (3.14)
−u2 u1 0
where u1 , u2 , u3 denote the three components of the vector u. Furthermore, given vectors u, v and w,
multiple cross products can be expanded in terms of the inner product as
18
Using these relations, (3.13) can be rewritten as
xu = xv cos α + nv (xv · nv )(1 − cos α) − (nv × xv ) sin α
= xv cos α + (nv × (nv × xv ) + xv )(1 − cos α) − (nv × xv ) sin α
= I3 − sin α[nv ×] + (1 − cos α)[nv ×]2 xv .
(3.16)
Comparing (3.16) and (3.9a), it can be seen that a rotation matrix can be parametrized in terms of α, n
as
Ruv (nv , α) = I3 − sin α[nv ×] + (1 − cos α)[nv ×]2 . (3.17)
Note that equivalently, Ruv (nv , α) can also be written as
Ruv (nv , α) = exp (−α[nv ×]) , (3.18)
since
∞
X k
exp (−α[nv ×]) = 1
k! (−α[nv ×])
k=0
v 1 2 v 2 1 3 v 1 4 v 2
= I3 − α[n ×] + 2! α [n ×] + 3! α [n ×] − 4! α [n ×] − . . .
3
v 1 2 1 4
1
α + . . . [nv ×]2
= I3 − α − 3! α + . . . [n ×] + 2! α − 4!
v v 2
= I3 − sin α[n ×] + (1 − cos α)[n ×] . (3.19)
The rotation vector introduced in this section parametrizes the orientation in only three parameters. It
is, however, not a unique parametrization since adding 2π to any angle α results in the same orientation.
This is called wrapping. As shown in (3.17) and (3.18), the rotation matrix can straightforwardly be
expressed in terms of the axis-angle representation.
19
z z z
z0 z0
z0 θ φ
y0
v y0 v v
u u u
ψ y y0 y y
0 0
x x
x x x0 x
Figure 3.5: Definition of Euler angles as used in this work with left: rotation ψ around the z-axis, middle:
rotation θ around the y-axis and right: rotation φ around the x-axis.
A unit quaternion is not a unique description of an orientation. The reason for this is that if q represents
a certain orientation, then −q describes the same orientation.
A rotation can be defined using unit quaternions as
c
x̄u = q uv x̄v (q uv ) , (3.24)
c
where (q uv ) = q vu denotes the quaternion conjugate, defined as
T
q c = q0 −qvT , (3.25)
Note that (3.26) is typically not a unit quaternion. The notation denotes the quaternion multiplication
given by
p0 q0 − pv · qv
pq = = pL q = q R p, (3.27)
p0 qv + q0 pv + pv × qv
where
−pT −qvT
p0 q0
pL , v
, qR , . (3.28)
pv p0 I3 + [pv ×] qv q0 I3 − [qv ×]
cos α2
q uv (nv , α) = , (3.30)
−nv sin α2
20
the two rotation formulations are equivalent since
u 1 01×3 0
x̄ =
03×1 I3 − 2 cos α2 sin α2 [nv ×] + 2 sin2 α2 [nv ×]2 xv
1 01×3 0
= . (3.31)
03×1 I3 − sin α[nv ×] + (1 − cos α) [nv ×]2 xv
T
Here, we made use of standard trigonometric relations and the fact that since knv k2 = 1, nv (nv ) =
I3 + [nv ×]2 . Hence, it can be concluded that q uv can be expressed in terms of α and nv as in (3.30).
Equivalently, q uv (nv , α) can also be written as
∞
X k
q uv (nv , α) = exp(− α2 n̄v ) = 1
k! − α2 n̄v , (3.32)
k=0
where
0 T
(n̄v ) = 1 0 0 0 , (3.33a)
T
1
(n̄v ) = 0 (nv )T , (3.33b)
2 T T
(n̄v ) = n̄v n̄v = −knv k22 03×1 = −1 03×1 , (3.33c)
T
3
(n̄v ) = 0 − (nv )T , (3.33d)
This leads to
∞
X k
q uv (nv , α) = exp(− α2 n̄v ) = 1
k! − α2 n̄v
k=0
!
1 α2 1 α4
1− 2! 43 + 4! 16 − ...
= 1 α5 v
− α2 nv + 1
3!
α
8 n v
− 5! 32 n + ...
cos α2
= . (3.34)
−nv sin α2
Note the similarity to (3.18) and (3.19). The reason why both rotation matrices and unit quaternions
can be described in terms of an exponential of a rotation vector will be discussed in §3.3.1.
3.3.1 Linearization
As mentioned in §3.2, the group of rotations in three dimensions is the special orthogonal group SO(3).
More specifically, SO(3) is a so-called matrix Lie group. For a discussion on the properties of matrix
21
Lie groups and on the reasons why SO(3) is indeed such a group we refer the reader to e.g. [8]. Since
rotations are a matrix Lie group, there exists an exponential map from a corresponding Lie algebra.
Using this property, it is possible to represent orientations on SO(3) using unit quaternions or rotation
matrices, while orientation deviations are represented using rotation vectors on R3 , see e.g. [14]. Hence,
we encode an orientation qtnb in terms of a linearization point parametrized either as a unit quaternion
q̃tnb or as a rotation matrix R̃tnb and an orientation deviation using a rotation vector ηt . Assuming that
the orientation deviation is expressed in the body frame n,1
n
η̄
qtnb = exp 2t q̃tnb , Rtnb = exp ([ηtn ×]) R̃tnb , (3.35)
logq : {q ∈ R4 : kqk2 = 1} → R , 3
(3.39a)
(log R)32
η = logR (R) = (log R)13 ,
(log R)21
logR : {R ∈ R3×3 : RRT = I3 , det R = 1} → R3 , (3.39b)
where log R is the standard matrix logarithm. Since we typically assume that ηtn is small, we will
frequently make use of the following approximations
1
expq (η) ≈ , logq (q) ≈ qv , (3.40a)
η
T
expR (η) ≈ I3 + [η×], logR (R) ≈ R32 R13 R21 . (3.40b)
The idea briefly outlined in this section is closely related to approaches used to estimate orientation
in robotics, see e.g. [47, 48, 14, 8, 38]. It is also related to the so-called multiplicative extended Kalman
filter (MEKF) frequently used in aeronautics, see e.g. [93, 28].
22
distributions. For instance, in [77, 44, 45] algorithms are presented to estimate orientation by modeling
it using a Bingham distribution.
The difficulties caused by directly using one of the four orientation parametrizations introduced in §3.2
in orientation estimation algorithms is widely recognized. Nevertheless, a large number of approaches
directly uses these parametrizations in estimation algorithms. For instance, it is common practice to
use unit quaternions in estimation algorithms and to normalize the resulting quaternions each time they
loose their normalization, see e.g. [124, 91, 89]. Different approaches to handle the normalization of the
quaternions in these algorithms are discussed in [67].
As was shown in Figure 2.3, the gyroscope measurement noise is quite Gaussian. Because of this, it is
typically assumed that ebω,t ∼ N (0, Σω ). If the sensor is properly calibrated, the measurements in the
three gyroscope axes are independent. In that case, it can be assumed that
2
σω,x 0 0
2
Σω = 0 σω,y 0 . (3.42)
2
0 0 σω,z
b
The gyroscope bias δω,t is slowly time-varying, as discussed in §2.4. There are two conceptually
different ways to treat this slowly time-varying bias. One is to treat the bias as a constant parameter,
assuming that it typically changes over a longer time period than the time of the experiment. The bias
can then either be pre-calibrated in a separate experiment, or it can be considered to be part of the
unknown parameters θ as introduced in §3.1. Alternatively, it can be assumed to be slowly time-varying.
This can be justified either by longer experiment times or by shorter bias stability. In the latter case,
b
δω,t can instead be considered as part of the state vector xt and can for instance be modeled as a random
walk
b b
δω,t+1 = δω,t + ebδω ,t , (3.43)
where ebδω ,t ∼ N (0, Σδω ,t ) represents how constant the gyroscope bias actually is.
Modeling the sensor noise and bias is related to the sensor properties. However, there are also
modeling choices related to the experiments that can be made. As described in §2.2, the angular velocity
b
ωib can be expressed as
b
= Rtbn ωie,t
n n b
ωib,t + ωen,t + ωnb,t . (3.44)
If the sensor does not travel over significant distances as compared to the size of the earth — which
is often the case for the applications discussed in Chapter 1 — the navigation frame n can safely be
n
assumed to be stationary. In that case, the transport rate ωen,t is zero. Although the earth rotation
ωie as expressed in the body frame b is not constant, its magnitude as compared to the magnitude of
the actual measurements is fairly small (see §2.2 and the experimental data presented in Example 2.3).
Assuming that the earth rotation is negligible and the navigation frame is stationary leads to the following
simplified measurement model
b b
yω,t = ωnb,t + δω,t + ebω,t . (3.45)
23
3.4.2 Accelerometer measurement models
The accelerometer measures the specific force ftb at each time instance t, see also §2.3. As shown in §2.4,
the accelerometer measurements are typically assumed to be corrupted by a bias δa,t and noise ea,t as
The accelerometer noise is typically quite Gaussian as was illustrated in Figure 2.4 and can hence be
modeled as eba,t ∼ N (0, Σa ). For a properly calibrated sensor, the covariance matrix Σa can often be
assumed to be diagonal.
b
The accelerometer bias δa,t is slowly time-varying. Similar to the gyroscope bias, the accelerometer
bias can either be modeled as a constant parameter, or as part of the time-varying state, for instance
using a random walk model as in (3.43).
As introduced in §2.3, the specific force measured by the accelerometer is given by
Assuming that the navigation frame is fixed to the earth frame, we derived a relation for anii as
Since the accelerometer measures both the local gravity vector and the linear acceleration of the
sensor, it provides information both about the change in position and about the inclination of the
sensor. For orientation estimation, only the information about the inclination is of concern. Hence, a
model for the linear acceleration needs to be made to express the relation between the inclination and the
measurements. To model this, it can be recognized that in practice, most accelerometer measurements
are dominated by the gravity vector, as illustrated in Example 3.3.
Example 3.3 (Magnitude of a sensor’s linear acceleration) Let us consider a 1D example where
a sensor has an initial velocity v1 = 0 m/s and accelerates with annn = 9.82 m/s2 . After 4.51 seconds,
the sensor will have traveled 100 meters. This is about twice as fast as the world record currently held
by Usain Bolt. In fact, humans can reach fairly high accelerations but can only accelerate for a short
time. Naturally, cars can accelerate to higher velocities than humans. The sensor in this example has
reached a final velocity of 160 km/h. Even in the case of a car it is therefore unlikely that it can have
an acceleration this high for a long period of time.
Since the accelerometer measurements are typically dominated by the gravity vector, a commonly
used model assumes the linear acceleration to be approximately zero
Naturally, the model (3.50) is almost never completely true. However, it can often be used as a sufficiently
good approximation of reality. Note that the noise term eba,t in this case does not only represent the
measurement noise, but also the model uncertainty. The model (3.50) can for instance be used in
combination with outlier rejection where measurements that clearly violate the assumption that the
linear acceleration is zero are disregarded. It is also possible to adapt the noise covariance matrix
Σa , depending on the sensor’s acceleration [39, 115]. Furthermore, it is possible to instead model the
acceleration based on physical reasoning [85].
24
Magnetometer models Magnetometers measure the local magnetic field, consisting of both the earth
magnetic field and the magnetic field due to the presence of magnetic material. The (local) earth magnetic
field is denoted mn and it is illustrated in Figure 3.6. Its horizontal component points towards the earth’s
magnetic north pole. The ratio between the horizontal and vertical component depends on the location
on the earth and can be expressed in terms of the so-called dip angle δ. The dip angle and the magnitude
of the earth magnetic field are accurately known from geophysical studies, see e.g. [102].
Assuming that the sensor does not travel over significant distances as compared to the size of the earth,
the local earth magnetic field can be modeled as being constant. In case no magnetic material is present
in the vicinity of the sensor, orientation information can be deduced from the magnetometer. More
specifically, magnetometers are typically used to complement accelerometers to provide information about
the sensor heading, i.e. about the orientation around the gravity vector which can not be determined from
the accelerometer measurements. Magnetometers provide information about the heading in all locations
on the earth except on the magnetic poles, where the local magnetic field mn is vertical. Orientation
can be estimated based on the direction of the magnetic field. The magnitude of the field is irrelevant.
Because of this, without loss of generality we model the earth magnetic field as
T
mn = cos δ 0 sin δ , (3.51)
i.e. we assume that kmn k2 = 1. Assuming that the magnetometer only measures the local magnetic
field, its measurements ym,t can be modeled as
where em,t ∼ N (0, Σm ). The noise em,t represents the magnetometer measurement noise as well as the
model uncertainty. Note that using the models (3.51) and (3.52), we define the heading with respect to
the magnetic north, which is sufficient for most applications. In case we would be interested in navigation
with respect to the true north instead, the magnetic declination needs to be taken into account. Since
the magnetic declination depends on the location on the earth it is necessary to know the location of the
sensor to correct for this.
In practice, the actual magnetic field can differ significantly from the earth magnetic field. In indoor
environments, for instance, presence of magnetic material in the structure of buildings and in furniture
influences the magnetic field that is measured by the magnetometer. Furthermore, the magnetic field is
affected in applications where the magnetometer is mounted in e.g. a vehicle, train or on a robot. In
case the magnetic material is rigidly attached to the sensor, the magnetometer can be calibrated for its
presence [75, 149, 118, 126]. The presence of magnetic material in the vicinity of the sensor that can not
be calibrated for is of major concern for practical applications. Because of this, there is a vast amount
of literature on the topic, see e.g. [21, 80, 120].
Note that when using magnetometers for orientation estimation, the presence of magnetic material
is typically considered to be an undesired disturbance. However, the presence of magnetic material can
also be considered to be a property which can be exploited. This is done in approaches which use the
magnetic field as a source of position information [143, 56, 119].
Position information Position information can be obtained from for instance GNSS or UWB mea-
surements. In this tutorial, we will consider a very basic measurement model where the sensors directly
measure the position as
25
mn
(a) (b)
Figure 3.6: (a) Schematic of the earth magnetic field lines (green) around the earth (blue).3 (b) Schematic
of a part of the earth where the local earth magnetic field mn makes an angle δ with the horizontal plane.
This angle is called the dip angle.
b
and that the orientation and angular velocity ωnb,t are related as
dq nb dRnb
dt = q nb 12 ω̄nb
b
, dt = Rnb [ωnb
b
×], (3.55)
depending on orientation parametrization. For a derivation of (3.55), see e.g. [59]. Using an Euler
discretization of (3.54) assuming that the acceleration is constant between samples, the dynamics of the
position and velocity can be expressed in terms of the acceleration as
T2 n
pnt+1 = pnt + T vn,t
n
+ 2 ann,t , (3.56a)
n n
vn,t+1 = vn,t + T annn,t , (3.56b)
where T is the time between two samples. Similarly, the dynamics of the orientation can be expressed
in terms of unit quaternions or rotation matrices as
nb
= qtnb expq T2 ωnb,t
b
qt+1 , (3.57a)
nb nb b
Rt+1 = Rt expR T ωnb,t . (3.57b)
Dynamic models describe how the state changes over time. For the problem of position and ori-
entation estimation using inertial sensors, there are two commonly used modeling alternatives for the
dynamics [50]. In the first, the state vector xt is chosen to consists of
T T
n T
T T T
xt = (pnt ) vn,t annn,t qtnb b
ωnb,t . (3.58)
The change in position, velocity and orientation states can then be described in terms of the velocity,
acceleration and angular velocity states, respectively. The dynamics of the acceleration and the angular
velocity can be described in terms of a motion model. Examples of motion models that can be used are
a constant acceleration model, which assumes that the dynamics of the acceleration can be described as
with wa,t ∼ N (0, Σw,a ), and a constant angular velocity model, which describes the dynamics of the
angular velocity as
b b
ωnb,t+1 = ωnb,t + wω,t , (3.60)
with wω,t ∼ N (0, Σw,ω ). The process noise terms wa,t and wω,t model the assumptions on how constant
the acceleration and angular velocity actually are.
3 Adapted version of ‘Dipolar magnetic field’ by Cyril Langlois available at http://texample.net under CC BY 2.5
(http://creativecommons.org/licenses/by/2.5).
26
Alternatively, the state vector xt can be chosen as
T T T
xt = (pnt )T n
vn,t qtnb . (3.61)
To describe the dynamics of the states, the inertial measurements can then be used as an input to the
dynamic equation (3.5). Hence, the change in position, velocity and orientation is directly modeled in
terms of the inertial measurements. In this case, expressions for annn,t and ωnb,t
b
in (3.56) and (3.57) are
obtained from the accelerometer measurement model and the gyroscope measurement model, see §3.4.
The process noise can explicitly be modeled in terms of the accelerometer measurement noise ea,t and
the gyroscope measurement noise eω,t .
The benefit of using a motion model for the state dynamics is that knowledge about the motion of the
sensor can be included in this model. However, it comes at the expense of having a larger state vector.
The benefit of using the inertial measurements as an input to the dynamics is that the process noise has
the intuitive interpretation of representing the inertial measurement noise. Hence, the latter approach
is often used for applications where it is difficult to obtain sensible motion models. Another difference
between the two approaches is that changes in the acceleration and angular velocity will have a slightly
faster effect on the state when the inertial measurements are used as an input to the dynamics. The
reason for this is that the constant acceleration and angular velocity models delay the effects of changes
in the dynamics. Because of this, their estimates typically look slightly more smooth.
For the prior p(x1 ), it is typically possible to get a reasonable estimate from data. For the position
and velocity, this can be modeled as
Here, the estimate p̆n1 can for instance be determined based on the first position measurement. In that
case, the uncertainty σp,i can also be chosen equal to the uncertainty of the position measurements. In
case no additional information is available, the estimates p̆n1 and v̆1n can be set to zero with an appropriate
standard deviation instead.
A commonly used method to determine the initial orientation is to use the first accelerometer and
magnetometer samples. This method is based on the fact that given two (or more) linearly independent
vectors in two coordinate frames, the rotation between the two coordinate frames can be determined. The
implicit assumption is that the accelerometer only measures the gravity vector and the magnetometer
only measures the local magnetic field. Hence, the four vectors are given by the measurements ya,t and
ym,t , the local gravity vector g n and the local magnetic field mn . These vectors are linearly independent
except when the measurements are obtained on the magnetic north or south poles where the dip angle
is δ = 0 and the magnetic field does not contain any heading information.
The accelerometer provides information about the sensor’s inclination. Heading information is pro-
vided by the magnetometer. However, at all locations except on the equator, the magnetometer also
provides information about the inclination due to its non-zero vertical component, see (3.51). In prac-
tice, the accelerometer typically provides more accurate inclination information. Hence, we choose to
use the magnetometer only to provide heading information by projecting the magnetic field and the
27
magnetometer measurement on the horizontal plane. Furthermore, we normalize the vectors. Because
of this, an adapted model uses the four normalized vectors
T ya,1
ĝ n = 0 0 1 , ĝ b = kya,1 k2 , (3.64a)
T
ym,1
m̂n = 1 0 0 , m̂b = ĝ b × kym,1 k2 × ĝ b . (3.64b)
A number of algorithms are available to estimate the orientation from these vectors. Well-known examples
are the TRIAD algorithm, the QUEST algorithm, see e.g. [137], and the method presented in [62]. For
our problem at hand, these methods give equivalent results, even though they use slightly different
solution strategies. Generally speaking, they solve the problem of determining the rotation q nb from
subj. to kq nb k2 = 1, (3.65)
Recall from (3.24) that q nb x̄b q bn is the rotation of the vector xb to the n-frame. The optimization
problem (3.65) therefore determines the orientation q nb that minimizes the distance between the nor-
malized magnetic field and gravity vectors measured in the first sample and the normalized magnetic
field and gravity vectors in the navigation frame. These four vectors were defined in (3.64).
Defining
L R L R
A = − ĝ¯n ĝ¯b ¯n
− m̂ ¯b
m̂ , (3.66)
where the left and right quaternion multiplications are defined in (3.28), (3.65) can equivalently be
written as
T
q̆1nb = arg min q nb Aq nb
q nb
subj. to kq nb k2 = 1. (3.67)
For a derivation, see [59]. The solution to this problem is given by the eigenvector corresponding to the
largest eigenvalue of A. Note that although this method can be used to compute the orientation from
any two linearly independent vectors in two coordinate frames, we only use it to compute a prior on the
orientation.
Based on the estimate q̆1nb from (3.67), we can model the orientation at time t = 1 in terms of an
orientation deviation
e
q1nb = expq η,i
2 q̆1nb , eη,i ∼ N (0, Ση,i ), (3.68a)
or in terms of a quaternion as
Explicit formulations for the covariance of the orientation estimates from the TRIAD and QUEST
algorithms are discussed by [136]. In practice, however, the accuracy of the estimates from (3.67) highly
depends on the validity of the model assumptions, i.e. on whether the sensor is indeed far from magnetic
material and whether the linear acceleration is indeed zero. Because this has such a significant influence
on the quality of the estimates, we choose Ση,i and Σq,i somewhat conservatively. Modeling that in 68%
of the cases the orientation error is less than 20◦ ,
2 20
Ση,i = ση,i I3 , ση,i = 180 π, (3.69a)
R T R
∂ expq (eη,i ) ∂ expq (eη,i )
Σq,i = 14 q̆1nb ∂eη,i Ση,i ∂eη,i q̆1bn , (3.69b)
where we use the fact that (q R )T = (q c )R . Note that the covariance Σq,i is defined in terms of the
covariance Ση,i to allow for explicit comparison between different algorithms in Chapter 4.
28
3.7 Resulting probabilistic models
The information from the previous sections can now be combined into one model which will be used in
the algorithms in Chapter 4. In this section, we describe our modeling choices for the pose estimation
problem and for the orientation estimation problem.
We assume that the sensor does not travel over significant distances as compared to the size of the
earth and hence keep the navigation frame n fixed with respect to the earth frame e. Furthermore, we
assume that the magnitude of the earth rotation and of the Coriolis acceleration are negligible. Our
gyroscope and accelerometer models are hence given by
In the remainder, for notational convenience we drop the subscripts n which indicate in which frame the
differentiation is performed, see §2.3, and use the shorthand notation an for annn . Furthermore, we will
b
denote ωnb simply by ω and omit the superscript b on the noise terms ea,t and eω,t and the bias terms
δa,t and δω,t . We assume that the inertial measurement noise is given by
i.e. we assume that the three sensor axes are independent and have the same noise levels.
where without loss of generality, we switch the sign on the noise. Note that the noise term ea,t should
be rotated to the navigation frame n by multiplying it with the rotation matrix Rtnb . However, because
of the assumption (3.71), the rotation matrix can be omitted without loss of generality. The dynamics
of the orientation parametrized using quaternions is given by
nb
= qtnb expq T2 (yω,t − δω,t − eω,t ) .
qt+1 (3.73)
where
with Σa = σa2 I3 and Σω = σω2 I3 . Note that we model the process noise on the position and velocity
states in terms of the accelerometer noise. However, we do not enforce these to have the same noise
realization. Hence, we use the notation ep,a,t and ev,a,t for the two process noise terms. The covariance
of both is equal to the covariance of the accelerometer noise. The initial position is assumed to be given
by the first position measurement as
29
while the initial velocity is assumed to be approximately zero as
The orientation at time t = 1 is given by the QUEST algorithm described in §3.6, parametrized in terms
of quaternions or rotation vectors as
e
q1nb = expq η,i
2 q̆1nb , eη,i ∼ N (0, Ση,i ), (3.74g)
q1nb = q̆1nb + eq,i , eq,i ∼ N (0, Σq,i ), (3.74h)
where the initial orientation uncertainty is given in terms of a standard deviation of 20◦ .
In Chapter 4 we assume that the inertial measurement are properly calibrated. Hence, we assume
that their biases δa,t and δω,t are zero. Calibration is the topic of Chapter 5 where we will also introduce
possible extensions of the state space model in which the bias terms are included either as states or as
unknown parameters.
where (3.75a) describes the dynamics while (3.75b) and (3.75c) describe the measurement models and
with Σω = σω2 I3 and Σa = σa2 I3 . The initial orientation is given by the QUEST algorithm described in
§3.6 and is modeled as in (3.74g) or (3.74h). Also for orientation estimation, in Chapter 4 we assume
that the inertial measurements are properly calibrated. Hence, we assume that the bias δω,t is zero.
30
Chapter 4
In this chapter we will focus on position and orientation estimation using the models (3.74) and (3.75)
derived in Chapter 3. In §4.1, we will first describe a method to solve the smoothing problem (3.3a).
Subsequently, in §4.2–§4.4, we will derive different methods for solving the filtering problem (3.3b).
In each section, after a general introduction of the estimation method, we will illustrate the method
by explicitly deriving algorithms to estimate the orientation using the state space model (3.75). The
orientation estimation problem also illustrates the most important parts of the pose estimation problem,
since most complexities lie in the parametrization of the orientation and in the nonlinear nature of the
orientation. In §4.5, we show some characteristics of the different algorithms for the orientation estimation
problem. In §4.6, we will discuss how the algorithms for orientation estimation can be extended to also
estimate the position. Throughout this section, we assume that the sensors are calibrated, i.e. we assume
that we do not have any unknown parameters θ in our models. Because of this, the models that we use
are the most basic models that can be used for position and orientation estimation using inertial sensors.
Here, we use the notation in terms of probability distributions as introduced in §3.1 and model the
measurements and the state dynamics as described in §3.4 and §3.5, respectively. Furthermore, we
assume that a prior on the initial state is obtained using the measurements at t = 1 as described in §3.6.
Because of this, the measurement model p(y1 | x1 ) from (3.3a) is explicitly omitted in (4.1). Note that in
practice, we typically minimize − log p(x1:N | y1:N ) instead of maximizing p(x1:N | y1:N ) itself, resulting
in the optimization problem
N
X N
X
arg min − log p(x1 ) − log p(xt | xt−1 ) − log p(yt | xt ). (4.2)
x1:N
t=2 t=2
There are various ways to solve problems of this kind, for instance particle smoothers [81], an ex-
tended Rauch-Tung-Striebel (RTS) smoother [127] and optimization methods, see e.g. [103, 95]. The
latter approach is closely related to iterated Kalman smoothers [10, 65]. We will solve the problem
using an optimization method. Compared to extended RTS smoothers, optimization methods allow for
more flexibility in the models that are being used. For instance, additional information outside of the
standard state space model can straightforwardly be included. Optimization approaches are typically
computationally heavier than extended RTS smoothers but less heavy than particle smoothers. The
latter are capable of capturing the whole distribution, which is a clear advantage when the distributions
are multi-modal. Optimization instead gives a point estimate and an associated measure of uncertainty.
This is typically sufficient for position and orientation estimation using inertial sensors.
31
4.1.1 Gauss-Newton optimization
To obtain a smoothing estimate of the position and orientation using optimization, we first recognize
that for our models (3.74) and (3.75), all probability distributions in (4.2) are Gaussian. Let us therefore
consider a slightly more general problem where the objective function consists of the product of Gaussian
probability functions p(ei (x1:N )), i = 1, . . . , M . Hence, the optimization problem can be written as
M
X
x̂1:N = arg min − log p (ei (x1:N )) . (4.3)
x1:N
i=1
Omitting the terms independent of x1:N , the optimization problem (4.3) reduces to
M
X
x̂1:N = arg min 1
2 kei (x1:N )k2Σ−1 , (4.5)
x1:N i
i=1
−1
with kei (x1:N )k2Σ−1 = eT
i (x1:N )Σi ei (x1:N ). The function that is being minimized in optimization prob-
i
lems, is often referred to as the objective function.
The solution to (4.5) can be found by studying the shape of the objective function as a function of
x1:N . This can be characterized in terms of the gradient G(x1:N ) and Hessian H(x1:N ), which provide
information about the slope and curvature of the function, respectively. Defining
−1 −1/2
eT T
i (x1:N )Σi ei (x1:N ) = εi εi , εi = Σi ei (x1:N ),
and the stacked variables
T
ε = εT
1 ··· εT
M ,
the gradient and the Hessian are given by
M
X T
G(x1:N ) = ∂εi
∂x1:N εi = J T (x1:N )ε, (4.6a)
i=1
M T
X 2
H(x1:N ) = ∂εi
∂x1:N
∂εi
∂x1:N + εT ∂ εi
i ∂x2
1:N
i=1
M
X 2
= J T (x1:N )J (x1:N ) + εT ∂ εi
i ∂x2 . (4.6b)
1:N
i=1
Note that for notational convenience, we have omitted the explicit dependence of ε on x1:N . In (4.6), we
introduced the notation J (x1:N ), which is the Jacobian of the vector ε with respect to x1:N as
∂ε1 ∂ε1
∂x1 ... ∂xN
. ..
J (x1:N ) = .. . , (4.7)
∂εM nε ∂εM nε
∂x1 ... ∂xN
where nε is the length of the vector εi . Instead of computing the true Hessian (4.6b), we compute an
approximation of it [103], given by
Ĥ(x1:N ) = J T (x1:N )J (x1:N ). (4.8)
This has the benefit of not having to compute second derivatives, at the same time as it guarantees that
the Hessian is positive semidefinite. The downside of using (4.8) is that it introduces an approximation.
The gradient and the (approximate) Hessian can be used to find the minimum of the objective
function. For our models (3.74) and (3.75), in which the functions ei (x1:N ) are nonlinear, an estimate
x̂1:N can iteratively be computed as
−1
(k+1) (k) (k) (k)
x̂1:N = x̂1:N − β (k) Ĥ(x̂1:N ) G(x̂1:N ), (4.9)
32
Figure 4.1: An illustration of the sparsity pattern that is present in the smoothing problem.
where k denotes the iteration number. The step length β (k) is computed for instance using a backtracking
−1
(k) (k)
line search [103, 16]. The search direction is computed as Ĥ(x̂1:N ) G(x̂1:N ). Note that an initial point
(0)
x̂1:N needs to be chosen close enough to the desired minimum to ensure convergence to this minimum.
In case the functions ei (x1:N ) would be linear, the problem (4.5) would be a least squares (LS)
problem for which the update (4.9) directly leads to the minimum of the objective function, irrespective
of the initial point. In our case where the functions ei (x1:N ) are nonlinear due to the nonlinear nature
of the orientation, the problem (4.5) is instead a nonlinear least squares (NLS) problem. Each iteration
in (4.9) can be interpreted as solving a LS problem around a linearization point. The linearization point
is updated after each iteration, bringing it closer to the minimum of the objective function. Computing
an estimate x̂1:N by iterating (4.9) until convergence, making use of the approximate Hessian from (4.8),
is called Gauss-Newton optimization.
Note that since the inertial sensors sample at high sampling rates, the length of the vector x1:N quickly
becomes fairly large. For instance, for inertial sensors sampling at 100Hz for 10 seconds, N = 1 000 and
the size of the (approximate) Hessian Ĥ(x) is 1 000nx × 1 000nx , where nx is the length of the vector
xt . However, as can be seen in (4.2), the components of the objective function only depend on the
current and next time steps xt and xt+1 . Hence, the structure of the Hessian (4.8) is of the form given in
Figure 4.1. There exist efficient algorithms to compute search directions for problems with this sparsity
pattern, which can be exploited using sparse matrix packages, see e.g. [29], or by using tools like dynamic
programming and message passing [11, 46, 123].
with
33
optimization problem since these norm constraints are non-convex. Instead, we encode an orientation
in terms of a linearization point parametrized as a unit quaternion q̃tnb and an orientation deviation
parametrized as a rotation vector ηtn as discussed in §3.3.1. Hence, we model the orientation as
n
η
qtnb = expq 2t q̃tnb . (4.12)
n
At each Gauss-Newton iteration (4.9), we estimate the state vector η1:N . Before starting the next
nb n
iteration, the linearization points q̃1:N are updated and the state vector η1:N is reset to zero.
Using the notation introduced in §3.3.1, the objective function (4.10) can be expressed in terms of
the orientation deviation ηtn by rewriting (4.11) as
ηn
eη,i = 2 logq expq ( 21 ) q̃1nb q̆1bn , (4.13a)
n n
η η
eω,t = T2 logq (q̃tbn expq ( 2t ))c expq ( t+1 nb
2 ) q̃t+1 − yω,t , (4.13b)
T
ea,t = ya,t + R̃tbn (expR (ηtn )) g n , (4.13c)
T
em,t = ym,t − R̃tbn (expR (ηtn )) m . n
(4.13d)
Here, R̃tbn is the rotation matrix representation of the linearization point q̃tbn . This leads to the following
derivatives
∂eη,i ∂ logq (q) R ∂ expq (η1n )
∂η1n = ∂q q̃1nb q̆1bn ∂η1n , (4.14a)
L n
∂eω,t ∂ log (q) nb R ∂ expq (ηt+1 )
= T1 ∂qq q̃tbn
∂η n q̃t+1 ∂η n , (4.14b)
t+1 t+1
n c
nb R ∂ (expq (ηt )) ∂ expq (ηt )
L n
∂eω,t 1 ∂ logq (q)
q̃tbn
∂ηtn = T ∂q q̃t+1 ∂ expq (ηtn ) ∂ηtn , (4.14c)
∂ea,t
∂ηtn ≈ R̃tbn [g n ×], (4.14d)
∂em,t
∂ηtn ≈ −R̃tbn [mn ×], (4.14e)
where, using (3.40) and the definition of the quaternion conjugate (3.25),
T c
∂ logq (q) 01×3 ∂ (expq (η)) 1 01×3
≈ , = ,
∂q I3 ∂ exp η 03×1 −I3
∂ expq η 01×3
≈ . (4.15)
∂η I3
Using the approximate derivatives (4.14), the gradient and approximate Hessian can be computed for
the Gauss-Newton iterations. The resulting solution is summarized in Algorithm 1. One of the inputs
nb,(0)
to the algorithm is an initial estimate of the orientation q̃1:N , which will aid the convergence to the
nb,(0)
desired minimum. There are (at least) two ways to obtain good initial estimates q̃1:N . First, they can
be obtained by direct integration of the gyroscope measurements. As discussed in §1.2, these estimates
will suffer from integration drift. However, they still form a good initial estimate for the optimization
problem. Second, one of the other, computationally cheaper estimation algorithms that will be discussed
in the remainder of this section can be used to obtain initial estimates of the orientation.
34
Algorithm 1 Smoothing estimates of the orientation using optimization
nb,(0)
Inputs: An initial estimate of the orientation q̃1:N , inertial data {ya,t , yω,t }N N
t=1 , magnetometer data {ym,t }t=1
and covariance matrices Σω , Σa and Σm .
nb n
Outputs: An estimate of the orientation q̂1:N and optionally its covariance cov(η̂1:N ).
n,(0)
1. Set η̂t = 03×1 for t = 1, . . . , N , set k = 0 and compute q̆1nb and Ση,i as described in §3.6.
2. while termination condition is not satisfied do
(a) Compute the gradient (4.6a) and the approximate Hessian (4.8) of the orientation smoothing prob-
lem (4.10) using the expressions for the different parts of the cost function and their Jacobians (4.13)
and (4.14).
n,(k+1)
(b) Apply the update (4.9) to obtain η̂1:N .
(c) Update the linearization point as
n,(k+1)
nb,(k+1) η̂t nb,(k)
q̃t = expq 2
q̃t , (4.16)
n,(k+1)
and set η̂t = 03×1 for t = 1, . . . , N .
(d) Set k = k + 1.
end while
nb nb,(k)
3. Set q̂1:N = q̃1:N .
4. Optionally compute
−1
n
cov(η̂1:N ) = J T (η̂1:N
n n
)J (η̂1:N ) . (4.17)
An intuition behind this expression is that the accuracy of the estimates is related to the sensitivity of
the objective function with respect to the states. The covariance of the estimate will play a crucial role
in the filtering approaches discussed in §4.2 and §4.3.
The matrix J T (x̂1:N )J (x̂1:N ) quickly becomes fairly large due to the high sampling rates of the
inertial sensors. Hence, computing its inverse can be computationally costly. We are, however, typically
only interested in a subset of the inverse. For instance, we are often only interested in diagonal or block
diagonal elements representing cov(xt ). It is therefore not necessary to explicitly form the complete
inverse, which makes the computation tractable also for larger problem sizes.
Note that without loss of generality, we have shifted our time indices as compared to the notation in
§3.1. The probability distribution p(xt+1 | y1:t ) can now be seen as a prior and can be obtained by
marginalizing out the previous state xt as
Z
p(xt+1 | y1:t ) = p(xt+1 , xt | y1:t )dxt
Z
= p(xt+1 | xt )p(xt | y1:t )dxt . (4.20)
35
Assuming that
Here, ηtn and q̃tnb provide information about the previous time step. We denote the orientation estimate
from the previous time step as q̂tnb and assume that η̂tn is zero since the linearization point is updated
after each iteration of the optimization algorithm. To start with a fairly good linearization point, we
nb,(0)
choose q̃t+1 at iteration k = 0 as
nb,(0)
= q̂tnb expq T
q̃t+1 2 yω,t . (4.24)
n
Following (4.22), the distribution p(ηt+1 | y1:t ) around this linearization point can now be written as
n n
; 0, Ft Pt|t FtT + Gt Σω GT
p(ηt+1 | y1:t ) ≈ N ηt+1 t , (4.25)
with
∂f (ηtn ,yω,t ,eω,t )
Ft = ∂ηtn eω,t =0
n nb
ηt =0,q̃t nb
=q̂t
ηtn
∂
=2 logq expq ( )
∂ηtn 2
ηtn =0
= I3 , (4.26a)
∂f (ηtn ,yω,t ,eω,t )
Gt = ∂eω,t eω,t =0
n nb
ηt =0,q̃t nb
=q̂t
∂
logq q̂tnb expq T2 (yω,t + eω,t )
=2 ∂eω,t
− T2 yω,t q̂tbn e =0
expq
ω,t
nb,(0)
≈ T R̃t+1 . (4.26b)
The covariance Pt|t in (4.25) can be approximated as the inverse of the Hessian of the objective
function from the previous time step, see also §4.1.3. We make use of the shorthand notation Pt+1|t =
Ft Pt|t FtT + Gt QGT
t and define
∂ef,t
ef,t = ηtn − 2 logq q̂t−1
nb
expq ( T2 yω,t−1 ) q̃tbn ,
∂ηtn = I3 . (4.27)
36
Note that ef,t is equal to zero for iteration k = 0 but can be non-zero for subsequent iterations. Using
this notation, the filtering problem (4.19) results in the following optimization problem
Note the similarity of this optimization problem to the smoothing formulation in (4.10). The term
kef,t kP −1 takes into account both the knowledge about the previous state xt and the dynamics. Fur-
t|t−1
−1
thermore, due to the fact that Pt|t−1 is time-varying, the uncertainty and cross-correlation of the states
at the previous time instance is taken into consideration. Including this term is similar to the inclusion
of an arrival cost in moving horizon estimation approaches [114].
After each Gauss-Newton iteration, we need to recompute the linearization point as
n,(k+1)
nb,(k+1) η̂ nb,(k)
q̃t = expq t 2 q̃t . (4.29)
We also need to compute the covariance around this updated linearization point as
T
(k+1) (k) (k) (k)
Pt|t−1 = Jt Pt|t−1 Jt , (4.30)
(k) (k)
where Jt can be derived similarly to the derivation of Ft in (4.26a). Since Jt = I3 , in practice the
relinearization of the covariance can be omitted. The process of estimating orientation using filtering is
summarized in Algorithm 2.
with
∂ft (xt ,ut ,wt ) ∂ft (xt ,ut ,wt )
Ft = ∂xt wt =0 , Gt = ∂vt wt =0 . (4.36)
xt =x̂t|t xt =x̂t|t
37
Algorithm 2 Filtering estimates of the orientation using optimization
Inputs: Inertial data {ya,t , yω,t }N N
t=1 , magnetometer data {ym,t }t=1 and covariance matrices Σω , Σa and Σm .
Outputs: An estimate of the orientation q̂tnb and its covariance Pt|t for t = 1, . . . N .
1. Compute q̆1nb and Σi as described in §3.6 and set q̂1nb = q̆1nb and P1|1 = Ση,i .
2. for t = 2, . . . N do
n,(0) nb,(0)
(a) Set η̂t = 03×1 , set k = 0, choose the linearization point q̃t as
nb,(0) nb T
q̃t = q̂t−1 expq 2
yω,t−1 , (4.31a)
n,(k+1)
η̂t = 03×1 .
iv. Set k = k + 1.
end while
nb,(k) n,(k)
(c) Set q̂tnb = q̃t , η̂tn = η̂t and compute Pt|t as
−1
Pt|t = J T (η̂tn )J (η̂tn ) . (4.33)
Here, the matrix P denotes the state covariance. The double subscripts on x̂t+1|t and Pt+1|t denote the
state estimate and the state covariance at time t + 1 given measurements up to time t. Similarly, x̂t|t
and Pt|t denote the state estimate and the state covariance at time t given measurements up to time t.
The measurement update makes use of the measurement model (4.34b) in combination with the
measurements yt to update the “predicted” state estimate as
with
and
∂ht (xt )
ŷt|t−1 = h(x̂t|t−1 ), Ht = ∂xt . (4.39)
xt =x̂t|t−1
Note that in (4.37) we have shifted our notation one time step compared to the notation in (4.35) to
avoid cluttering the notation. The EKF iteratively performs a time update and a measurement update
to estimate the state and its covariance.
38
the EKF time and measurement updates. Using the state space model (3.75) and using unit quaternions
as states in the EKF, the dynamic model is given by
nb
= ft (qtnb , yω,t , eω,t ) = qtnb expq T2 (yω,t − eω,t )
qt+1
R nb
= expq T2 (yω,t − eω,t ) qt
L
= qtnb expq T2 (yω,t − eω,t ) .
(4.40)
In the measurement update of the EKF, the state is updated using the accelerometer and magne-
tometer measurements. Using the measurement models
bn
∂Rt|t−1
The derivative nb
∂qt|t−1
can be computed from the definition of the relation between and the rotation
matrix and the quaternion representation given in (A.10).
Note that the quaternion obtained from the measurement update (4.37) is no longer normalized. We
nb
denote this unnormalized quaternion and its covariance by q̃t|t and P̃t|t , respectively. The ˜· instead
of the ˆ· is meant to explicitly indicate that the quaternion still needs to be updated. This is done
by an additional renormalization step as compared to a standard EKF implementation. A possible
interpretation of the renormalization is as an additional measurement update without measurement
noise. Hence, we adjust both the quaternion and its covariance as
nb
nb q̃t|t
q̂t|t = nb
kq̃t|t k2
, Pt|t = Jt P̃t|t JtT , (4.44a)
with
T
1 nb nb
Jt = q̃
kq̃ nb k3 t|t
q̃t|t . (4.44b)
t|t 2
Here, q T is the standard vector transpose. The resulting EKF is summarized in Algorithm 3.
39
Algorithm 3 Orientation estimation using an EKF with quaternion states
Inputs: Inertial data {ya,t , yω,t }N N
t=1 , magnetometer data {ym,t }t=1 and covariance matrices Σω , Σa and Σm .
nb
Outputs: An estimate of the orientation q̂t|t and its covariance Pt|t for t = 1, . . . N .
with Q = Σω and
L
R nb ∂ expq (eω,t−1 )
Ft−1 = expq ( T2 yω,t−1 ) , Gt−1 = − T2 q̂t−1|t−1 ∂eω,t−1
.
∂Rbn
− nb t|t−1
gn
∂qt|t−1 nb
=q̂ nb
Σa 0
q
Ht = ∂Rbn t|t−1 t|t−1 , R= .
t|t−1 n 0 Σm
m
nb
∂qt|t−1 nb nb qt|t−1 =q̂t|t−1
1 nb nb
T
with Jt = nb k3 q̃t|t
kq̃t|t
q̃t|t .
2
end for
In the time update of the EKF, we first use (3.75a) to directly update the linearization point as
nb nb
expq T2 yω,t .
q̃t+1|t = q̃t|t (4.49)
Around this linearization point, the derivatives of the dynamic model (4.48) are given by
∂f (η n ,y ,eω,t )
Ft = t t ∂ηω,t n eω,t =0 = I3 ,
t n =0
ηt
∂f (η n ,yω,t ,eω,t ) nb
Gt = t t∂eω,t eω,t =0 = T R̃t+1|t . (4.50a)
n =0
ηt
40
ηtn as
Equivalently, the magnetometer measurement equation can be written in terms of the state ηtn as
From these equations, the derivatives Ht as defined in (4.39) can straightforwardly be computed.
After the measurement update, the orientation deviation η̂tn is non-zero. Hence, as an additional step
in the EKF, we update the linearization point and reset the state. In our algorithm, we consider the
relinearization as the “measurement update” for the linearization point, i.e. the relinearization updates
nb nb
the linearization point q̃t|t−1 to q̃t|t as
η̂tn
nb nb
q̃t|t = expq 2 q̃t|t−1 . (4.53)
For the same reasons as in §4.2, the relinearization of the covariance can be omitted. The resulting EKF
is summarized in Algorithm 4. Note the similarities between this algorithm and Algorithm 2.
η̂tn = Kt εt , (4.55a)
P̃t|t = Pt|t−1 − Kt St KtT , (4.55b)
(c) Relinearize
η̂tn
nb nb
q̃t|t = expq 2
q̃t|t−1 . (4.56)
end for
41
4.4 Complementary filtering
An alternative to using EKFs for orientation estimation is to use complementary filters [58, 17]. This
type of algorithms again estimates the orientation at time t given the measurements y1:t . However, it
does not use the probabilistic models presented in Chapter 3. Instead, complementary filters explicitly
use the fact that both the gyroscope and the combination of the accelerometer and the magnetometer
provide information about the orientation of the sensor. The orientation estimates obtained from the
accelerometer and the magnetometer measurements are noisy but accurate over long periods of time.
On the other hand, the orientation estimates using the gyroscope measurements are accurate on a short
time scale but they drift over time. These properties can be interpreted and exploited in the frequency
domain. The orientation estimates using the gyroscope have desirable properties at high frequencies. We
would therefore like to filter these using a high-pass filter. Conversely, the orientation estimates obtained
from the accelerometer and magnetometer measurements have desirable properties at low frequencies.
We would therefore like to filter these orientation estimates using a low-pass filter.
This can be illustrated by considering the one-dimensional case of estimating an angle θ from gyro-
scope measurements yω and magnetometer measurements ym . We denote the angle estimated by the
complementary filter by θ̂, the angle obtained from the magnetometer measurements by θm and the
angle obtained from the gyroscope measurements by θω . Note that the latter is obtained by integrating
the gyroscope measurements. The Laplace transforms of θ, θm , θω and yω are denoted by Θ(s), Θm (s),
Θω (s) and Yω (s), respectively. The complementary filter computes Θ(s) as
where G(s) is a low-pass filter and 1 − G(s) is hence a high-pass filter. Note that the sum of the two
filters, G(s) and 1 − G(s), is equal to one, which is the reason behind the name complementary filter.
1
Choosing G(s) as a first-order low-pass filter G(s) = as+1 and using Euler backward discretization, the
filter (4.57) can be written in discrete time as
θ̂t = (1 − γ)θm,t + γ θ̂t−1 + T yω,t , (4.58)
a
where γ = T +a . The relation (4.58) allows us to recursively estimate the angle θ̂. The only parameter that
needs to be chosen to run the complementary filter (4.58) is the parameter a of the low-pass filter G(s).
Choosing a large a (and hence a γ close to one) results in a lower cut-off frequency and a more significant
contribution of the gyroscope measurements on the orientation estimates. Conversely, a small a (and
hence a γ close to zero) results in a higher cut-off frequency and a more significant contribution of the
magnetometer measurements on the orientation estimates.
There is a strong relationship between complementary and Kalman filtering for linear models, see
e.g. [58, 17]. To highlight the similarities and differences between complementary and extended Kalman
filtering for orientation estimation, let us define the estimate from the complementary filter as θ̂t|t and
write the recursion (4.58) equivalently as
So far, we have discussed the one-dimensional case of estimating an angle θ. Let us now focus on
estimating orientation in three dimensions and parametrize this as a unit quaternion q nb . Two well-
known complementary filter implementations to estimate the orientation q nb are presented in [90] and
[89]. Open-source implementations of both algorithms are available online [155]. The filter presented
in [89] is specifically designed to be computationally efficient and has for instance been implemented
in the robot operating system (ROS) [146]. In this section, we will derive a complementary filter that
is inspired by this implementation but which is also meant to illustrate the similarities and differences
between complementary filters and extended Kalman filters for orientation estimation. Because of that,
the complementary filter that we will present is not as computationally efficient as the one in [89].
Similar to the one-dimensional case (4.59a), the orientation estimate of the complementary filter can
be updated using the gyroscope measurements as
nb nb
expq T2 yω,t ,
q̂t|t−1 = q̂t−1|t−1 (4.60)
42
where q̂ nb is the orientation estimate from the complementary filter and the double subscripts are defined
analogously to (4.59). Note that the update (4.60) is equivalent to the time update in the EKF in
Algorithm 3.
nb
The orientation from the accelerometer and magnetometer measurements, denoted Ram,t , can be
obtained by solving
nb bn
qam,t = arg min kȳa,t + qam,t ḡ n qam,t
nb
k2Σ−1 +
nb a
qam,t
bn
kȳm,t − qam,t m̄n qam,t
nb
k2Σ−1
m
nb
subj. to kqam,t k2 = 1, (4.61)
see also §3.6. Similar to [89], we do not completely solve the optimization problem (4.61) in each recursion
nb,(0) nb
of the complementary filter. Instead, we use an initial estimate Ram,t = q̂t|t−1 and only perform one
iteration of an optimization algorithm. In [89], one iteration of a gradient descent algorithm is performed.
We instead perform one Gauss-Newton update as in (4.9). Hence, using
bn −1/2
ya,t + R̂t|t−1 g n Σa
εt =
−1/2
, (4.62a)
bn
ym,t − R̂t|t−1 mn Σm
bn
∂Ram,t n −1/2
nb
∂qam,t nb
g Σ a
nb
Jt = ∂q∂ε
nb
t
=
qam,t =q̂t|t−1 ,
(4.62b)
bn
am,t ∂R
am,t n −1/2
− ∂qnb m Σ
m
am,t
nb =q̂ nb
qam,t t|t−1
nb
the estimate qam,t is given by
nb nb
−1
qam,t = q̂t|t−1 − β JtT Jt JtT εt . (4.63)
Analogously to (4.59b), the second update of the complementary filter is therefore given by
nb
nb
−1 T
q̂t|t = (1 − γ) q̂t|t−1 − β JtT Jt nb
Jt εt + γ q̂t|t−1
nb
−1 T
= q̂t|t−1 − (1 − γ)β JtT Jt Jt εt . (4.64)
There are many similarities between (4.64) and the measurement update in the EKF in Algorithm 3.
First of all, if we neglect the presence of Σa and Σm for a moment, Jt and εt in (4.62) are equal to −Ht
and εt in the measurement update in Algorithm 3. Inclusion of Σa and Σm in (4.62) ensures that the
uncertainty of the accelerometer and magnetometer measurements is properly taken into account when
nb nb
obtaining qam,t . The contributions to the orientation estimate q̂t|t from integration of the gyroscope
nb
measurements and from qam,t are determined by the factor (1 − γ)β. In the EKF on the other hand,
the uncertainty of the gyroscope, accelerometer and magnetometer measurements is taken into account
through the covariance matrices Σω , Σa and Σm .
Second of all, comparing (4.64) to the EKF measurement update (4.37), the update (4.64) can be
interpreted as a scaled version of an EKF update with Pt|t−1 = I4 and R = 03×3 . In that case, the
−1
matrix Kt in the EKF would be given by Kt = HtT Ht HtT . The matrix Ht HtT is rank deficient.
Because of this, to compute Kt , we would have to use the pseudo-inverse, see e.g. [46]. Denoting the
† −1 T
pseudo-inverse of a matrix as · † , Kt = HtT Ht HtT = HtT Ht Ht . An important difference between
the EKF and complementary update is that the scaling factor (1 − γ)β in (4.64) is constant, while the
matrix Pt|t−1 in the EKF is time-varying. In the remainder, we will denote this constant (1 − γ)β by α
for notational brevity.
Our complementary filter implementation is summarized in Algorithm 5. Note again the similarity
to Algorithm 3. To stress this similarity even more, we call the update (4.60) the time update of the
complementary filter and (4.64) the measurement update.
43
Algorithm 5 Orientation estimation using a complementary filter
Inputs: Inertial data {ya,t , yω,t }N N
t=1 , magnetometer data {ym,t }t=1 and covariance matrices Σa and Σm .
nb
Outputs: An estimate of the orientation q̂t|t for t = 1, . . . N .
end for
of the different algorithms will be analyzed. The simulated data allows for controlled analysis of the
workings of the algorithms. Furthermore, it allows us to compare the different algorithms using Monte
Carlo simulations. The experimental data shows the applicability to real-world scenarios. We will start
by introducing the data sets.
Experimental data is collected using the setup shown in Figure 4.2, where data is collected using
multiple mobile IMUs and smartphones. The algorithms presented in this section can be applied to
measurements from any of these devices. However, we focus our analysis on the data from the Trivisio
Colibri Wireless IMU [148]. In Figure 4.3, the inertial and magnetometer measurements from this
IMU are displayed for around 100 seconds during which the sensor is rotated around all three axes. The
experiments are performed in a lab equipped with multiple cameras [151], able to track the optical markers
shown in Figure 4.2. This provides highly accurate ground truth reference position and orientation
information, against which we can compare our estimates. For comparison, the optical and IMU data
need to be time-synchronized and aligned. We synchronize the data by correlating the norms of the
gyroscope measurements and of the angular velocity estimated by the optical system. Alignment is done
using the orientation estimates in combination with Theorem 4.2 from [59].
In Figure 4.4, simulated inertial and magnetometer measurements are displayed. The data represents
a sensor that is kept stationary for 100 samples, after which it is rotated around all three axes. The sensor
is assumed to be rotated around the origin of the accelerometer triad. Hence, during the entire data set,
the accelerometer is assumed to only measure the gravity vector. The magnitude of the simulated gravity
vector is 9.82 m/s2 . The magnitude of the simulated local magnetic field is equal to one. Its direction
is approximately equal to that in Linköping, Sweden, where a dip angle of 71◦ leads to a magnetic field
T
mn = 0.33 0 −0.95 . The simulated noise levels are
44
Optical marker IMU
Figure 4.2: Experimental setup where an IMU is used to collected inertial and magnetometer measure-
ments. Optical markers are tracked using multiple cameras, leading to accurate reference position and
orientation estimates. Note that the experimental setup also contains additional IMUs and smartphones.
The data from these sensors is not considered in this work.
10
ya,t [m/s2 ]
−10
0 20 40 60 80 100
Time [s]
0.5
yω,t [rad/s]
−0.5
0 20 40 60 80 100
Time [s]
1
ym,t [arbitrary units]
−1
0 20 40 60 80 100
Time [s]
Figure 4.3: Measurements from an accelerometer (ya,t , top), a gyroscope (yω,t , middle) and a magne-
tometer (ym,t , bottom) for 100 seconds of data collected with the IMU shown in Figure 4.2.
45
10
ya,t [m/s2 ]
−10
0 100 200 300 400
Sample [#]
0.1
0.05
yω,t [rad/s]
-0.05
0 100 200 300 400
Sample [#]
ym,t [arbitrary units]
−1
Figure 4.4: Simulated measurements from an accelerometer (ya,t , top), a gyroscope (yω,t , middle) and a
magnetometer (ym,t , bottom).
46
Note that we deliberately chose the noise levels to be fairly high, to clearly illustrate the workings of the
different algorithms.
Although our algorithms parametrize the orientation as quaternions, it is typically more intuitive to
visualize the orientation estimates in Euler angles. Hence, we visualize our results in terms of roll, pitch
and heading (yaw) angles. Both for the experimental data and for the simulated data, we are able to
compare our estimates q̂tnb to reference orientations denoted qref,t
nb
. To represent the orientation error, we
compute a difference quaternion ∆qt as
c
∆qt = q̂tnb qref,t
nb
, (4.68)
which can be converted to Euler angles for visualization. Note that using this definition, the orientation
errors in Euler angles can be interpreted as the errors in roll, pitch and heading.
Example 4.1 (Orientation estimation using inertial and magnetometer data) The orientation
errors from the smoothing optimization approach in Algorithm 1 using simulated inertial and magnetome-
ter measurements as illustrated in Figure 4.4 are depicted in the top plot of Figure 4.5. For comparison
we also show the orientation errors from dead-reckoning the gyroscope measurements in the bottom plot
(see also §1.2). These errors can be seen to drift over time.
Although the accelerometer and the magnetometer measurement noises are of equal magnitude, the
heading angle is estimated with less accuracy compared to the roll and pitch angles. The reason for this
is twofold. First, the signal to noise ratio for the magnetometer is worse than that of the accelerometer,
since the magnetometer signal has a magnitude of 1 while the accelerometer signal has a magnitude of
9.82 m/s2 . Second, only the horizontal component of the local magnetic field vector provides heading
information. This component is fairly small due to the large dip angle (71◦ ) in Linköping, Sweden.
The accelerometer provides inclination information, while the magnetometer provides heading infor-
mation, see §3.4. In case only inertial measurements and no magnetometer measurements are available,
the heading can only be estimated using the gyroscope measurements and will therefore drift over time.
This is illustrated in Example 4.2.
Example 4.2 (Orientation estimation using only inertial measurements) The orientation errors
from the smoothing optimization approach in Algorithm 1 using simulated inertial measurements as pre-
sented in Figure 4.4 can be found in Figure 4.6. The roll and pitch angles can be seen to be accurate,
while the heading angle drifts significantly. To obtain the results in Figure 4.6, we used data with the
same noise realization as in Example 4.1. Hence, we refer to Figure 4.5 for comparison to the orientation
errors from dead-reckoning the gyroscope measurements. The drift in the heading angle can be seen to
be similar to the drift from dead-reckoning of the gyroscope measurements.
The two examples above assume that our state space model (3.75) is an accurate description of the
measurements. In practice, however, this is not always the case, for instance due to the presence of
magnetic material in the vicinity of the sensor. In Example 4.3 we illustrate that if the state space model
does not accurately describe the data, it is not possible to obtain accurate orientation estimates.
Example 4.3 (Orientation estimation in the presence of magnetic material) We simulate 400
samples of stationary data. Between samples 150 and 250, we simulate the presence of a magnetic ma-
T
terial, causing a change in the magnetic field of 0.1 0.3 0.5 . In Figure 4.7, we show the adapted
47
10
Orientation error [◦ ]
0
−10
−20
−30
0 100 200 300 400
Sample [#]
10
Orientation error [◦ ]
−10
−20
−30
0 100 200 300 400
Sample [#]
Figure 4.5: Orientation errors in roll (blue), pitch (green) and heading (red) using simulated measure-
ments for (top) Algorithm 1 using inertial and magnetometer measurements and (bottom) dead-reckoning
of the gyroscope measurements.
magnetometer data and the orientation estimates using the smoothing optimization approach from §4.1.
As can be seen, the heading estimates show significant errors when the magnetic material is present.
Depending on the amount of disturbance and the uncertainty in the inertial and magnetometer measure-
ments, magnetic material can also result in errors in the roll and pitch estimates.
48
10
Orientation error [◦ ]
0
−10
−20
−30
0 100 200 300 400
Sample [#]
Figure 4.6: Orientation errors in roll (blue), pitch (green) and heading (red) using simulated measure-
ments for Algorithm 1 using inertial measurements only.
The resulting estimates and bounds are visualized in terms of Euler angles in Figure 4.8 for simulated
data similar to the data presented in Figure 4.4. The orientation and its covariance are estimated using
Algorithm 4. As can be seen, the bounds are difficult to interpret due to the wrapping of the Euler
angles.
As argued in [38], it is more intuitive to directly represent the uncertainty in terms of orientation
deviations. The covariance cov(η̂tn ) can be interpreted as the uncertainty in the roll, pitch and heading
angles as illustrated in Example 4.4.
Example 4.4 (Orientation estimation using only inertial measurements (continued)) Since the
accelerometer provides only inclination information, in the case of Example 4.2 where magnetometer mea-
surements are unavailable, we expect only the roll and pitch angles to be estimated with small uncertainty.
In fact, we expect the uncertainty of the heading at t = 1 to be equal to the uncertainty of the initial Σi
from §3.6 and to steadily grow over time, depending on the amount of gyroscope noise. In Figure 4.9,
we plot the standard deviation σ of the orientation estimates computed using the smoothing algorithm
from §4.1 as the square root of the diagonal elements of cov(η̂tn ). As can be seen, the standard deviation
of the yaw angle at t = 1 is indeed 20◦ as modeled in §3.6. The increase in the uncertainty in the yaw
angle exactly matches the increase of the uncertainty due to dead-reckoning.
From Example 4.4 it can be concluded that cov(η̂tn ) seems to be an intuitive measure of the uncertainty
of the orientation estimates. The covariance cov(q̂tnb ) computed by Algorithm 3 relates to cov(η̂tn ) as
η̂ n
cov(q̂tnb ) = cov expq ( 2t ) q̃tnb
R ∂ expq (η̂tn ) ∂ expq (η̂tn ) T
R
= 14 q̃tnb n
∂ η̂t cov(η̂t
n
) n
∂ η̂t q̃tbn , (4.71a)
cov(η̂tn ) = cov 2 logq (q̂tnb q̃tbn )
The relation (4.71) allows us to compare the orientation estimates and covariances from the different
algorithms in more detail in Example 4.5.
Example 4.5 (Orientation estimation using only inertial measurements (continued)) As dis-
cussed in Example 4.4, using only inertial measurements and no magnetometer measurements, we can
only expect to be able to accurately estimate the inclination. The uncertainty of the heading estimates
grows over time. We will now analyze the behavior of the different algorithms in more detail for this
specific example. In Table 4.1, we show the root mean square error (RMSE) values over 100 Monte
Carlo simulations for Algorithms 1–4. In Figure 4.10, we also represent the orientation estimates from
the four algorithms for one of these realizations. As can be seen from both Table 4.1 and Figure 4.10,
as expected, the smoothing algorithm outperforms the other algorithms. However, more surprisingly, the
EKF with quaternion states has much larger errors in the heading angle. In Figure 4.11, we also show
49
ym,t [arbitrary units]
0
−1
30
Orientation error [◦ ]
20
10
Figure 4.7: Top: Simulated magnetometer measurements ym,t for 400 samples of stationary data. Be-
tween samples 150 and 250 we simulate the presence of a magnetic material in the vicinity of the sensor.
Bottom: Orientation estimates in roll (blue), pitch (green) and heading (red) using the simulated inertial
and magnetometer measurements.
Table 4.1: Mean RMSE values over 100 Monte Carlo simulations estimating orientation using only
inertial measurements.
the standard deviations of the estimates from all algorithms. As can be seen, the EKF with quaternion
states over-estimates its confidence in the estimates of the heading direction. This can most likely be
attributed to linearization issues.
From Example 4.5, it can be concluded that not properly estimating the covariance can have negative
effects on the quality of the estimates. It can also be concluded from this section that covariances can
best be represented in terms of cov(η̂tn ) but that they are difficult to visualize in Euler angles. Because
of that, in the remainder of this section, we will typically plot the uncertainty in a separate plot like in
Figure 4.11.
50
180
90
Roll [◦ ]
0
−90
−180
0 100 200 300 400
Sample [#]
90
Pitch [◦ ]
−90
0 100 200 300 400
Sample [#]
180
90
Heading [◦ ]
−90
−180
0 100 200 300 400
Sample [#]
Figure 4.8: Orientation estimates (solid) and 3σ bounds (dashed) in roll (blue), pitch (green) and heading
(red) using inertial and magnetometer measurements.
20
σ[◦ ]
10
0
0 100 200 300 400
Sample [#]
Figure 4.9: Standard deviation σ in degrees of the orientation estimates in roll (blue), pitch (green) and
heading (red) using only inertial measurements.
51
Orientation error [◦ ]
Orientation error [◦ ]
0 0
−10 −10
−20 −20
−30 −30
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(a) Smoothing optimization (Alg. 1). (b) Filtering optimization (Alg. 2).
Orientation error [◦ ]
Orientation error [◦ ]
0 0
−10 −10
−20 −20
−30 −30
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(c) EKF quaternions (Alg. 3) (d) EKF orientation deviation (Alg. 4).
Figure 4.10: Orientation estimates of Algorithms 1–4 in roll (blue), pitch (green) and heading (red) using
only inertial measurements.
Table 4.2: RMSE of the orientation estimates obtained using Algorithms 1–5 and the experimental data
presented in Figure 4.3.
approximately true. Because of this, we choose σa = 2.6 · 10−1 and σm = 2.5 · 10−1 . These values are a
factor 10 respectively 100 larger than the experimentally determined noise values. Note that this choice
is highly dependent on the data set. The RMSE values as compared to the optical reference system
for the different methods described in this chapter are summarized in Table 4.2. A good value for α
in the complementary filter is experimentally determined to be 0.001 for this specific data set. As an
illustration of the estimates, the orientation estimates as obtained using the smoothing algorithm and
the orientations from the optical reference system are shown in Figure 4.12.
It is of course difficult to draw quantitative conclusions based on only one data set. The RMSE
values in Table 4.2 should therefore mainly be seen as an indication of the performance. Comparing the
different algorithms amongst each other is hard. In fact, the algorithms perform differently with respect
to each other for different choices of the covariance matrices. Because of this, we will study the accuracy
of the different methods using simulated data instead.
We run 100 Monte Carlo simulations where the simulated data illustrated in Figure 4.4 is generated
with different noise realizations. Table 4.3 shows the mean RMSE for the five estimation algorithms.
Algorithms 1–4 use the noise covariance matrices that are also used to generate the data. The comple-
mentary filter also uses these to determine the orientation from the accelerometer and magnetometer
data. To combine this with the orientation from the gyroscope measurements, we need to determine
a good value for α. We empirically choose a value that leads to good performance for this data set.
The orientation from the accelerometer and magnetometer measurements can be expected to be more
accurate in the roll and the pitch than in the heading, see Example 4.1. Since α is scalar, however,
52
20 20
σ[◦ ]
σ[◦ ]
10 10
0 0
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(a) Smoothing optimization (Alg. 1). (b) Filtering optimization (Alg. 2).
20 20
σ[◦ ]
σ[◦ ]
10 10
0 0
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(c) EKF quaternions (Alg. 3). (d) EKF orientation deviation (Alg. 4).
Figure 4.11: Standard deviation σ in degrees of the orientation estimates of Algorithms 1–4 in roll (blue),
pitch (green) and heading (red) using only inertial measurements.
Table 4.3: Mean RMSE of the orientation estimates from 100 Monte Carlo simulations using Algo-
rithms 1–5.
it is not possible to weigh these contributions differently. Because of this, we present results both for
α = 0.07, which leads to small RMSE values in the heading but relatively larger RMSE in the roll and
the pitch, and for α = 0.7, which leads to small RMSE values in the roll and pitch but large RMSE in
the heading.
From the results summarized in Table 4.3, it can be seen that the smoothing approach outperforms
the filtering approaches. The estimates for one of the noise realizations are shown in Figure 4.13. For
Algorithms 1–4, we also depict the covariance estimates in Figure 4.14. The filtering approaches from
Algorithms 2–4 estimate the standard deviation of the orientation errors at t = 1 to be equal to 20◦ .
After this, they can be seen to converge to around 3.16◦ degrees for the heading angle and 0.46◦ for
roll and pitch angles. The smoothing algorithm estimates an uncertainty in the heading angle of around
3.17◦ for the first and last sample, while converging to a standard deviation of 2.25◦ for the middle of
the data set. For the roll and pitch angles, the initial and final uncertainties are estimated to be around
0.73◦ , converging to 0.39◦ for the middle of the data set. Note that these values correspond fairly well
with the RMSE values in Table 4.3.
For the Monte Carlo simulations described above, the three filtering algorithms perform similarly.
However, differences can be seen when an update of the filter needs to correct the orientation estimates
significantly. Examples for when this happens are when the initial orientation is not accurately known
or when magnetometer measurements are not available for a longer period of time. In these cases, the
uncertainty of the state is large and large corrections to the state estimates are needed when measure-
ments become available. To analyze this case in more detail, we assume that the estimate of the initial
orientation q̆1nb is normal distributed around the true initial orientation with a standard deviation of
20◦ . Hence, we do not use the first accelerometer and magnetometer data for initialization. Note that a
53
200
100
Roll [◦ ]
0
−100
−200
0 20 40 60 80 100
Time [s]
100
50
Pitch [◦ ]
−50
−100
0 20 40 60 80 100
Time [s]
200
100
Heading [◦ ]
−100
−200
0 20 40 60 80 100
Time [s]
Figure 4.12: Red: Orientation from the optical reference system. Blue: Orientation estimates obtained
using Algorithm 1 for the experimental data from Figure 4.3.
Table 4.4: Mean RMSE of the orientation estimates from 100 Monte Carlo simulations. The estimate
of the initial orientation is assumed to be normal distributed around the true initial orientation with a
standard deviation of 20◦ .
54
Orientation error [◦ ]
Orientation error [◦ ]
10 10
0 0
−10 −10
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(a) Smoothing optimization (Alg. 1). (b) Filtering optimization (Alg. 2).
Orientation error [◦ ]
Orientation error [◦ ]
10 10
0 0
−10 −10
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(c) EKF quaternions (Alg. 3). (d) EKF orientation deviation (Alg. 4).
Orientation error [◦ ]
Orientation error [◦ ]
40
10
20
0 0
−20
−10
−40
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(e) Complementary filter (Alg. 5), α = (f) Complementary filter (Alg. 5), α =
0.07. 0.7.
Figure 4.13: Orientation errors of Algorithms 1–5 in roll (blue), pitch (green) and heading (red) using
simulated inertial and magnetometer measurements.
55
3 20
σ[◦ ]
σ[◦ ]
10
1
0
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(a) Smoothing optimization (Alg. 1). (b) Filtering optimization (Alg. 2).
20 20
σ[◦ ]
σ[◦ ]
10 10
0 0
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(c) EKF quaternions (Alg. 3). (d) EKF orientation deviation (Alg. 4).
Figure 4.14: Standard deviation σ in degrees of the orientation estimates of Algorithms 1–4 in roll
(blue), pitch (green) and heading (red) using simulated inertial and magnetometer measurements. Note
the different scale on the vertical axis of (a) as compared to (b)–(d).
standard deviation of 20◦ is equal to the uncertainty on the initial state assumed by the algorithms. The
results for 100 Monte Carlo simulations are summarized in Table 4.4. As can be seen, specifically the
EKF with quaternion states and the complementary filter with α = 0.07 perform worse than Algorithm 2
and Algorithm 4 for this data.
Which algorithm to use is highly application-specific. However, in general it can be concluded that all
five algorithms actually produce fairly good orientation estimates, assuming that the models from §3.7
are indeed valid. The smoothing algorithm performs better than the filtering approaches but it is also the
most computationally expensive. The EKF with quaternion states and the complementary filter suffer
from linearization issues when large orientation corrections need to be made or when magnetometer data
is unavailable.
56
T
with xt = pT
t vtT (ηtn )T and
In this section, we will discuss some details about the workings of the pose estimation algorithm using this
model. We will not go through a complete derivation of the four algorithms. However, the adaptations
that are needed to use Algorithms 1–4 for pose estimation can be found in Appendix B.
An important observation is that ea,p,t and ea,v,t in (4.73d) and (4.73e) depend on the orientation Rtnb .
Because of this, the position, velocity and orientation states are coupled. The position measurements
therefore do not only provide information about the position and velocity, but also about the orientation
of the sensor. This is the reason why it is no longer essential to include magnetometer data and to assume
that the acceleration is approximately zero. However, the accuracy of the orientation estimates depends
on the movements of the sensor. This will be illustrated below. For this, we simulate 400 samples of
inertial and position measurements for a non-rotating sensor with noise levels
We consider four different sensor motions. The results in this section are based on the solution to the
smoothing optimization problem (4.72). First, we simulate data assuming that the sensor is stationary.
For this case, the position measurements provide information about the inclination of the sensor, but
not about its heading. This is illustrated in Example 4.6.
Example 4.6 (Pose estimation for a stationary sensor) We estimate the pose of a stationary sen-
sor using simulated data and a smoothing algorithm that solves (4.72) as described in §4.1. The orien-
tation error for a specific noise realization is depicted in Figure 4.15(a). The inclination errors can be
seen to be small, while the heading estimates drift.
Next, in Example 4.7, we consider the case where the sensor has a constant linear acceleration. For
this case, a drift in the orientation estimates can be seen in the direction that is orthogonal to the
direction of the accelerometer measurements.
Example 4.7 (Pose estimation for a sensor with constant linear acceleration) We estimate the
pose of a sensor with an acceleration of 1 m/s2 in the y-direction using simulated data and obtain
smoothed estimates by solving (4.72). The orientation error for a specific noise realization is depicted in
Figure 4.15(b). Again, a drift can be seen in the orientation estimates. This drift is no longer only in
the heading direction, but there is also a small drift on the roll angle.
Finally, in Example 4.8 we consider the case of a time-varying linear acceleration. Based on simulated
data, we show that accurate heading estimates can be obtained for this case. Furthermore, we show that
the larger the acceleration, the more accurate the heading estimates will be.
Example 4.8 (Pose estimation for a sensor with time-varying linear acceleration) We estimate
the pose of a sensor with an acceleration in the y-direction of (ya,t )y ∼ N (0, 0.5) m/s2 using simulated
57
Orientation error [◦ ]
Orientation error [◦ ]
10 10
5 5
0 0
−5 −5
−10 −10
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(a) Stationary. (b) Constant acceleration.
Orientation error [◦ ]
Orientation error [◦ ]
10 10
5 5
0 0
−5 −5
−10 −10
0 100 200 300 400 0 100 200 300 400
Sample [#] Sample [#]
(c) Acceleration (ya,t )y ∼ N (0, 0.5). (d) Acceleration (ya,t )y ∼ N (0, 5).
Figure 4.15: Orientation errors of the roll (blue), pitch (green) and heading (red) using simulated inertial
and magnetometer measurements.
Table 4.5: Mean RMSE of the position and orientation estimates from 100 Monte Carlo simulations.
Considered are a stationary sensor, a sensor with constant acceleration and two cases of time-varying
accelerations with different magnitudes.
data and compute smoothed estimates by solving (4.72). The orientation error for a specific noise real-
ization is depicted in Figure 4.15(c). Furthermore, we simulate data with (ya,t )y ∼ N (0, 5) m/s2 . The
orientation errors based on this data can be found in Figure 4.15(d). As can be seen, for these cases, it is
possible obtain reliable heading estimates using the state space model (3.74). The larger the acceleration,
the more accurate the heading estimates.
In general, it can be concluded that it is possible to estimate position and orientation using the state
space model (3.74). Except in the cases of constant or zero acceleration, it is possible to obtain drift-free
orientation estimates. The heading accuracy depends on the amount of acceleration. This is summarized
in Table 4.5 where the mean RMSE of the state estimates over 100 Monte Carlo simulations is shown.
Four cases were considered, inspired by Examples 4.6–4.8.
58
Chapter 5
Calibration
In Chapter 4, we assumed that the sensors were properly calibrated. In practice, however, there are
often calibration parameters to be taken into account. Examples of calibration parameters are the
inertial sensor biases discussed in Chapter 2. Furthermore, calibration is specifically of concern when
combining the inertial data with other sensors. In these cases, it is important that the inertial sensor axes
and the axes of the additional sensors are aligned. Examples include using inertial sensors in combination
with magnetometers [75, 126, 15] and with cameras [60, 83, 98].
In this chapter we will introduce several useful calibration methods. In §5.1 we explain how calibration
parameters can be included as unknowns in the smoothing and filtering algorithms from §4.1–§4.3. This
results in MAP estimates of the parameters. In §5.2 we instead focus on obtaining maximum likelihood
(ML) estimates of the parameters. In §5.3, the workings of the calibration algorithms are illustrated by
considering the gyroscope bias to be unknown in the orientation estimation problem. Finally, in §5.4, we
discuss the topic of identifiability. Parameters are said to be identifiable if they can be estimated from
the available data.
with
N
Y
p(x1:N , θ | y1:N ) ∝ p(θ)p(x1 ) p(xt | xt−1 , θ)p(yt | xt , θ). (5.2)
t=1
Recall that a discussion on the choice of the prior of the parameters p(θ) and the states p(x1 ) can be
found in §3.6.
Within the filtering context we typically model the parameters as slowly time-varying states. These
can be estimated by solving
n o
x̂t , θ̂t = arg max p(xt , θt | y1:t ), (5.3)
xt ,θt
where
and
p(xt , θt | y1:t−1 ) =
ZZ
p(xt , θt | xt−1 , θt−1 )p(xt−1 , θt−1 | y1:t−1 )dxt−1 dθt−1 . (5.4b)
59
Note that compared to §3.1, in (5.4) we do not consider the parameters to be part of xt but instead
represent them explicitly. A prior p(θ1 ) on the parameters at t = 1 has to be included as well as a
dynamic model of the parameters.
Both the formulations (5.1) and (5.3) compute MAP estimates of the parameters. Algorithms 1–4
presented in Chapter 4 can straightforwardly be extended to also estimate these unknown parameters θ
or θ1:N . This is illustrated in Example 5.1 for the case of orientation estimation in the presence of an
unknown gyroscope bias. Note that Algorithm 5 can also be extended to estimate an unknown gyroscope
bias. We will, however, not consider this. Instead, we refer the reader to [90].
Example 5.1 (MAP estimates of the gyroscope bias) It is possible to estimate an unknown gyro-
scope bias in the state space model (3.75). For this, the dynamic model (3.75a) in the smoothing problem
described in §4.1 is assumed to include a constant gyroscope bias δω as
nb
= qtnb expq T2 (yω,t − δω − eω,t ) .
qt+1 (5.5a)
In the filtering algorithms in §4.2 and §4.3, the dynamic model is instead assumed to include a slowly
time-varying gyroscope bias δω,t as
nb
= qtnb expq T2 (yω,t − δω,t − eω,t ) ,
qt+1 (5.5b)
where the dynamics of the gyroscope bias can be described as a random walk (see also §3.5)
The smoothing algorithm presented in §4.1 can be extended to also estimate δω . Furthermore, the fil-
tering algorithms presented in §4.2 and §4.3 can be extended to estimate δω,t for t = 1, . . . , N . Only
minor changes to the algorithms presented in these sections are needed. These mainly concern including
derivatives with respect to the additional unknowns δω or δω,t . Explicit expressions for these can be found
in Appendix C.
Here, Θ ⊆ Rnθ and L(θ; y1:N ) is referred to as the likelihood function. It is defined as L(θ; y1:N ) ,
pθ (Y1:N = y1:N ), where Y1:N are random variables and y1:N are a particular realization of Y1:N . Using
conditional probabilities and the fact that the logarithm is a monotonic function we have the following
equivalent formulation of (5.6),
N
X
θ̂ml = arg min − log pθ (Yt = yt | Y1:t−1 = y1:t−1 ), (5.7)
θ∈Θ t=1
where we use the convention that y1:0 , ∅. The ML estimator (5.7) enjoys well-understood theoretical
properties including strong consistency, asymptotic normality, and asymptotic efficiency [82].
Due to the nonlinear nature of the orientation parametrization, our estimation problems are nonlinear,
implying that there is no closed form solution available for the one step ahead predictor pθ (Yt = yt |
Y1:t−1 = y1:t−1 ) in (5.7). However, similar to the filtering approaches from Chapter 4, it is possible to
approximate the one step ahead predictor according to
pθ (Yt = yt | Y1:t−1 = y1:t−1 ) ≈ N yt ; ŷt|t−1 (θ), St (θ) , (5.8)
where ŷt|t−1 (θ) and St (θ) are defined in (4.39) and (4.38), respectively. Inserting (5.8) into (5.7) and
neglecting all constants not depending on θ results in the following optimization problem,
N
1X
θ̂ = arg min kyt − ŷt|t−1 (θ)k2S −1 (θ) + log det St (θ). (5.9)
θ∈Θ 2 t=1 t
60
Table 5.1: Mean RMSE of the orientation estimates from 100 Monte Carlo simulations in the presence
of a gyroscope bias that is being estimated.
Unlike the optimization problems discussed so far, it is not straightforward to obtain an analytical
expression of the gradient of (5.9). This is because it is defined recursively through the filtering update
equations. In [3] and [134], different approaches to derive analytical expressions for objective functions of
the same type as (5.9) are provided. They, however, consider the case of a linear model. Some methods
for obtaining ML estimates of parameters in nonlinear models are explained in the tutorial by [131].
Instead of deriving analytical expressions for the gradient of (5.9), it is also possible to compute
a numerical approximation of the gradient. Numerical gradients can be used in a number of different
optimization algorithms, such as the Broyden-Fletcher-Goldfarb-Shanno (BFGS) method, see e.g. [103].
Similar to the Gauss-Newton method, in the BFGS method, the parameters are iteratively updated until
convergence. However, instead of using the Hessian approximation (4.8), BFGS iteratively estimates the
Hessian using information from previous iterations. Hence, solving (5.9) using BFGS with numerical
gradients, requires running at least nθ + 1 filtering algorithms for each iteration. These are required
to evaluate the objective function and to compute the numerical gradients. More evaluations can be
necessary to compute a step length, see also §4.1.
Example 5.2 (ML estimates of the gyroscope bias) To obtain ML estimates of the gyroscope bias,
we run the EKF with orientation deviation states from Algorithm 4 to obtain ŷt|t−1 (δω ) and St (δω ) for
a given value of δω . This allows us to evaluate the objective function in (5.9). To compute δ̂ω , the
optimization problem (5.9) is solved iteratively using BFGS.
Note that σδω is a factor 10 larger than the value discussed in §3.6 to clearly illustrate the effect of
the presence of a gyroscope bias. The priors p(θ) and p(θ1 ) in the smoothing and filtering algorithms
are set equal to the distribution in (5.10). The covariance of the random walk model (5.5c) is set as
Σδω,t = σδ2ω,t I3 with σδω,t = 1 · 10−10 . This small value ensures that after convergence, the bias estimate
is quite constant. The resulting mean RMSEs of the orientation over 100 Monte Carlo simulations are
summarized in Table 5.1. Since the filtering algorithms typically have similar characteristics as discussed
in §4.5, we only consider the EKF with orientation deviation states here. Comparing these results to the
ones presented in Table 4.3, the RMSEs of the smoothing optimization algorithm are almost the same
as when there was no gyroscope bias present. However, the filtering results are worse. This is because
the bias needs some time to be properly estimated. This is illustrated in Figure 5.1 where the gyroscope
bias estimates and their uncertainties are shown for the filtering algorithm.
A major difference between the MAP and the ML approaches, is that the MAP takes into account
a prior on the gyroscope bias. We analyze the effect of this prior using 500 Monte Carlo simulations,
T
simulating the gyroscope bias to be δω = 0.05 0.01 −0.04 rad/s. We study the estimated gyroscope
61
0.2
δω,t [rad/s]
0
−0.2
50 100
Sample [#]
Figure 5.1: Filtering estimates of the gyroscope bias and their 3σ confidence intervals with the x-, y-
and z-components in blue, green and red, respectively. Note that the estimates for only the first 100
samples are shown, to focus on the period in which the estimates converge.
biases using ML estimation, and using MAP estimation by including the gyroscope bias as an unknown
in the smoothing algorithm. The smoothing algorithm assumes two different priors on the gyroscope
bias δω ∼ N (0, σδ2ω I3 ). In the first case, the prior on the gyroscope bias can well describe the data
(σδω = 0.05). In the other case, the prior is too tight (σδω = 1 · 10−3 ). The mean and standard
deviations for the gyroscope bias estimates are summarized in Table 5.2. As can be seen, when the prior
is chosen appropriately, the ML and MAP estimates are comparable. If the prior is too tight, the MAP
estimates can be seen to be biased towards zero.
Table 5.2: Mean and standard deviation of the gyroscope estimates over 500 Monte Carlo simulations
T
with (0.05 0.01 − 0.04) rad/s. Considered are the cases of ML estimation and MAP estimation by
including the gyroscope bias as an unknown in a smoothing algorithm with a prior on the gyroscope bias
of δω ∼ N (0, σδ2ω I3 ).
5.4 Identifiability
Parameters are said to be identifiable if it is possible to determine a unique parameter value from the
data and if this value is equal to the true value [82]. The concept of identifiability is closely related
to the concept of observability which is concerned with the question of if the time-varying states can
be determined from the available data [69]. The states discussed in Chapter 4 are typically observable.
Identifiability, however, becomes of concern when estimating calibration parameters. Specifically, in
many applications, certain parameters are not identifiable when the sensor is completely stationary and
sufficient excitation in terms of change in position and orientation is needed to make the parameters
identifiable. This is illustrated in Example 5.3 for the case of identifiability of the gyroscope bias.
Example 5.3 (Identifiability of the gyroscope bias) We consider the example of orientation es-
timation using only inertial measurements in the presence of a gyroscope bias. We simulate data as
described in §4.5. The filtering estimates of the gyroscope bias and their uncertainties from an EKF with
orientation deviation states are shown in Figure 5.2. Using only inertial measurements, the gyroscope
bias of the sensor’s z-axis is not identifiable when the sensor is placed horizontally. However, when the
sensor is rotated, the accelerometer provides information that aids the estimation and the bias can be
seen to converge. Note the difference with Figure 5.1, where only the first 100 samples were displayed
62
0.2
δω,t [rad/s]
0
−0.2
0 100 200 300 400
Sample [#]
Figure 5.2: Filtering estimates of the gyroscope bias and their 3σ confidence intervals with the x-, y-
and z-components in blue, green and red, respectively. The estimates are obtained using an EKF and
inertial (but no magnetometer) measurements. As can be seen, the estimates of the gyroscope bias in
the x- and y-axes converge quickly while the estimates of the gyroscope bias in the z-axis only start to
converge after 100 samples, when the sensor starts to rotate around the x-axis.
and the bias estimates in the z-axis converged significantly faster due to the inclusion of magnetometer
measurements.
63
Chapter 6
In this tutorial, we have presented a number of algorithms and modeling possibilities for position and
orientation estimation using inertial sensors. The algorithms derived in Chapter 4 are based on the simple
state space models (3.74) and (3.75). These models assume the availability of inertial measurements
supplemented with either magnetometer or position measurements. In this chapter, we will illustrate
how the same or similar algorithms can be used when additional and/or different supplementary sensors
are available and discuss a number of possible extensions. The models used in this chapter will be more
complex, but the information provided by the inertial sensors will remain one of the basic building blocks.
We are not aiming for a full coverage, since that would require too much space. Instead we provide a
number of case studies to illustrate what can be done when it comes to including the developments from
Chapters 1–5 into a more complex setting. The case studies are the modeling of a non-Gaussian noise
distribution to make use of radio-based time of arrival measurements (§6.1), the fusion of information
from inertial sensors with cameras (§6.2), and motion capture based on information from multiple inertial
sensors in combination with information about the human motion (§6.3 and §6.4).
UWB receiver
Figure 6.1: A UWB setup consisting of a number of stationary receivers obtaining time of arrival mea-
surements of signal pulses originating from a mobile transmitter.
64
0.4 5
Negative log-likelihood
0.3 4
0.2 3
0.1 2
0 1
−2 0 2 −2 0 2
eu,mk eu,mk
Figure 6.2: Probability density function (left) and negative log-likelihood (right) of a N (0, 1) distribu-
tion (blue, dashed), a Cauchy(0, 1) distribution (green, dotted) and the asymmetric distribution (6.2)
assuming σ = γ = 1 (red).
Here, c denotes the speed of light, tk is the position of the transmitter at the time of transmitting the
k th pulse, rm is the position of the mth receiver and ∆τm is its clock-offset. The time when pulse k is
transmitted is denoted by τk . Finally, eu,mk is measurement noise.
Using time of arrival measurements from a number of receivers it is possible to compute the position of
the transmitter at the time when the pulse was sent. In many applications, the mobile transmitters have
a much less accurate clock than the stationary receivers and τk is therefore assumed to be an unknown
variable to be estimated. For a setup with M stationary transmitters, the position of the transmitter
when transmitting the pulse k and the time when the pulse was transmitted can be estimated using
multilateration [22, 43, 130, 125].
Due to non-line-of-sight conditions and/or multipath we expect a small number of measurements to
be delayed. In [74], we present an approach that models the measurement noise eu,mk using a tailored
asymmetric heavy-tailed distribution. Here, a heavy-tailed Cauchy distribution allows for measurement
delays, while a Gaussian distribution excludes the physically unreasonable possibility of pulses traveling
faster than the speed of light as
(2 − α) N (0, σ 2 )
for eu,mk < 0, (6.2a)
eu,mk ∼
α Cauchy(0, γ) for eu,mk ≥ 0. (6.2b)
The presence of the constants α and 2 − α is motivated by the fact that the proposed asymmetric
probability density function needs to integrate to one. The parameter α is defined as a function of σ and
γ. The proposed asymmetric probability density function and its corresponding negative log-likelihood,
given by
LG for eu,mk < 0, (6.3a)
− log p (eu,mk ) =
LC for eu,mk ≥ 0, (6.3b)
e2u,mk 1 2 1
LG , 2σ 2 +2 log σ + 2 log 2π − log (2 − α) ,
e2
LC , log 1 + u,mk
γ2 + 12 log γ 2 + log π − log α,
are both depicted in Figure 6.2 in red. For comparison, the Gaussian and Cauchy probability density
functions are also depicted, in blue and green, respectively. Related studies have also modeled this
presence of delayed measurements using skew-t distributions [104, 99], Gaussian mixture models [100] or
by introducing a number of unknown delays [59].
The filtering and smoothing algorithms posed as optimization problems, see §4.1 and §4.2, can
straightforwardly be adapted to include the asymmetric probability density function by using (6.3) in
65
l
image plane
l
f plc,z
f
plc,x iu
pinhole i
i
(a) Illustration of the pinhole camera model (b) 2D visualization of the relation between
where an object l results in an image i in the the position of the point l with respect to
image plane. The image plane lies a distance the pinhole and the position of its image i.
f behind the pinhole.
Figure 6.3: Illustrations of the relation between a point or object l in the environment and its image i.
the objective function instead of the weighted least squares term representing the measurement model,
see e.g. (4.72). Extended Kalman filters intrinsically assume that the process and measurement noise are
Gaussian. For the extended Kalman filters implementations presented in §4.3.1 and §4.3.2, the extension
to non-Gaussian noise is therefore less straightforward. Adaptations of the Kalman filter to non-Gaussian
noise are, however, possible, see e.g. [122] and the references therein for a discussion on Kalman filtering
in the presence of student’s t distributed noise.
Here, the superscript c indicates that the position plc is expressed in the camera frame c. The origin of
the camera frame is located at the pinhole and its axes are aligned with the image plane. It is possible
to express the position plc in terms of the position of the camera and the position of the object l as
pclc,t = Rtcn pnl − pnc,t .
(6.5)
Here, pnc,t denotes the position of the camera at time t expressed in the navigation frame. The rotation
from the navigation frame n to the camera frame c at time t is denoted by Rtcn . Finally, the position
of the point l in the navigation frame is denoted by pnl . Note that the objects in the environment are
assumed to be stationary, i.e. pnl does not change over time. Assuming that the position of the object
pnl is known, using (6.4) and (6.5), the image i provides information about the position pnc,t and the
orientation Rtcn of the camera.
66
To be able to combine the information from the camera images with inertial measurements, it is
possible to express the position pc,t and the orientation Rtcn of the camera in terms of the position pnt
and the orientation Rtbn of the body frame as
Here, the distance between the body frame and the camera frame is denoted by dcb and the rotation
between these two frames is denoted by Rcb . Both are assumed to be constant and can be determined
using dedicated calibration algorithms [60, 83, 72, 98, 116]. The relation (6.5) can therefore be expressed
in terms of Rtbn and pnt as
Using (6.4), the measurement model (3.74b) can now be replaced with measurements yc,t from the
camera as
T
i pc pc
yc,t = u,t = f pclc,z,t pclc,z,t + ec,t , (6.8)
iv,t lc,x,t lc,y,t
where ec,t is measurement noise and pclc,t is given by (6.7). Including this measurement model in (3.74)
allows us to combine inertial sensors with camera images to estimate the position and orientation of the
sensor.
Note that at each time instance there can be multiple measurements from the camera image. One
version of this adapted state space model estimates the position and orientation of the sensor assuming
that the position of the objects pnl in the environment is known and constant. Alternatively, the positions
pnl for objects l = 1, . . . , L are considered to be unknown and part of the state vector. The latter is
called simultaneous localization and mapping (SLAM), where a map of the environment is built from
measurements of a mobile sensor whose position is unknown and to be estimated. There exist many
algorithms for doing SLAM, both using EKF and optimization-based approaches, see e.g. the tutorial
papers [32, 5] or [20] for a more recent overview.
Placement of the sensors on the body segments. The position and orientation of each sensor can
be expressed in terms of its position and orientation on the corresponding body segment. Ideally,
the sensor would be rigidly attached to the segment. However, it is physically impossible to place
the sensor directly on the bone. Since it needs to be placed on the soft tissue instead, the sensor will
inevitably move slightly with respect to the bone. We therefore model the position and orientation
of each sensor on its corresponding body segment as approximately constant.
67
Figure 6.4: Left: Olympic and world champion speed skating Ireen Wüst wearing sensors on her body.
Right: graphical representation of the estimated orientation and position of her body segments.
Joints between the body segments. A number of equality constraints enforce the body segments to
be connected at the joint locations at all times. Note that equality constraints can straightforwardly
be included in optimization problems [16, 103].
Exclusion of magnetometer measurements. The magnetic field in indoor environments is not con-
stant, see e.g. [80]. This is specifically of concern in motion capture applications, since the magnetic
field measured at the different sensor locations is typically different [87, 25, 35, 133]. Because of
this, we do not include magnetometer measurements in the model. The inclusion of the equality
constraints enforcing the body segments to be connected allows us to do so, since incorporating
these constraints, the sensor’s relative position and orientation become observable as long as the
subject is not standing completely still [59].
As before, we denote the time-varying states by xt and the constant parameters by θ. However, to
highlight the different parts of the model, we split the states xt in states xSt i pertaining to the sensor Si
and states xtBi pertaining to the body segment Bi , for i = 1, . . . , NS . Here, NS is the number of sensors
attached to the body and it is assumed to be equal to the number of body segments that are modeled.
Using this notation, the optimization problem that is solved in [73] can be summarized as
NT X
X NS NT X
X NS
min − log p(xSt i | xSt−1
i
)− log p(xB Si
t | xt )
i
x1:N ,θ
t=2 i=1 t=1 i=1
| {z }| {z }
S Placement of sensor Si
Dynamics of the state xt i
on body segment Bi
NS
X NS
X
− log p(xS1 i ) − log p(θSi )
i=1 i=1
| {z }
Prior
subj. to c(x1:N ) = 0, (6.9)
where the number of time steps is denoted NT and c(x1:N ) denote the equality constraints from assuming
that the body segments are connected at the joints. Note the similarities and differences between the
structure of (6.9) as compared to (3.3a) or (4.72). The problem (6.9) is formulated as a smoothing
algorithm. In [97], a similar method is presented in which a sliding window approach is used to allow
for online estimation. In EKF formulations it is not straightforward to include equality constraints.
However, as discussed in [97], it is possible to include the equality constraints as a measurement update
in the EKF instead.
Optionally, additional knowledge about the human body can be included in (6.9). For instance, in
[73], we include the knowledge that for some joints, it is known that their rotation is (mainly) limited to
one or two axes. An example of this is the knee which is a hinge joint, although it can in practice flex
a little around the other axes too. The optimization problem for that case would include a term that
minimizes the rotation around certain axes. In [144], a solution to the inertial motion capture problem
is presented that includes additional biomechanical information. For instance, the shape of the body
segments is approximated by a capsule and the sensors are assumed to be placed on the surface of the
capsule. In [152], the focus is instead on estimation of human motion using a much smaller amount
68
of sensors. Because of the smaller number of sensors, the information about the human body and its
motions is more crucial in their problem formulation.
69
Chapter 7
Concluding Remarks
The goal of this tutorial was not to give a complete overview of all algorithms that can be used for
position and orientation estimation. Instead, our aim was to give a pedagogical introduction to the topic
of position and orientation estimation using inertial sensors, allowing newcomers to this problem to get
up to speed as fast as possible by reading one paper. By integrating the inertial sensor measurements
(so-called dead-reckoning), it is possible to obtain information about the position and orientation of the
sensor. However, errors in the measurements will accumulate and the estimates will drift. Because of
this, to obtain accurate position and orientation estimates using inertial measurements, it is necessary to
use additional sensors and additional models. In this tutorial, we have considered two separate estimation
problems. The first is orientation estimation using inertial and magnetometer measurements, assuming
that the acceleration of the sensor is approximately zero. Including magnetometer measurements removes
the drift in the heading direction (as illustrated in Example 4.2), while assuming that the acceleration
is approximately zero removes the drift in the inclination. The second estimation problem that we have
considered is pose estimation using inertial and position measurements. Using inertial measurements,
the position and orientation estimates are coupled and the position measurements therefore provide
information also about the orientation.
A number of algorithms for position and orientation estimation have been introduced in Chapter 4.
These include smoothing and filtering solved as an optimization problem, two different extended Kalman
filter implementations and a version of the complementary filter. The filtering approaches use the data
up to a certain time t to estimate the position and orientation at this time t. Smoothing instead makes
use of all data from time t = 1, . . . , N . In general, using all data to obtain the estimates will naturally
lead to better estimates. The filtering approaches can be seen to be quite uncertain about their esti-
mates for the first samples and require some time to “converge” to accurate estimates. This is even more
pronounced in the presence of calibration parameters as discussed in Chapter 5. Although smoothing
algorithms give better estimates, practical applications might not allow for computing smoothing esti-
mates because of computational limitations or real-time requirements. For the examples discussed in this
paper, the optimization-based filtering algorithm and the EKF with orientation deviation states perform
very similarly. The EKF with quaternion states, however, was able to handle wrong initial orientations
less well as shown in Table 4.4. Furthermore, it underestimated the uncertainty in the heading direction
in the absence of magnetometer measurements, see also Example 4.5. The complementary filter is a good
alternative to these methods, but its estimation accuracy decreases when orientation estimates from the
accelerometer and magnetometer measurements are of significantly better quality in the inclination than
in the heading direction. In Chapter 6 we have discussed possible extensions to the simple models con-
sidered earlier in this tutorial. One of the benefits of the optimization-based approaches is that these
extensions can straightforwardly be incorporated into the framework.
Apart from the differences between the estimation algorithms discussed in this tutorial, it can also
be concluded that the position and orientation estimation problems using inertial sensors are actually
quite forgiving. Any of the algorithms introduced in this tutorial can give reasonably good estimates
with fairly little effort. However, careful modeling is important since the quality of the estimates of the
algorithms highly depends on the validity of the models. This was illustrated in Example 4.3 for the
influence of magnetic material in the vicinity of the sensor on the quality of the orientation estimates.
In recent years, inertial sensors have undergone major developments. The quality of their measurements
has improved while their cost has decreased, leading to an increase in availability. Furthermore, available
computational resources are steadily increasing. Because of these reasons, we believe that inertial sensors
70
can be used for even more diverse applications in the future.
71
Chapter 8
Abbreviation Meaning
AR Augmented reality
BFGS Broyden-Fletcher-Goldfarb-Shanno
EKF Extended Kalman filter
GNSS Global navigation satellite system
IMU Inertial measurement unit
LS Least squares
MAP Maximum a posteriori
MEKF Multiplicative extended Kalman filter
MEMS Microelectromechanical system
ML Maximum likelihood
NLOS Non-line-of-sight
NLS Nonlinear least squares
RMSE Root mean square error
ROS Robot operating system
RTS Rauch-Tung-Striebel
SLAM Simultaneous localization and mapping
TOA Time of arrival
UWB Ultrawideband
VR Virtual reality
72
Notation Meaning
xt State vector x at time t.
yt Measurement vector y at time t.
θ Parameter vector.
x1:N , y1:N States and measurements for t = 1, . . . , N .
x̂ Estimate of x.
q nb Unit quaternion representing the rotation from
the b-frame to the n-frame.
Rnb Rotation matrix representing the rotation from
the b-frame to the n-frame.
xn Vector x expressed in the n-frame.
RN Set of real numbers in N dimensions.
SO(3) Special orthogonal group in three dimensions.
∈ Element of.
⊆ Subset of.
∅ Empty set.
N (µ, Σ) Gaussian distribution with mean µ and covari-
ance Σ.
N (x ; µ, Σ) Random variable x that is normal distributed
with mean µ and covariance Σ
p( · ) Probability density function.
p(a, b) Joint probability of a and b.
p(a | b) Conditional distribution of a given b.
∼ Distributed according to.
∝ Proportional to.
In Identity matrix of size n × n.
0m×n Zero matrix of size m × n.
Notation Meaning
arg max Maximizing argument.
arg min Minimizing argument.
a·b Inner product of the vectors a and b.
a×b Cross product of the vectors a and b.
[a×] Cross product matrix of the vector a as defined
in (3.14).
kak2 Two-norm of the vector a.
kakW Weighted norm of the vector a.
ā Quaternion representation of the vector a as de-
fined in (3.26).
AT Transpose of the matrix A.
A−1 Inverse of the matrix A.
det A Determinant of the matrix A.
expR (η) Mapping from the vector η to a rotation matrix
as defined in (3.37b).
logR (R) Mapping from the rotation matrix R to a vector
as defined in (3.39b).
qc Conjugate of the unit quaternion q as defined
in (3.25).
Quaternion multiplication as defined in (3.27).
qL Left quaternion multiplication matrix of the
quaternion q as defined in (3.28).
qR Right quaternion multiplication matrix of the
quaternion q as defined in (3.28).
expq (η) Mapping from the vector η to a unit quaternion
as defined in (3.37a).
logq (q) Mapping from the unit quaternion q to a vector
as defined in (3.39a).
73
Acknowledgements
This research was financially supported by the projects Probabilistic modeling of dynamical systems (Con-
tract number: 621-2013-5524), NewLEADS - New Directions in Learning Dynamical Systems (Contract
number: 621-2016-06079) and CADICS, all funded by the Swedish Research Council, the project AS-
SEMBLE (Contract number: RIT15-0012) funded by the Swedish Foundation for Strategic Research
(SSF) and the EPSRC grant Autonomous behaviour and learning in an uncertain world (Grant num-
ber: EP/J012300/1). High accuracy reference measurements are provided through the use of the Vi-
con real-time tracking system courtesy of the UAS Technologies Lab, Artificial Intelligence and Inte-
grated Computer Systems Division (AIICS) at the Department of Computer and Information Science
(IDA), Linköping University, Sweden http://www.ida.liu.se/divisions/aiics/aiicssite/index.
en.shtml. The authors would also like to thank Fredrik Olsson, Michael Lorenz, Dr. Oliver Wood-
ford, Dr. Gustaf Hendeby, Prof. Fredrik Gustafsson and Prof. Magnus Herberthson for comments and
suggestions that greatly improved this tutorial.
74
Appendix A
Orientation Parametrizations
In §A.1 of this appendix, we will summarize some important results on quaternion algebra that we make
frequent use of throughout this tutorial. In §A.2, we summarize some results on how to convert between
the different orientation parametrizations.
A special case is the unit quaternion, for which kqk2 = 1. We use unit quaternions as a parametrization of
orientations. An example of a quaternion that is not a unit quaternion and that we frequently encounter
in this tutorial is the quaternion representation of a vector. For a vector v, its quaternion representation
is given by
0
v̄ = . (A.2)
v
q v̄ q c . (A.3)
This can alternatively be defined in terms of the left and right multiplication matrices
p q = pL q = q R p, (A.5)
with
−pT −qvT
p0 q0
pL , v
, qR , , (A.6)
pv p0 I3 + [pv ×] qv q0 I3 − [qv ×]
75
Hence, the rotation of the vector v is given by
R
q v̄ q c = q L (q c ) v̄
−qvT −qvT
q0 q0 0
=
qv q0 I3 + [qv ×] qv q0 I3 − [qv ×] v
1 01×3 0
= . (A.9)
03×1 qv qvT + q02 I3 + 2q0 [qv ×] + [qv ×]2 v
Note the similarity with (A.9), where the rotation of the vector v can equivalently be expressed as Rv.
Conversely, a rotation matrix
R11 R12 R13
R = R21 R22 R23 , (A.11)
R31 R32 R33
Note that a practical implementation needs to take care of the fact that the conversion (A.12) only leads
to sensible results if 1 + Tr R > 0 and q0 6= 0. To resolve this issue, the conversion is typically performed
in different ways depending on the trace of the matrix R and its diagonal values, see e.g. [6].
A rotation vector η can be expressed in terms of a unit quaternion q via the quaternion exponential
as
cos kηk2
q = expq η = η . (A.13)
kηk2 sin kηk2
Note that any practical implementation needs to take care of the fact that this equation is singular at
T
η = 0, in which case expq η = 1 0 0 0 . The inverse operation is executed by the quaternion
logarithm,
arccos q0 arccos q0
η = logq q = sin arccos q0 qv = kqv k2 qv . (A.14)
Note that this equation is singular at qv = 0. In this case, log q should return 03×1 .
The rotation vector η can also be converted into a rotation matrix as
(log R)32
R = expR η = exp ([η×]) , η = logR R = (log R)13 , (A.15)
(log R)21
where log R is the matrix logarithm and logR and expR are the mappings introduced in (3.37) and (3.39).
A rotation in terms of Euler angles can be expressed as a rotation matrix R as
1 0 0 cos θ 0 − sin θ cos ψ sin ψ 0
R = 0 cos φ sin φ 0 1 0 − sin ψ cos ψ 0 (A.16)
0 − sin φ cos φ sin θ 0 cos θ 0 0 1
cos θ cos ψ cos θ sin ψ − sin θ
= sin φ sin θ cos ψ − cos φ sin ψ sin φ sin θ sin ψ + cos φ cos ψ sin φ cos θ .
cos φ sin θ cos ψ + sin φ sin ψ cos φ sin θ sin ψ − sin φ cos ψ cos φ cos θ
76
The rotation matrix R can be converted into Euler angles as
−1 2q1 q2 −2q0 q3
ψ = tan−1 R 12
R11 = tan 2 2
2q +2q −1
, (A.17a)
0 1
Using the relations presented in this section, it is possible to convert the parametrizations discussed
in §3.2 into each other.
77
Appendix B
Pose Estimation
In this appendix, we will introduce the necessary components to extend Algorithms 1–4 to pose estimation
algorithms using the state space model (3.74).
Similar to the update of the linearization point in (4.31a), we also update the estimates of the position
and velocity before starting the optimization algorithm, such that ef,t is equal to zero for iteration k = 0.
78
B.3 Extended Kalman filter with quaternion states
Following the notation in §4.3, the following matrices are needed for implementing the EKF for pose
estimation with quaternion states
nb
T 2 ∂Rt|t
I
3 T I 3 2 ∂qt|tnb y a,t
nb nb
qt|t =q̂t|t
nb , H = I3 , R = Σp ,
Ft =
0 I T
∂Rt|t
y (B.5a)
3 a,t
nb
∂qt|t
nb nb
qt|t =q̂t|t
T
R
0 0 expq ( 2 yω,t )
I6 Σa 0 0
!
0
Gt = L
∂ expq (eω,t ) , Q = 0 Σa 0 . (B.5b)
0 − T2 q̂t|tnb
∂eω,t 0 0 Σω
79
Appendix C
In this appendix, we will introduce the necessary components to extend Algorithms 1–4 to estimate an
unknown gyroscope bias. Note that the measurement models are independent of the gyroscope bias.
The dynamic models and time update equations are adapted as described below.
Modeling the dynamics of the gyroscope bias as a random walk, ef,t is extended from (4.27) to
∂e T T
where ∂xf,tt = I6 and xt = (ηtn )T δω,t
.
The covariance matrix Pt+1|t is given by Pt+1|t = Ft Pt|t FtT + Gt QGT
t with
nb,(0)
I3 −T R̃t+1
Ft = , (C.5a)
0 I3
nb,(0)
T R̃t+1 0 Σω 0
Gt = , Q= . (C.5b)
0 I3 0 Σδω,t
80
C.3 Extended Kalman filter with quaternion states
To include estimation of an unknown gyroscope bias in Algorithm 3, the matrices for the time update
of the EKF need to be adapted to
R L !
T T nb ∂ expq (δω,t )
Ft = exp (
q 2 (y ω,t − δ̂ ω,t|t )) − 2 q̂t|t ∂δω,t , (C.6a)
03×4 I3
L !
T nb ∂ expq (eω,t )
Σω 0 − q̂ 0
Q= , Gt = 2 t|t ∂eω,t . (C.6b)
0 Σδω,t 0 I3
Note that also for this algorithm a prior on the gyroscope bias needs to be included. Furthermore, the
renormalization of the covariance in (4.44) needs to be adjusted to the size of the covariance matrix as
T
1 nb nb
q̃
nb k3 t|t q̃t|t 0
Jt = kq̃t|t 2
. (C.7)
0 I3
and Q is given in (C.5b). A prior on the gyroscope bias also needs to be included.
81
Bibliography
[1] S. Adler, S. Schmitt, K. Wolter, and M. Kyas. A survey of experimental evaluation in indoor
localization research. In Proceedings of the IEEE International Conference on Indoor Positioning
and Indoor Navigation (IPIN), pages 1–10, Banff, Alberta, Canada, October 2015.
[2] D. W. Allan. Statistics of atomic frequency standards. Proceedings of the IEEE, 54(2):221–230,
1966.
[3] K. J. Åström. Maximum likelihood and prediction error methods. Automatica, 16(5):551–574,
1980.
[4] A. Avci, S. Bosch, M. Marin-Perianu, R. Marin-Perianu, and P. Havinga. Activity recognition
using inertial sensing for healthcare, wellbeing and sports applications: A survey. In Proceedings
of the 23rd International Conference on Architecture of Computing Systems (ARCS), pages 1–10,
Hannover, Germany, February 2010.
[5] T. Bailey and H. Durrant-Whyte. Simultaneous localization and mapping (SLAM): Part II. IEEE
Robotics & Automation Magazine, 13(3):108–117, 2006.
[6] M. J. Baker. EuclideanSpace – Mathematics and computing. http://www.euclideanspace.com/
maths/geometry/rotations/conversions/matrixToQuaternion/, 2016. Accessed on October 4.
[7] N. Barbour and G. Schmidt. Inertial sensor technology trends. IEEE Sensors Journal, 1(4):332–
339, 2001.
[8] T. D. Barfoot. State Estimation for Robotics. Cambridge University Press, 2017.
[9] H. Bay, T. Tuytelaars, and L. Van Gool. SURF: Speeded up robust features. In Proceedings of the
European Conference on Computer Vision (ECCV), pages 404–417, Graz, Austria, May 2006.
[10] B. M. Bell. The iterated Kalman smoother as a Gauss-Newton method. SIAM Journal on Opti-
mization, 4(3):626–636, 1994.
[11] D. P. Bertsekas. Dynamic programming and optimal control, volume 1. Athena Scientific Belmont,
MA, 3rd edition, 1995.
[12] C. M. Bishop. Pattern recognition and machine learning. Springer Science + Business Media, 2006.
[13] G. Bleser and D. Stricker. Advanced tracking through efficient image processing and visual–inertial
sensor fusion. Computers & Graphics, 33(1):59–72, 2009.
[16] S. Boyd and L. Vandenberghe. Convex Optimization. Cambridge University Press, 2004.
[17] R. G. Brown. Integrated navigation systems and Kalman filtering: A perspective. Navigation:
Journal of the Institute of Navigation, 19(4):355–362, 1972.
82
[18] M. Bryson, M. Johnson-Roberson, and S. Sukkarieh. Airborne smoothing and mapping using
vision and inertial sensors. In Proceedings of the IEEE International Conference on Robotics and
Automation (ICRA), pages 3143–3148, Kobe, Japan, May 2009.
[19] A. Bulling, U. Blanke, and B. Schiele. A tutorial on human activity recognition using body-worn
inertial sensors. ACM Computing Surveys (CSUR), 46(3):Article 33, 2014.
[20] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard.
Past, present, and future of simultaneous localization and mapping: Toward the robust-perception
age. IEEE Transactions on Robotics, 32(6):1309–1332, 2016.
[21] J. Callmer. Autonomous Localization in Unknown Environments. Dissertation no. 1520, Linköping
University, Linköping, Sweden, June 2013.
[22] Y. T. Chan and K. C. Ho. A simple and efficient estimator for hyperbolic location. IEEE Trans-
actions on Signal Processing, 42(8):1905–1915, 1994.
[23] H. Chao, Y. Cao, and Y. Chen. Autopilots for small unmanned aerial vehicles: A survey. The
International Journal of Control, Automation and Systems, 8(1):36–44, 2010.
[24] D. S. Colomar, J.-O. Nilsson, and P. Händel. Smoothing for ZUPT-aided INSs. In Proceedings
of the IEEE International Conference on Indoor Positioning and Indoor Navigation (IPIN), pages
1–5, Sydney, Australia, November 2012.
[25] G. Cooper, I. Sheret, L. McMillan, K. Siliverdis, N. Sha, D. Hodgins, L. Kenney, and D. Howard. In-
ertial sensor-based knee flexion/extension angle estimation. Journal of Biomechanics, 42(16):2678
– 2685, 2009.
[26] P. Corke, J. Lobo, and J. Dias. An introduction to inertial and visual sensing. The International
Journal of Robotics Research, 26(6):519–535, 2007.
[27] J. A. Corrales, F. A. Candelas, and F. Torres. Hybrid tracking of human operators using IMU/UWB
data fusion by a Kalman filter. In Proceedings of the 3rd ACM/IEEE International Conference on
Human-Robot Interaction (HRI), pages 193–200, Amsterdam, the Netherlands, 2008.
[28] J. L. Crassidis, F. L. Markley, and Y. Cheng. A survey of nonlinear attitude estimation methods.
Journal of Guidance, Control, and Dynamics, 30(1):12–28, 2007.
[29] T. A. Davis. Direct methods for sparse linear systems. SIAM, 2006.
[30] A. De Angelis, J. O. Nilsson, I. Skog, P. Händel, and P. Carbone. Indoor positioning by ultrawide
band radio aided inertial navigation. Metrology and Measurement Systems, 17(3):447–460, 2010.
[31] J. Diebel. Representing attitude: Euler angles, unit quaternions, and rotation vectors. Stanford
University, Stanford, California, Tech. Rep. 94301–9010, 2006.
[32] H. Durrant-Whyte and T. Bailey. Simultaneous localization and mapping: Part I. IEEE Robotics
& Automation Magazine, 13(2):99–110, 2006.
[33] N. El-Sheimy, H. Hou, and X. Niu. Analysis and modeling of inertial sensors using Allan variance.
IEEE Transactions on Instrumentation and Measurement, 57(1):140–149, 2008.
[34] L. Euler. Formulae generales pro translatione quacunque corporum rigidorum. Novi Commentarii
Academiae Scientiarum Imperialis Petropolitanae, 20:189–207, 1776.
[35] J. Favre, B. M. Jolles, R. Aissaoui, and K. Aminian. Ambulatory measurement of 3D knee joint
angle. Journal of Biomechanics, 41(5):1029–1035, 2008.
[36] M. A. Fischler and R. C. Bolles. Random sample consensus: A paradigm for model fitting with ap-
plications to image analysis and automated cartography. Communications of the ACM, 24(6):381–
395, 1981.
[37] Fitbit. http:/www.fitbit.com, 2017. Accessed on March 17.
[38] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza. On-manifold preintegration for real-time
visual-inertial odometry. IEEE Transactions on Robotics, 33(1):1–21, 2017.
83
[39] E. Foxlin. Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter. In
Proceedings of the IEEE Virtual Reality Annual International Symposium, pages 185–194, Santa
Clara, CA, USA, Mar.–Apr. 1996.
[40] E. Foxlin. Pedestrian tracking with shoe-mounted inertial sensors. IEEE Computer Graphics and
Applications, 25(6):38–46, 2005.
[41] E. Foxlin and L. Naimark. Vis-tracker: A wearable vision-inertial self-tracker. In Proceedings of
IEEE Virtual Reality, Los Angeles, USA, March 2003.
[42] Garmin. https://www.garmin.com, 2017. Accessed on March 17.
[43] S. Gezici, Z. Tian, G. B. Giannakis, H. Kobayashi, A. F Molisch, H. V. Poor, and Z. Sahinoglu.
Localization via ultra-wideband radios: a look at positioning aspects for future sensor networks.
IEEE Signal Processing Magazine, 22(4):70–84, 2005.
[44] I. Gilitschenski, G. Kurz, S. J. Julier, and U. D. Hanebeck. Unscented orientation estimation based
on the Bingham distribution. IEEE Transactions on Automatic Control, 61(1):172–177, 2016.
[45] J. Glover and L. P. Kaelbling. Tracking 3-D rotations with the quaternion Bingham filter. Computer
Science and Artificial Intelligence Laboratory Technical Report, 2013.
[46] G. H. Golub and C. F. van Loan. Matrix Computations. The John Hopkins University Press,
Baltimore, 4 edition, 2013.
[47] G. Grisetti, R. Kümmerle, C. Stachniss, and W. Burgard. A tutorial on graph-based SLAM. IEEE
Intelligent Transportation Systems Magazine, 2(4):31–43, 2010.
[48] G. Grisetti, R. Kümmerle, C. Stachniss, U. Frese, and C. Hertzberg. Hierarchical optimization on
manifolds for online 2D and 3D mapping. In Proceedings of the IEEE International Conference on
Robotics and Automation (ICRA), pages 273–278, Anchorage, Alaska, May 2010.
[49] S. Grzonka, F. Dijoux, A. Karwath, and W. Burgard. Mapping indoor environments based on
human activity. In Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA), pages 476–481, Anchorage, Alaska, May 2010.
[50] F. Gustafsson. Statistical Sensor Fusion. Studentlitteratur, 2012.
[51] F. Gustafsson and F. Gunnarsson. Mobile positioning using wireless networks: possibilities and
fundamental limitations based on available wireless network measurements. IEEE Signal Processing
Magazine, 22(4):41–53, 2005.
[52] W. R. Hamilton. On quaternions; or on a new system of imaginaries in algebra. Philosophical
Magazine, xxv, 1844.
[53] M. Hardegger, D. Roggen, A. Calatroni, and G. Tröster. S-SMART: A unified Bayesian framework
for simultaneous semantic mapping, activity recognition, and tracking. ACM Transactions on
Intelligent Systems and Technology (TIST), 7(3):34, 2016.
[54] R. Harle. A survey of indoor inertial positioning systems for pedestrians. IEEE Communications
Surveys & Tutorials, 15(3):1281–1293, 2013.
[55] C. Harris and M. Stephens. A combined corner and edge detector. In Proceedings of the Alvey
Vision Conference, volume 15, pages 147–151, Manchester, United Kingdom, Aug.–Sept. 1988.
[56] J. Haverinen and A. Kemppainen. Global indoor self-localization based on the ambient magnetic
field. Robotics and Autonomous Systems, 57(10):1028–1035, 2009.
[57] G. Hendeby, F. Gustafsson, N. Wahlström, and S. Gunnarsson. Platform for teaching sensor fusion
using a smartphone. International Journal of Engineering Education, 33(2B):781–789, 2017.
[58] W. T. Higgins. A comparison of complementary and Kalman filtering. IEEE Transactions on
Aerospace and Electronic Systems, (3):321–325, 1975.
[59] J. D. Hol. Sensor fusion and calibration of inertial sensors, vision, ultra-wideband and GPS.
Dissertation no. 1368, Linköping University, Linköping, Sweden, June 2011.
84
[60] J. D. Hol, T. B. Schön, and F. Gustafsson. Modeling and calibration of inertial and vision sensors.
The International Journal of Robotics Research, 29(2-3):231–244, 2010.
[61] J. D. Hol, T. B. Schön, H. Luinge, P. J. Slycke, and F. Gustafsson. Robust real-time tracking
by fusing measurements from inertial and vision sensors. Journal of Real-Time Image Processing,
2(2-3):149–160, 2007.
[62] B. K. P. Horn. Closed-form solution of absolute orientation using unit quaternions. Journal of the
Optical Society of America A, 4(4):629–642, 1987.
[63] IEEE. IEEE standard for inertial systems terminology, 2009. Technical report, IEEE Std 1559.
[70] D. H. Kang, Y. J. Jung, A. J. Park, and J. W. Kim. Human body motion capture system using
magnetic and inertial sensor modules. In Proceedings of the 5th International Universal Commu-
nication Symposium (IUCS), pages 1–4, Gumi, Korea, October 2011.
[71] E. D. Kaplan and C. J. Hegarty. Understanding GPS: principles and applications. Artech House,
1996.
[72] J. Kelly and G. S. Sukhatme. Visual-inertial sensor fusion: Localization, mapping and sensor-to-
sensor self-calibration. The International Journal of Robotics Research, 30(1):56–79, 2011.
[73] M. Kok, J. D. Hol, and T. B. Schön. An optimization-based approach to human body motion
capture using inertial sensors. In Proceedings of the 19th World Congress of the International
Federation of Automatic Control, pages 79–85, Cape Town, South Africa, August 2014.
[74] M. Kok, J. D. Hol, and T. B. Schön. Indoor positioning using ultrawideband and inertial measure-
ments. IEEE Transactions on Vehicular Technology, 64(4):1293–1303, 2015.
[75] M. Kok and T. B. Schön. Magnetometer calibration using inertial sensors. IEEE Sensors Journal,
16(14):5679–5689, 2016.
[76] J. B. Kuipers. Quaternions and Rotation Sequences: a primer with applications to orbits, aerospace,
and virtual reality. Princeton University Press, 1999.
[77] G. Kurz, I. Gilitschenski, S. Julier, and U. D. Hanebeck. Recursive estimation of orientation based
on the Bingham distribution. In Proceedings of the 16th International Conference on Information
Fusion, pages 1487–1494, Istanbul, Turkey, July 2013.
85
[80] G. Ligorio and A. M. Sabatini. Dealing with magnetic disturbances in human motion capture: A
survey of techniques. Micromachines, 7(3):43, 2016.
[81] F. Lindsten and T. B. Schön. Backward simulation methods for Monte Carlo statistical inference.
Foundations and Trends in Machine Learning, 6(1):1–143, 2013.
[82] L. Ljung. System Identification, Theory for the User. Prentice Hall PTR, 2 edition, 1999.
[83] J. Lobo and J. Dias. Relative pose calibration between visual and inertial sensors. The International
Journal of Robotics Research, 26(6):561–575, 2007.
[84] D. G. Lowe. Distinctive image features from scale-invariant keypoints. The International Journal
of Computer Vision, 60(2):91–110, 2004.
[85] H. J. Luinge. Inertial Sensing of Human Movement. PhD thesis, University of Twente, Enschede,
the Netherlands, October 2002.
[86] H. J. Luinge and P. H. Veltink. Measuring orientation of human body segments using miniature
gyroscopes and accelerometers. Medical and Biological Engineering and Computing, 43(2):273–282,
2005.
[87] H. J. Luinge, P. H. Veltink, and C. T. M. Baten. Ambulatory measurement of arm orientation.
Journal of Biomechanics, 40(1):78–85, 2007.
[88] T. Lupton and S. Sukkarieh. Visual-inertial-aided navigation for high-dynamic motion in built
environments without initial conditions. IEEE Transactions on Robotics, 28(1):61 –76, February
2012.
[89] S. O. H. Madgwick, A. J. L. Harrison, and R. Vaidyanathan. Estimation of IMU and MARG
orientation using a gradient descent algorithm. In Proceedings of the IEEE International Conference
on Rehabilitation Robotics, pages 1–7, Zürich, Switserland, Jun.–Jul. 2011.
[90] R. Mahony, T. Hamel, and J.-M. Pflimlin. Nonlinear complementary filters on the special orthog-
onal group. IEEE Transactions on Automatic Control, 53(5):1203–1218, 2008.
[91] 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 of the IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS), volume 4, pages 2003–2011,
Maui, Hawaii, USA, Oct.–Nov. 2001.
[92] J. B. Marion and S. T. Thornton. Classical dynamics of particles and systems. Harcourt College
Publishers, 4 edition, 1995.
[93] F. L. Markley. Attitude error representations for Kalman filtering. Journal of Guidance, Control,
and Dynamics, 26(2):311–317, 2003.
[94] A. Martinelli. Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute
scale, and bias determination. IEEE Transactions on Robotics, 28(1):44–60, 2012.
[95] J. Mattingley and S. Boyd. Real-time convex optimization in signal processing. IEEE Signal
Processing Magazine, 27(3):50–61, 2010.
[96] J. L. Meriam and L. G. Kraige. Engineering mechanics: dynamics. John Wiley & Sons, 4 edition,
1998.
[97] M. Miezal, B. Taetz, and G. Bleser. On inertial body tracking in the presence of model calibration
errors. Sensors, 16(7):1132, 2016.
[98] F. M. Mirzaei and S. I. Roumeliotis. A Kalman filter-based algorithm for IMU-camera calibration:
Observability analysis and performance evaluation. IEEE Transactions on Robotics, 24(5):1143–
1156, 2008.
[99] P. Müller, J. A. del Peral-Rosado, R. Piché, and G. Seco-Granados. Statistical trilateration with
skew-t distributed errors in LTE networks. IEEE Transactions on Wireless Communications,
15(10):7114–7127, 2016.
86
[100] P. Müller, H. Wymeersch, and R. Piché. UWB positioning with generalized gaussian mixture
filters. IEEE Transactions on Mobile Computing, 13(10):2406–2414, 2014.
[101] National Aeronautics and Space Administatration. Earth fact sheet. http://nssdc.gsfc.nasa.
gov/planetary/factsheet/earthfact.html, 2016. Accessed on September 7.
[104] H. Nurminen, T. Ardeshiri, R. Piché, and F. Gustafsson. Robust inference for state-space models
with skewed measurement noise. IEEE Signal Processing Letters, 22(11):1898–1902, 2015.
[105] H. Nyqvist and F. Gustafsson. A high-performance tracking system based on camera and IMU. In
Proceedings of the 16th International Conference on Information Fusion, pages 2065–2072, Istan-
bul, Turkey, July 2013.
[106] F. Olsson, M. Kok, K. Halvorsen, and T. B. Schön. Accelerometer calibration using sensor fusion
with a gyroscope. In Proceedings of the IEEE Workshop on Statistical Signal Processing, pages
660–664, Palma de Mallorca, Spain, June 2016.
[107] B. Palais, R. Palais, and S. Rodi. A disorienting look at Euler’s theorem on the axis of a rotation.
American Mathematical Monthly, 116(10):892–909, 2009.
[108] G. Panahandeh, I. Skog, and M. Jansson. Calibration of the accelerometer triad of an inertial
measurement unit, maximum likelihood estimation and Cramer-Rao bound. In Proceedings of the
IEEE International Conference on Indoor Positioning and Indoor Navigation (IPIN), pages 1–6,
Zurich, Switserland, September 2010.
[109] M. Perlmutter and L. Robin. High-performance, low cost inertial MEMS: A market in motion!
In Proceedings of the IEEE/ION Position Location and Navigation Symposium (PLANS), pages
225–229, Myrtle Beach, South Carolina, USA, 2012.
[110] S. Pittet, V. Renaudin, B. Merminod, and M. Kasser. UWB and MEMS based indoor navigation.
Journal of Navigation, 61(3):369–384, July 2008.
[113] C. V. Rao, J. B. Rawlings, and J. H. Lee. Constrained linear state estimation – a moving horizon
approach. Automatica, 37(10):1619–1628, 2001.
[114] C. V. Rao, J. B. Rawlings, and D. Q. Mayne. Constrained state estimation for nonlinear discrete-
time systems: Stability and moving horizon approximations. IEEE Transactions on Automatic
Control, 48(2):246–258, 2003.
[115] H. Rehbinder and X. Hu. Drift-free attitude estimation for accelerated rigid bodies. Automatica,
40(4):653–659, 2004.
[116] J. Rehder and R. Siegwart. Camera/IMU calibration revisited. IEEE Sensors Journal, 17(11):3257–
3268, 2017.
[117] A. Reiss, G. Hendeby, G. Bleser, and D. Stricker. Activity recognition using biomechanical model
based pose estimation. In Proceedings of the 5th European Conference on Smart Sensing and
Context, pages 42–55, Passau, Germany, November 2010.
[118] V. Renaudin, M. H. Afzal, and G. Lachapelle. Complete triaxis magnetometer calibration in the
magnetic domain. Journal of Sensors, 2010. Article ID 967245.
87
[119] P. Robertson, M. Frassl, M. Angermann, M. Doniec, B. J. Julian, M. Garcia Puyol, M. Khider,
M. Lichtenstern, and L. Bruno. Simultaneous localization and mapping for pedestrians using
distortions of the local magnetic field intensity in large indoor environments. In Proceedings of the
IEEE International Conference on Indoor Positioning and Indoor Navigation (IPIN), pages 1–10,
Montbéliard, France, October 2013.
[120] D. Roetenberg, H. J. Luinge, C. T. M. Baten, and P. H. Veltink. Compensation of magnetic
disturbances improves inertial and magnetic sensing of human body segment orientation. IEEE
Transactions on Neural Systems and Rehabilitation Engineering, 13(3):395–405, 2005.
[121] D. Roetenberg, H. J. Luinge, and P. Slycke. Xsens MVN: Full 6DOF human motion tracking using
miniature inertial sensors, May 2013.
[122] M. Roth, T. Ardeshiri, E. Özkan, and F. Gustafsson. Robust Bayesian filtering and smoothing
using student’s t distribution. ArXiv e-prints, March 2017. arXiv:1703.02428.
[123] Y. Saad. Iterative methods for sparse linear systems. SIAM, 2003.
[124] A. M. Sabatini. Quaternion-based extended Kalman filter for determining orientation by inertial
and magnetic sensing. IEEE Transactions on Biomedical Engineering, 53(7):1346–1356, 2006.
[125] Z. Sahinoglu, S. Gezici, and I. Güvenc. Ultra-wideband Positioning Systems: Theoretical Limits,
Ranging Algorithms, and Protocols. Cambridge University Press, 2008.
[126] S. Salehi, N. Mostofi, and G. Bleser. A practical in-field magnetometer calibration method for
IMUs. In Proceedings of the IROS Workshop on Cognitive Assistive Systems: Closing the Action-
Perception Loop, pages 39–44, Vila Moura, Portugal, October 2012.
[127] S. Särkkä. Bayesian Filtering and Smoothing. Cambridge University Press, 2013.
[128] P. G. Savage. Strapdown inertial navigation integration algorithm design part 1: Attitude algo-
rithms. Journal of Guidance, Control and Dynamics, 21(1):19–28, 1998.
[129] P. G. Savage. Strapdown inertial navigation integration algorithm design part 2: Velocity and
position algorithms. Journal of Guidance, Control and Dynamics, 21(2):208–221, 1998.
[130] A. H. Sayed, A. Tarighat, and N. Khajehnouri. Network-based wireless location: challenges faced in
developing techniques for accurate wireless location information. IEEE Signal Processing Magazine,
22(4):24–40, 2005.
[131] T. B. Schön, F. Lindsten, J. Dahlin, J. Wågberg, C. A. Naesseth, A. Svensson, and L. Dai. Sequen-
tial monte carlo methods for system identification. In Proceedings of the 17th IFAC Symposium on
System Identification, pages 775–786, Beijing, China, October 2015.
[132] S. Sczyslo, J. Schroeder, S. Galler, and T. Kaiser. Hybrid localization using UWB and inertial sen-
sors. In Proceedings of the IEEE International Conference on Ultra-Wideband (ICUWB), volume 3,
pages 89–92, Hannover, Germany, September 2008.
[133] T. Seel, J. Raisch, and T. Schauer. IMU-based joint angle measurement for gait analysis. Sensors,
14(4):6891–6909, 2014.
[134] M. Segal and E. Weinstein. A new method for evaluating the log-likelihood gradient, the Hessian,
and the Fisher information matrix for linear dynamic systems. IEEE Transactions on Information
Theory, 35(3):682–687, 1989.
[135] M. D. Shuster. A survey of attitude representations. The Journal of the Astronautical Sciences,
41(4):439–517, Oct.–Dec. 1993.
[136] M. D. Shuster. The TRIAD algorithm as maximum likelihood estimation. The Journal of the
Astronautical Sciences, 54(1):113–123, 2006.
[137] M. D. Shuster and S. D. Oh. Three-axis attitude determination from vector observations. Journal
of Guidance, Control, and Dynamics, 4(1):70–77, 1981.
88
[138] Z. Sjanic, M. A. Skoglund, and F. Gustafsson. EM-SLAM with inertial/visual applications. IEEE
Transactions on Aerospace and Electronic Systems, 53(1):273–285, 2017.
[139] I. Skog and P. Händel. In-car positioning and navigation technologies – A survey. IEEE Transac-
tions on Intelligent Transportation Systems, 10(1):4–21, 2009.
[140] I. Skog, P. Handel, J.-O. Nilsson, and J. Rantakokko. Zero-velocity detection – An algorithm
evaluation. IEEE Transactions on Biomedical Engineering, 57(11):2657–2666, 2010.
[141] M. A. Skoglund, G. Hendeby, and D. Axehill. Extended Kalman filter modifications based on
an optimization view point. In Proceedings of the 18th International Conference on Information
Fusion, pages 1856–1861, Washington, D.C., USA, 2015.
[142] M. A. Skoglund, Z. Sjanic, and M. Kok. On orientation estimation using iterative methods in
Euclidean space. In Proceedings of the 20th International Conference on Information Fusion,
pages 1–8, Xi’an, China, July 2017.
[143] A. Solin, M. Kok, N. Wahlström, T. B. Schön, and S. Särkkä. Modeling and interpolation of the
ambient magnetic field by Gaussian processes. ArXiv e-prints, September 2015. arXiv:1509.04634.
[144] B. Taetz, G. Bleser, and M. Miezal. Towards self-calibrating inertial body motion capture. In Pro-
ceedings of the 19th International Conference on Information Fusion, pages 1751–1759, Heidelberg,
Germany, July 2016.
[145] D. Tedaldi, A. Pretto, and E. Menegatti. A robust and easy to implement method for IMU
calibration without external equipments. In Proceedings of the IEEE International Conference on
Robotics and Automation (ICRA), pages 3042–3049, Hong Kong, China, May–Jun. 2014.
[146] The Robot Operating System. http://www.ros.org/, 2017. Accessed on February 13.
[147] D. H. Titterton and J. L. Weston. Strapdown inertial navigation technology. IEE radar, sonar,
navigaton and avionics series 5. Peter Peregrinus Ltd. on behalf of the Institution of Electrical
Engineers, 1997.
[148] Trivisio Prototyping GmbH. http://www.trivisio.com, 2016. Accessed on October 24.
[149] J. F. Vasconcelos, G. Elkaim, C. Silvestre, P. Oliveira, and B. Cardeira. Geometric approach
to strapdown magnetometer calibration in sensor frame. IEEE Transactions on Aerospace and
Electronic systems, 47(2):1293–1306, April 2011.
[150] M. Verhaegen and V. Verdult. Filtering and system identification: a least squares approach. Cam-
bridge university press, 2007.
[151] Vicon. http://www.vicon.com, 2016. Accessed on August 5.
[152] T. von Marcard, B. Rosenhahn, M. Black, and G. Pons-Moll. Sparse inertial poser: Automatic 3D
human pose estimation from sparse IMUs. Computer Graphics Forum, 36(2):349–360, April 2017.
[153] A. Vydhyanathan, G. Bellusci, H. Luinge, and P. Slycke. The next generation Xsens motion trackers
for industrial applications. https://www.xsens.com/download/pdf/documentation/mti/mti_
white_paper.pdf, 2015.
[154] O. J. Woodman. An introduction to inertial navigation. University of Cambridge, Computer
Laboratory, Tech. Rep. UCAMCL-TR-696, 2007.
[155] x-io Technologies Limited. http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/,
2017. Accessed on February 13.
[156] Xsens Technologies B.V. http://www.xsens.com, 2016. Accessed on October 4.
[157] X. Yun and E. R. Bachmann. Design, implementation, and experimental results of a quaternion-
based Kalman filter for human body motion tracking. IEEE Transactions on Robotics, 22(6):1216–
1227, 2006.
[158] A. M. Zoubir, V. Koivunen, Y. Chakhchoukh, and M. Muma. Robust estimation in signal pro-
cessing: A tutorial-style treatment of fundamental concepts. IEEE Signal Processing Magazine,
29(4):61–80, 2012.
89