The SO(3) and SE(3) Lie Algebras of

Rigid Body Rotations and Motions and

arXiv:2205.12572v4 [cs.RO] 17 Aug 2023

their Application to Discrete Integration,

Gradient Descent Optimization, and State
Estimation

Universidad Politécnica de Madrid

Centro de Automática y Robótica

August 2023

Author: Eduardo Gallo

Abstract

Classical mathematical techniques such as discrete integration, gradient descent optimization, and state estima-
tion (exemplified by the Runge-Kutta method, Gauss-Newton minimization, and extended Kalman filter or EKF,
respectively), rely on linear algebra and hence are only applicable to state vectors belonging to Euclidean spaces
when implemented as described in the literature. This document discusses how to modify these methods so
they can be applied to non-Euclidean state vectors, such as those containing rotations and full motions of rigid
bodies.

To do so, this document provides an in-depth review of the concept of manifolds or Lie groups, together with
their tangent spaces or Lie algebras, their exponential and logarithmic maps, the analysis of perturbations, the
treatment of uncertainty and covariance, and in particular the definitions of the Jacobians required to employ
the previously mentioned calculus methods. These concepts are particularized to the specific cases of the SO (3)
and SE (3) Lie groups, known as the special orthogonal and special Euclidean groups of R3 , which represent the
rigid body rotations and motions, describing their various possible parameterizations as well as their advantages
and disadvantages.

Keywords: Lie algebra, SO(3), SE(3), manifold, tangent space, state estimation, EKF, discrete integration,
Runge-Kutta, gradient descent optimization, minimization, Gauss-Newton
Notation

This document is organized into chapters (e.g., chapter 4), sections (e.g., section 4.4), subsections (e.g., section
4.4.2), and so on. Note that the prefix “sub” is not employed when referring to subsections. Referencing tables
(e.g., table 4.5) and figures (e.g., figure 2.2) is also straightforward. When a reference appears in between
parenthesis, it refers to an equation or mathematical expression, as in (4.29), and when in between square
brackets, to a bibliographic reference, as in [1]. Acronyms are displayed in uppercase teletype font, and reference
to the proper location within the acronym list, as in EKF.

With respect to the mathematical notation, vectors and matrices appear in bold (e.g., x or R), and in general the
former are lowercase and the latter uppercase, although not always. A variable or vector with a hat over it < ˆ· >
refers to its estimated value, and with a dot < ·˙ > to its time derivative. Other commonly employed symbols for
vectors are the wide hat < b· >, which refers to its skew-symmetric form, the double vertical bars < k · k >, which
refer to its norm, and the wedge < ·∧ >, which implies a tangent space representation. The superindex T denotes
the transpose of a vector or matrix. In the case of a scalar, the vertical bars < | · | > refer to its absolute value.
The left arrow ←− represents an update operation, in which the value on the right of the arrow is assigned to
the variable on the left.

Superindexes are employed over vectors to specify the reference frame in which they are viewed or evaluated
(e.g., xA refers to the vector x viewed in frame A, while xB is the same vector but viewed in frame B). Where
two reference frames appear as subindexes to a vector, it means that the vector goes from the first frame to the
second. For example, xCAB refers to the vector x from frame A to frame B viewed in frame C1 .

Many different types of Jacobians or function partial derivative matrices can be found in this document (formal
definitions can be found in sections 3.3.4 and 3.4), and a special notation is hence required. Jacobians are
represented by a J combined with a subindex and a superindex:

• The subindex provides information about the function domain, and is composed by a symbol representing
how increments are added to the domain, followed by the domain itself. A + is employed when the domain
belongs to an Euclidean space2 but not when it is a manifold or Lie group; in these cases, ⊕ means that
increments are added in the local tangent space and ⊞ that they are added in the global tangent space.

• The superindex is composed by a second symbol representing how differences are computed in the function
image or codomain, followed by the codomain itself. A − is employed when the codomain is Euclidean3 ,
⊖ when the differences are evaluated in the local tangent space to the codomain manifold, and ⊟ when
evaluated in the global tangent space.

1
This is analogous to the vector x of frame B with respect to frame A viewed in frame C.
2
The subindex symbol may sometimes be omitted when it is clear from the context that the domain is Euclidean.
3
The superindex symbol may sometimes be omitted when it is clear from the context that the codomain is Euclidean.
Acronyms

CDF Cumulative Distribution Functions PDF Probability Density Function

ECEF Earth Centered Earth Fixed PMF Probability Mass Function
EKF Extended Kalman Filter PSD Power Spectral Density
NED North East Down ScLERP Screw linear interpolation
ODE Ordinary Differential Equation SLERP Spherical linear interpolation
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Contents

Abstract

Notation

Acronyms

1 Introduction and Outline 1

2 Stochastic Processes and Euclidean Space Methods 2

2.1 Random Variables, Stochastic Processes, and White Noise . . . . . . . . . . . . . . . . . . . . . . . . 2
2.1.1 Random Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
2.1.2 Random Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.1.3 Stochastic Processes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.1.4 White Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1.5 White Noise Gaussian Processes and their Integration . . . . . . . . . . . . . . . . . . . . . . 7
2.2 Robust Statistics and M-Estimators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3 Discrete Integration in Euclidean Spaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.4 Gradient Descent Optimization in Euclidean Spaces . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.5 State Estimation in Euclidean Spaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.5.1 Sampled Data Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.5.2 Sampled Observations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.5.3 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3 Introduction to Lie Algebra 20

3.1 Algebraic Structures, Maps, and Metric Spaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.2 Points, Vectors, and Axes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.3 Lie Groups and Lie Algebras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.3.1 Lie Algebra Velocities, Hat and Vee Operators . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.3.2 Exponential and Logarithmic Maps, Plus and Minus Operators . . . . . . . . . . . . . . . . . 24
3.3.3 Adjoint Action . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.3.4 Right and Left Lie Group Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.3.5 Lie Groups Uncertainty and Covariance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.4 Euclidean and Lie Jacobians . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.5 Discrete Integration in Lie Groups . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.6 Gradient Descent Optimization in Lie Groups . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.7 State Estimation in Lie Groups . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

4 Rotation of Rigid Bodies 34

4.1 Special Orthogonal (Lie) Group . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.2 Rotation Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.3 Rotation Vector as Tangent Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
4.4 Unit Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

i
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

4.4.1 Complex Numbers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.4.2 Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
4.4.3 Unit Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
4.5 Half Rotation Vector as Tangent Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
4.6 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.7 Rotational Motion Algebraic Operations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.7.1 Powers, Exponentials and Logarithms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.7.2 Spherical Linear Interpolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.7.3 Plus and Minus Operators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.8 Rotational Motion Time Derivative and Angular Velocity . . . . . . . . . . . . . . . . . . . . . . . . 45
4.9 Rotational Motion Point Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.10 Rotational Motion Adjoint . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.11 Rotational Motion Uncertainty and Covariance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.12 Rotational Motion Jacobians . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.13 Rotational Motion Discrete Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.14 Rotational Motion Gauss-Newton Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.15 Rotational Motion State Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.16 Applications of the Various Rotation Representations . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5 Motion of Rigid Bodies 51

5.1 Special Euclidean (Lie) Group . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
5.2 Affine Representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
5.3 Homogeneous Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5.4 Transform Vector as Tangent Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.5 Unit Dual Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.5.1 Dual Numbers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.5.2 Dual Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.5.3 Dual Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.5.4 Unit Dual Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.6 Half Transform Vector as Tangent Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.7 Screw as Tangent Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
5.8 Rigid Body Motion Algebraic Operations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.8.1 Powers, Exponentials and Logarithms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
5.8.2 Screw Linear Interpolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
5.8.3 Plus and Minus Operators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
5.9 Rigid Body Motion Time Derivative and Twist . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
5.10 Rigid Body Motion Point Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
5.11 Rigid Body Motion Adjoint . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
5.12 Rigid Body Motion Uncertainty and Covariance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
5.13 Rigid Body Motion Jacobians . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
5.14 Rigid Body Motion Discrete Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
5.15 Rigid Body Motion Gauss-Newton Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
5.16 Rigid Body Motion State Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
5.17 Applications of the Various Motion Representations . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

6 Relative Motion of Multiple Rigid Bodies 73

Bibliography 76

ii
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Chapter 1

Introduction and Outline

The classical implementations of common calculus techniques such as discrete integration (exemplified by the
Runge-Kutta method), gradient descent optimization (Gauss-Newton or Levenberg-Marquardt), and state esti-
mation (extended Kalman filter or EKF), are designed to work on state vectors belonging to Euclidean spaces1
and hence rely of linear algebra [2]. There exist two possible approaches for those cases in which the state vector
contains non-Euclidean components, such as rigid body motions or rotations:

• The solution most commonly observed in the literature is to incorporate each component of the pose2
(attitude) as an unconstrained real number into the state vector. The above techniques can then be
employed without difficulties, but the resulting state vector does not comply with the constraints imposed
by having some of its members being components of a rigid body pose (attitude), as these constraints have
not been taken into account when integrating, optimizing, or estimating. The solutions hence need to be
projected back into the space of valid rigid body poses (attitudes), but this does not hide the fact that the
whole process has been performed without respecting the motion (rotation) constraints, which often has
negative consequences for the accuracy, stability, and consistency of the solution [1].

• The approach taken in some recent robotics literature, in particular in the field of motion estimation for
navigation, consists on reformulating the above calculus techniques (integration, optimization, filtering)
taking into account that some members of the state vector represent rigid body poses (attitudes) and
hence can not be treated as Euclidean. By modeling these states properly, the quality of the solution can
be improved. The use of Lie theory, with its manifolds and tangent spaces, enables the construction of
rigorous calculus techniques to handle uncertainties, derivatives, and integrals of non-Euclidean elements
with precision and ease [1].

Chapter 2 discusses key statistical concepts such as random variables, stochastic processes, white noise, and
robust estimators, and describes the classical approaches (this is, when applied to Euclidean spaces) to discrete
integration, gradient descent optimization, and state estimation. Chapter 3 introduces the concepts of Lie groups
and their tangent spaces or Lie algebras, and adapts the three calculus techniques so they can be applied when the
state vector contains non-Euclidean Lie group components. Chapters 4 and 5 particularize the generic concepts
of chapter 3 to the specific cases of both rigid body rotations and complete motions, placing special emphasis on
the Lie Jacobians that enable the application of the modified calculus methods to non-Euclidean spaces. Last,
chapter 6 discusses the relationships among the velocities and accelerations (both linear and angular) of various
rigid bodies or references frames.

1
Although Euclidean spaces are formally defined in section 3.1, they can be informally understood as those that comply with the
five axioms of Euclidean geometry (1st things that are equal to the same thing are also equal to one another; 2nd if equals be added
to equals, the wholes are equal; 3rd if equals be subtracted from equals, the remainders are equal; 4th things that coincide with one
another are equal to one another; 5th the whole is greater than the part).
2
The pose of an object includes both its position and attitude.

1
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Chapter 2

Stochastic Processes and Euclidean Space

Methods

The material in this chapter can be found in many textbooks and technical documents. It is summarized here to
act as an introduction to the next chapters, which make frequent use of the concepts and equations discussed here.
Section 2.1 provides an introduction to statistics and the concepts of random variables, stochastic processes, and
white noise, followed by an overview of robust statistics and its associated estimators in section 2.2. The following
sections describe the classical approaches to three frequent calculus problems, such as discrete integration (section
2.3), gradient descent optimization (section 2.4), and state estimation (section 2.5). The solutions, known as the
Runge-Kutta integration method, the Gauss-Newton minimization, and the extended Kalman filter or EKF, are
intended for state vectors in which all components can be considered Euclidean. Note that the main objective of
this document is how to modify these three techniques so they can cope with non-Euclidean state vectors.

2.1 Random Variables, Stochastic Processes, and White Noise

This section provides an introduction to the random variables and processes required to model those physical
systems that can not be represented by deterministic models due to their inherent randomness, which results in
the same set of parameter values and initial conditions leading to different outputs.

2.1.1 Random Variables

Consider a random experiment (one in which the outcome is uncertain) with a sample space Ω (collection of
possible elementary outcomes of the experiment), and let ω be a sample point belonging to Ω. A random variable
X (ω) (generally just X) is a single valued real function that assigns a real number, called the value of X (ω), to
each sample point ω ∈ Ω [3, 4]. A random variable hence represents a map between the sample and real spaces
{X : Ω → R | ω ∈ Ω → X (ω) ∈ R}. The realization of a random variable is the real variable obtained after a
given experiment.

A random variable X is completely described by its cumulative distribution function (CDF) FX , which represents
the probability that the value of X is less or equal than the function input [5]:

FX (x) = P [X ≤ x] −∞<x<∞ (2.1)

Random variables can also be described by the CDF derivative, known as the probability mass function (PMF) pX
in case of discrete random variables (those that can take at most a countable number of possible values) or as
probability density function (PDF) fX for continuous ones (those that can take an uncountable number of possible

2
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

values):
X
FX (x) = pX (xk ) (2.2)
xk ≤x
Z x
FX (x) = fX (y) dy (2.3)
−∞

The expected value E [X] or mean µX of a random variable X is a function defined as its average value over a
large number of experiments, and represents its central or typical value:
X

 xk pX (xk ) when X is discrete

k
E [X] = µX = Z ∞ (2.4)


 y fX (y) dy when X is continuous
−∞

If a function acts on a random variable, then its output is also a random variable [6], and it is hence possible
2
to compute the expected value of the output random variable1 . The variance σX , Var (X), or second central
moment of a random variable X, is the expected value of the squared deviation of X from its mean, and measures
the spread of its PMF or PDF about its expected value [3], this is, how much the random variable is expected to
deviate from its mean. The square root of the variance is called the standard deviation σX .
X 2
 (xk − µX ) pX (xk ) when X is discrete
h i 

2 2 k
Var (X) = σX = E (X − µX ) = Z ∞ (2.5)

 2
 (y − µX ) fX (y) dy when X is continuous
−∞

 
The variance and mean of a random variable are related by the following expression, where E X2 is the second
moment of X.
h i    
2 2
σX = E (X − µX ) = E X2 − 2 µX E [X] + µ2X = E X2 − µ2X = µ 2 − µ2X (2.6)
X

2

2
The notation X ∼ µX , σX means that the random variable X has µX mean and σX variance. A normal or
2 2


Gaussian random variable X of parameters µX and σX [5], represented as X ∼ N µX , σX , is one whose PDF
responds to:
!
2
1 (x − µX )
fX (x) = p exp − 2 −∞<x<∞ (2.7)
2
2 π σX 2 σX

2

2
The expected value and variance of a normal random variable N µX , σX are µX and σX , respectively. A normal
random variable N (0, 1) of zero mean and unit variance is called a standard normal random variable. It is worth
noting that any affine2 function of a Gaussian random variable results in a Gaussian random variable [6].

The discrete uniform distribution assigns the same probability to each of its N possible values [3]. Represented
by X ∼ U (a, a + N − 1), its expected value is a + (N − 1)/2 and its variance is (N2 − 1)/12. Its PMF responds
to:
1
 xk = a, a + 1, . . . , a + N − 1
pX (xk ) = N (2.8)

0 otherwise

Consider now two random variables X and Y defined in the same sample space Ω with expected values µX and
1
Note that the mean can be considered as the expected value of the f (X) = X function.
2
In this contest affine means a function of the form y = a x + b, while linear means y = a x.

3
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

2 2
µY , respectively, and variances σX and σY . They are called independent if their results do not depend on each
other:

FXY (x, y) = P [X ≤ x, Y ≤ y] = P [X ≤ x] P [Y ≤ y] = FX (x) FY (y) (2.9)

The central limit theorem states that the sum of independent random variables tends towards a Gaussian random
variable, regardless of the CDF of the individual random variables that contribute to the sum [3, 6]. If a given
random variable X is realized many times, the law of large numbers states that the average of the realizations is
close to the random variable expected value µX , and tends to it as the numbers of realizations grows [3].

Stochastic simulations, also known as Monte Carlo simulations, use randomness to solve complex problems that
may have a deterministic nature [7, 8] and that are based on multiple unknown parameters, many of which are
difficult to obtain experimentally [9]. They rely on defining the domain of possible inputs, randomly generating
inputs from a probability distribution over the domain, performing a deterministic computation based on those
inputs, and finally aggregating the results by means of a set of metrics.

The correlation RXY and the covariance CXY of the random variables X and Y are two measures of the linear
correlation between both random variables [3]. They are defined as:

R (X, Y) = RXY = E [X · Y] = µX·Y (2.10)

C (X, Y) = CXY = E [(X − µX ) · (Y − µY )] = E [X · Y] − µX µY = RXY − µX µY (2.11)

Two random variables are uncorrelated if their covariance is zero (CXY = 0 → RXY = µX µY ). Independent
random variables are always uncorrelated, but not the other way around, as two uncorrelated random variables
may not necessarily be independent if there exists a nonlinear dependence between them. Orthogonal random
variables are those whose correlation is zero (RXY = 0), so they may or may not also be uncorrelated. If they
are, at least one of them is zero mean.

The expected value and variance of the sum and product of two random variables are of particular interest.
 2 2
Given two random variables X and Y with expected values {µX , µY } and variances σX , σY , its sum X + Y and
product X · Y are also random variables, as indicated above. The following expressions can be easily obtained by
applying the equations above3 [10]:

µX+Y = E [X + Y] = µX + µY (2.12)
h i
2 2
σX+Y = E (X + Y) − µ2X+Y = σX
2 2
+ 2 CXY + σY (2.13)
µX·Y = E [X · Y] = RXY = CXY + µX µY (2.14)
h i
2 2
σX·Y = E (X · Y) − µ2X·Y = C 2 2 − C2XY − 2 CXY µX µY + σX
2 2 2 2
σY + σX µY + µ2X σY
2
(2.15)
X Y

2.1.2 Random Vectors

T
A random vector X = [X1 . . . Xn ] is a collection of random variables obtained from the same sample space Ω
[3, 4]. The random vector joint CDF, PMF, and PDF are defined as follows:

FX1 ,...,Xn (x1 , . . . , xn ) = P [{X1 ≤ x1 } ∩ . . . ∩ {Xn ≤ xn }] = P [X1 ≤ x1 , . . . , Xn ≤ xn ] (2.16)

X X
FX1 ,...,Xn (x1 , . . . , xn ) = ··· pX1 ,...,Xn (k1 , . . . , kn ) (2.17)
k1 ≤x1 kn ≤xn
Z x1 Z xn
FX1 ,...,Xn (x1 , . . . , xn ) = ··· fX1 ,...,Xn (y1 , . . . , yn ) dyn . . . dy1 (2.18)
−∞ −∞

When the components X1 , . . . , Xn of the random vector X are independent from each other, its joint CDF, PMF,
and PDF are just the product of the respective functions of each of the random vector components [5]. The
3
These expressions can be further simplified when X and Y are uncorrelated.

4
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

expected value E [X] or mean µX and the variance σ 2X of a random vector X are defined as the vectors of those
of its components:
T
E [X] = µX = [µX1 . . . µXn ] (2.19)
 2 T
Var (X) = σ 2X = σX1 2
. . . σXn (2.20)

Given two random vectors X ∈ Rm and Y ∈ Rn , their correlation matrix RXY is defined so Rij = R (Xi , Yj ),
while their covariance matrix CXY verifies that Cij = C (Xi , Yj ) [5]:
h i
R (X, Y) = RXY = E X YT (2.21)
h i h i
T
C (X, Y) = CXY = E (X − µX ) (Y − µY ) = E X Y T − µX µT T
Y = RXY − µX µY (2.22)

The autocorrelation and autocovariance matrices RXX and CXX of a random vector X ∈ Rm are de-
fined
 as the correlation and
 covariance matrices of that vector with itself. Both are square, symmetric
T T T T m


RXX = RXX , CXX = CXX , positive semidefinite z RXX z ≥ 0, z CXX z ≥ 0, ∀ z ∈ R , and their diagonals
contain the second moments and variances of each of the random variables Xi within X:
h i
R (X) = RXX = E X XT (2.23)
h i h i
T
C (X) = CXX = E (X − µX ) (X − µX ) = E X XT − µX µT X = RXX − µX µX
T
(2.24)

A normal or Gaussian random vector is that whose components are all normal random variables. As in the case of
scalar random variables, an affine transformation of a Gaussian random vector results in a new Gaussian random
vector.

2.1.3 Stochastic Processes

A random process or stochastic process enlarges the concept of random vector (or random variable when
the vector size is one) to include time. Given a sample vector ω of the sample space Ω and a pa-
rameter t belonging to a parameter set T (generally time), a stochastic process assigns a real vector
{X : T, Ω → Rm | t ∈ T, ω ∈ Ω → X (t, ω) ∈ Rm } [3, 4, 11]. If the sample vector ω is fixed, the random pro-
cess X (t) behaves as a function of time; on the other hand, if the time is fixed, the stochastic process X (ω)
defaults to a random vector. A stochastic process is thus a family of random vectors (either discrete or continu-
ous) indexed by a continuous parameter t ∈ T. If the parameter is discrete t ∈ Z+ 4 , then the appropriate name
is stochastic sequence [5, 6].

Size one stochastic processes X (t, ω), generally represented just by X (t), are completely described by their CDF,
while if X (t) is instead a random vector, it is represented by its joint CDF:

FX (x; t) = P [X (t) ≤ x] (2.25)

FX (x1 , . . . , xn ; t) = P [X1 (t) ≤ x1 , . . . , Xn (t) ≤ xn ] (2.26)

The joint PMF and PDF are also defined in similar fashion. The ensemble average or mean of a random process
becomes a function of time:

µX (t) = E [X (t)] (2.27)

Note that the random process X (t) evaluated at different times comprises different random vectors of the same
size. It is then possible to apply the concepts of autocorrelation and autocovariance introduced in section 2.1.2
to any two of these random vectors, providing quantitative measures of the similarity of the random process at
two different times, this is, measuring by how much a signal is similar to its time shifted version [3]. This results
4 +
Z represents the set of positive integers.

5
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

in the autocorrelation RXX (t, t + τ ) and the autocovariance CXX (t, t + τ ):

h i
RXX (t, t + τ ) = E X (t) XT (t + τ ) (2.28)
h

T i
CXX (t, t + τ ) = E X (t) − µX (t) X (t + τ ) − µX (t + τ )
h i
= E X (t) XT (t + τ ) − µX (t) µT T
X (t + τ ) = RXX (t, t + τ ) − µX (t) µX (t + τ ) (2.29)

The autocovariance is zero when the two observations of X are independent, meaning that there is no coupling
between X (t) and X (t + τ ), and they are called uncorrelated. As with the random vectors, the reverse is not
true, as two uncorrelated observations does not necessarily mean that they are independent.

A wide sense stationary process is that in which the mean does not vary with time and the autocorrelation
depends exclusively on the time difference5 :

µXWSS (t) = E [XWSS (t)] = µXWSS (2.30)

h i
RXXWSS (t, t + τ ) = E XWSS (t) XT WSS (t + τ ) = RXXWSS (τ ) (2.31)

Consider now two stochastic processes X (t) and Y (t) defined in the same sample space Ω. The crosscorrelation
RXY (t, t + τ ) and crosscovariance CXY (t, t + τ ) measure how similar two different processes (or signals) are
when one is time shifted with respect to the other [3]:
h i
RXY (t, t + τ ) = E X (t) Y T (t + τ ) (2.32)
h

T i
CXY (t, t + τ ) = E X (t) − µX (t) Y (t + τ ) − µY (t + τ )
h i
= E X (t) Y T (t + τ ) − µX (t) µT T
Y (t + τ ) = RXY (t, t + τ ) − µX (t) µY (t + τ ) (2.33)

Two processes X (t) and Y (t) are orthogonal if their crosscorrelation is zero for all t and t + τ , while they are
uncorrelated if their crosscovariance is zero. They are jointly wide sense stationary if their crosscorrelation is
independent of the absolute time:

RXYWSS (t, t + τ ) = RXYWSS (τ ) (2.34)

Consider also a stochastic process X (t) that has one realization x (t). It is then possible to define the time
   
average A X (t) and the time autocorrelation R X (t) , τ for continuous processes as:
Z T
  1
A X (t) = lim x (t) dt (2.35)
T→∞ 2 T −T
  h i
R X (t) , τ = A X (t) XT (t + τ ) (2.36)

The discrete time definitions can be derived accordingly. Finally, an ergodic process [6] is a stationary random
process for which
 
AERG X (t) = EERG [X] (2.37)
 
RERG X (t) , τ = RXXERG (τ ) (2.38)

2.1.4 White Noise

If any two random vectors X (t1 ) and X (t2 ) taken from a stochastic process X (t) are independent for all t1 6= t2 ,
then the random process X (t) is called white noise. Otherwise, it is known as colored noise [6].

The whiteness or color content of a stochastic process can be characterized by its power spectrum or power
spectral density (PSD) SXX (ω). For wide sense stationary processes, it is defined as the Fourier transform of its
5
Strict sense stationary processes are those in which the complete CDF is time invariant, not only its mean and autocorrelation.
The definition is usually too restrictive for any practical use.

6
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

autocorrelation function RXX (τ ) [6]:

 X ∞



 RXX (k) exp (−i ω k) ω ∈ [−π, π] when X is discrete
SXX (ω) = Zk=−∞ (2.39)

 ∞

 RXX (τ ) exp (−i ω τ ) dτ when X is continuous
−∞

The autocorrelation can be recovered by means of the inverse Fourier transform:

 Z ∞
 1

RXX (k) = 2π SXX (ω) exp (i ω k) dω when X (t) is discrete
−∞
Z ∞ (2.40)

 1
RXX (τ ) = SXX (ω) exp (i ω τ ) dω when X (t) is continuous
2π −∞

In case of continuous time wide sense stationary stochastic processes6, the power is defined as:
Z ∞
1
PXX = SXX (ω) dω (2.41)
2π −∞

In the case of two continuous time jointly wide sense stationary stochastic processes X (t) and Y (t), the cross
power spectrum SXY (ω) is defined as the Fourier transform of their crosscorrelation RXY (τ ) [6]:
Z ∞
SXY (ω) = RXY (τ ) exp (−i ω τ ) dτ (2.42)
−∞
Z ∞
1
RXY (τ ) = SXY (ω) exp (i ω τ ) dω (2.43)
2π −∞

A white noise process N (t) (continuous time) or N (k) (discrete time) is one whose PSD is constant for all
frequencies, this is, a random process having equal power at all frequencies [5]. These processes do not have any
correlation with themselves except at the present time [6]. The definition for discrete time processes relies on the
Kronecker delta function δk 7 :

RNN (k) = σ 2 δk (2.44)

2
SNN (ω) = σ = RNN (0) ∀ ω ∈ [−π, π] (2.45)

while that for continuous time processes makes use of the impulse Dirac delta function δ (τ )8 :

RNN (τ ) = σ 2 δ (τ ) (2.46)
2
SNN (ω) = σ = RNN (0) ∀ω ∈ R (2.47)

2.1.5 White Noise Gaussian Processes and their Integration

A Gaussian process is a stochastic process {X (t) , t ∈ T} in which for any choice of n real coefficients a1 , . . . , an
and choice of k time instants t1 , . . . , tk in the time set T, the random variable a1 X (t1 ) + . . . + ak X (tk ) is a
normal random variable. It can be proven that the random variables X (t1 ) , . . . , X (tk ) are mutually uncorrelated
and independent. A Gaussian process hence associates the random variable occurring at any time instant with
a normal distribution, and is fully defined by its expected value and covariance function [3], so strict and wide
sense stationary processes coincide in the case of Gaussian processes.
6
Similar expressions can be easily obtained for discrete time processes.
7
The Kronecker delta function δ (k) is valued 0 for all k except at k = 0, where it is 1.
8
The Dirac delta function δ (τ ) is valued 0 everywhere except at τ = 0, where it is ∞. Its integral over any space containing τ = 0
is 1.

7
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Considering now9 that µ (t) is a zero mean white noise Gaussian process with PSD σ 2 :

E [µ (t)] = 0 E [µ (t) µ (τ )] = σ 2 δ (t − τ ) (2.48)

and also consider a standard normal random variable N (0, 1) so that σ N is identically distributed to µ (t):
 
E [σ N] = 0 Var (σ N) = E σ 2 N2 = σ 2 (2.49)

The integration of a white noise Gaussian process is known as a 1st order random walk [12], characterized by
variances that grow linearly with time as well as PSDs that fall off as the inverse of the square of the sampling
frequency [13]. µ (t) can then be integrated over a timespan k ∆t by means of the rectangular rule, and its
expected value and variance evaluated:
Z k ∆t k
X
a (t) = a (k ∆t) = µ (τ ) dτ = ∆t σ Ni (2.50)
0 i=1
E [a (t)] = σ k ∆t E [N] = 0 (2.51)

h
2 i k
X  
Var a (t) = E a (t) − E [a (t)] = σ 2 ∆t2 E N2i = σ 2 ∆t2 k = σ 2 t ∆t (2.52)
i=1

The second integration of a white noise Gaussian process is known as a 2nd order random walk :
Z k ∆t Z tZ υ
b (t) = b (k ∆t) = a (τ ) dτ = µ (τ ) dτ dυ
0 0 0
k
X i
X k
X
= ∆t ∆t σ Nj = ∆t2 σ (k − i + 1) Ni (2.53)
i=1 j=1 i=1
k
X
2
E [b (t)] = ∆t σ (k − i + 1) E [Ni ] = 0 (2.54)
i=1

h
2 i k
X  
2
Var b (t) = E b (t) − E [b (t)] = ∆t4 σ 2 (k − i + 1) E N2i
i=1
2 2
σ σ 3
= ∆t4 k (k + 1) (2 k + 1) ≈ t ∆t (2.55)
6 3

The third integration of a white noise Gaussian process is known as a 3rd order random walk :
Z k ∆t Z tZ υZ κ
c (t) = c (k ∆t) = b (τ ) dτ = µ (τ ) dτ dκ dυ
0 0 0 0
 
Xk X i X j k
X k−i+1
X
= ∆t ∆t ∆t σ Nk = ∆t3 σ  j Ni  (2.56)
i=1 j=1 k=1 i=1 j=1
k k−i+1
X X
E [c (t)] = ∆t3 σ j E [Ni ] = 0 (2.57)
i=1 j=1
 2

h
2 i k
X k−i+1
X   σ2 σ2 5
Var c (t) = E c (t) − E [c (t)] = ∆t6 σ 2  j E N2i ≈ ∆t6 k5 = t ∆t (2.58)
i=1 j=1
20 20

Note that while the expected values or means of all random walks are zero, their variances grow with the 1st , 3rd ,
and 5th powers of time, respectively, so the expected deviations with respect to the zero means at any given time
are proportional to t1/2 , t3/2 , and t5/2 respectively.
9
It is necessary to point out that many of the expressions obtained in this section are the result of long but not necessarily complex
mathematical operations relying on the repeated application of the expressions derived in section 2.1. The interested reader should
not have many problems in obtaining the same results as shown here.

8
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

2.2 Robust Statistics and M-Estimators

Classic statistical methods rely heavily on conditions often not met in practice, such as data errors being normally
distributed, or that it is possible to apply the central limit theorem to obtain normally distributed estimates. As
a result, they work poorly when there exist outliers in the data. Robust statistical methods are those that emulate
the classic ones but are not unduly affected by outliers or other small departures from the model assumptions, in
particular the lack of normally distributed inputs. A robust statistic is resistant to errors in the results produced
by deviations from the normality assumption.

A measure of central tendency provides the central or typical value of a random variable or probability distribution,
and can also be called the center, location, or average of the distribution. The classic measure of central tendency
is the mean µX introduced in section 2.1.1, but the median ηX , defined as the middle value that separates the
higher valued half from the lower half in the data set, provides better results if outliers are present10 .

The dispersion, variability, scatter, or spread of a random variable or distribution measures the extent to which
a distribution is stretched or squeezed, quantifying the statistical dispersion of the data, and is often used to
estimate the scale, and hence these are known as measures of scale. The classic measures such as the variance
2
σX and standard deviation σX introduced in section 2.1.1 are very sensitive to contaminated data. On the other
hand, the median absolute deviation MADX , defined as the median of the absolute values of the differences
between the data values and the overall median of the data set, is more robust and should replace the standard
deviation if the normality of the input data can not be guaranteed11 . To do so, it must be corrected with the
following factor to ensure results coincide if applied to a normal distribution:

MADX
σX ≈ 1.4826 MADX = (2.59)
0.6745

Median
Mean
Huber
Tukey
Υ (x)

w (x)
ρ (x)

x x x

Figure 2.1: Robust estimators (not to scale)

An statistical estimator is a method to estimate the coefficients of a parametric model based on observations of
a data set that comply with certain assumptions. Extremum estimators of parametric models are those that can
be calculated by minimization of a certain objective function Qn that depends on the n samples:

θ̂ = arg min Qn (θ) (2.60)

θ∈Θ

where Θ is the parameter space. If the objective function is a sampled measure of central tendency, the estimator
is known as an M-estimator, and can be expressed as follows [14]:

n
!
X
θ̂ = arg min ρ (xi , θ) (2.61)
θ∈Θ i=1

where ρ is a function with certain properties [14]. Least squares (minimum of the sum of squares of the differences
10
An additional measure of central tendency is the mode, or most frequent value in the data set.
11
An additional measure of scale is the inter quartile range (IQR) or difference between the 75% and 25% percentile of a sample.

9
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

between the samples and a given value) and maximum likelihood (ML) estimators (derivative of likelihood function
with respect to parameters is zero) are both special cases of M-estimators. The function ρ and its derivative
Υ can be chosen to provide the estimator with desirable properties when the data are truly from the assumed
distribution, and to be robust when it comes from a model that is close but does not comply with the input
X
assumptions. The minimization of ρ (xi , θ) can always be done directly, but it is often simpler to derivate with
respect to x and solve for the root of the derivative. When this is possible (most practical cases), the M-estimator
if of Υ-type; if not, ρ-type.

Given a function ρ (x), known as the objective or error function, and its derivative with x Υ (x) = dρ (x) /dx,
the weight function ω (x) = Υ (x) /x represents the relative weight given to each observation. When the observed
samples can only be positive and do not need to be squared, the objective function is usually replaced by
√

̺ (x) = ρ x ; its derivative ̺′ (x) = d̺ (x) /dx coincides with the weight function once the scale (1/2) is removed
and x replaced by x2 , but it represents the same concept.

Median
Mean
Huber

̺′ x 2
Tukey
̺ x2

x2 x2

Figure 2.2: Robust estimators (not to scale)

The most common M-estimators [15] are listed below, where the expressions consider with no loss of generality
that the samples are errors and hence ρ (x) should have a minimum at zero12 :

• The absolute error or median estimator employs ρη (x) = |x|. Although not differentiable, methods that rely
of its Υ-type can use Υη (x) = sign (x). The weight wη (x) = 1/|x| of each sample is inversely proportional
to its absolute value.

• The squared error or mean estimator relies on ρµ (x) = x2 /2 or Υµ (x) = x, and it represents the least
squares method employed to minimize the sum of squared differences. All data samples are assigned the
same weight as wµ (x) = 1.

• The Huber estimator behaves as the mean (squared error) when the sample absolute value is inferior to
a certain threshold δHUB , where all samples are weighted equally, and as a median estimator (absolute
error) when superior, reducing the sample weight as its absolute value increases. As outliers are generally
characterized by a big value x, their relative weight with respect to the remaining samples is reduced.
( 2
x /2 |x| ≤ δHUB
ρHUB (x) =   (2.62)
δHUB |x| − δHUB /2 |x| > δHUB
(
1 |x| ≤ δHUB
wHUB (x) = (2.63)
δHUB / |x| |x| > δHUB

• The bisquare or Tukey estimator provides a much quicker weight reduction with absolute value than the
Huber estimator, as the diminution starts at 0 and is complete at δTUK , above which the samples are
12
If this is not the case, just replace x by x − xMIN .

10
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

neglected.
 2 2

x 1 − x
 x4
+ |x| ≤ δTUK
ρTUK (x) = 2 δTUK 2 3 δTUK 4 (2.64)


δTUK 2 / 6 |x| > δTUK
 
 1 − x2 / δTUK 2 2
|x| ≤ δTUK
wTUK (x) = (2.65)
0 |x| > δTUK

Smaller values of the tuning constants δHUB and δTUK provide more resistance to outliers at the expense of
lower efficiency when the errors are normally distributed; their values are usually set to δHUB = 1.345 σX and
δTUK = 4.685 σX [15], where σX represents the inputs standard deviation provided by (2.59).

2.3 Discrete Integration in Euclidean Spaces

Let x (t) ∈ Rm be an Euclidean space time varying state vector for which its value at a given dis-
crete time xk = x (tk ) is known. The objective is to determine the state vector value at a later time
xk+1 = x (tk+1 ) = x (tk + ∆t) by relying on evaluations of the state vector derivative with time:


ẋ (t) = f x (t) , t (2.66)

The initial value first order ordinary differential equation (ODE) problem can be solved with varying degrees of
complexity and accuracy [16], three of which are described below:

• Euler’s method is a first order approach that relies on evaluating the time derivative at tk and considering
that its value does not change for the duration of the integration interval ∆t. Its error is proportional to
the square of the integration interval:

xk+1 ≈ xk + ∆t ẋ(xk , tk ) (2.67)

• Heun’s method is a second order approach that requires two evaluations of the time derivative. The constant
gradient is estimated as the average between the time derivative evaluation at the initial state and that at
the result of Euler’s method, and results in an error proportional to the cube of the integration interval:

v1 = ẋ(xk , tk ) (2.68)
v2 = ẋ(xk + ∆t v1 , tk + ∆t) (2.69)
∆t
xk+1 ≈ xk + [v1 + v2 ] (2.70)
2

• The 4th order Runge-Kutta method is the de facto standard and relies on four evaluations of the state vector
time derivative to obtain an error proportional to the fifth power of the integration interval:

v1 = ẋ(xk , tk ) (2.71)
∆t v1 ∆t
v2 = ẋ(xk + , tk + ) (2.72)
2 2
∆t v2 ∆t
v3 = ẋ(xk + , tk + ) (2.73)
2 2
v4 = ẋ(xk + ∆t v3 , tk + ∆t) (2.74)
hv v2 v3 v4 i
1
xk+1 ≈ xk + ∆t + + + (2.75)
6 3 3 6

11
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

2.4 Gradient Descent Optimization in Euclidean Spaces

Let x ∈ Rm be an Euclidean vector, {f : Rm → Rn | f (x) ∈ Rn , ∀ x ∈ Rm } a nonlinear map for which it is
also possible to evaluate its Jacobian {J : Rm → Rnxm | J (x) = ∂f (x)/∂x ∈ Rnxm , ∀ x ∈ Rm }, and E (x) = f (x)
− f T ∈ Rn an error or cost function containing the difference between the map f and a target result f T .

The objective is to determine an input vector x = x0 + ∆x in the vicinity of a known initial value x0 , for which
the cost function norm kE (x) k ∈ R holds a local minimum, this is, kE (x0 + ∆x) k < kE (x0 ) k, ∀ x0 ∈ Rm . The
Gauss-Newton optimization method provides a solution to this problem that relies on iteratively advancing the
solution per (2.76) starting with x0 :

xk+1 ←− xk + ∆xk (2.76)

Adopting a lighter notation in which f k = f (xk ), Jk = J (xk ), and E k = E (xk ), the process concludes when the
step diminution of the cost function norm falls below a given threshold (kE k k − kE k+1 k < δ). The Gauss-Newton
method consists on linearizing each step by performing a first order Taylor expansion of the cost function before
minimizing its norm by equaling its derivative with respect to ∆xk to zero [17]:

E k+1 = f k+1 − f T ≈ f k + Jk ∆xk − f T = E k + Jk ∆xk (2.77)

kE k+1 k = ET T T T T T
k+1 E k+1 = E k E k + ∆xk Jk Jk ∆xk + 2 ∆xk Jk E k (2.78)
∂kE k+1 k
−1
= 0 −→ 2 JT T T
k Jk ∆xk + 2 Jk E k = 0 −→ ∆xk = − Jk Jk JT
k Ek (2.79)
∂∆xk

The Gauss-Newton algorithm is just one type of a more generic class of iterative minimization methods grouped
under the name of gradient descent methods. The Newton method relies on minimizing (equaling its ∆xk deriva-
tive to zero) a second order Taylor expansion of the cost function norm N (x) = kE (x) k ∈ R, which requires

the computation of both its gradient ∇ : Rm → R1xm | ∇ (x) = ∂N (x)/∂x ∈ R1xm , ∀ x ∈ Rm and its Hessian

H : Rm → Rmxm | H (x) = ∂ 2 N (x)/∂x2 ∈ Rmxm , ∀ x ∈ Rm at each step, resulting in:

∆xk = −H−1 T
k ∇k (2.80)

As the Hessian H can be difficult or expensive to compute, there exist several approximations that reduce
the computational cost of each step, such as the steepest descent method, which replaces the Hessian with the
product of a constant and the identity matrix Im ∈ Rmxm , and the diagonal approximation, which sets to zero all
H components outside its main diagonal. In this sense, the Gauss-Newton method is just another approximation
that employs a first order simplification of the Hessian, as proven in [18].

The convergence of none of these methods is guaranteed. In general, both Gauss-Newton and Newton work better
near the local minimum, where the quadratic approximation is good, but may diverge when the initial value is
further away, where the steepest descent and diagonal approximation methods may be more robust. To ensure
that the error gets smaller in each iteration, it may be convenient to advance with a smaller ∆xk step. The
Levenberg-Marquardt algorithm employs a varying ratio between the Gauss-Newton (or Newton) and diagonal
approximations to the Hessian, moving towards the former when the error kE k k decreases, and towards the latter
while repeating the step if it increases.

2.5 State Estimation in Euclidean Spaces

State estimation is the problem of determining the value of the state of a dynamic system based on a series of
noisy equations that describe the evolution of the state with time, together with a series of noisy measurements or
observations of variables that also depend on the state. State or state vector refers to those variables that provide
a representation of the condition or status of the system at a given instant in time. Section 2.5.1 discusses the

12
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

equations that describe the system dynamics, this is, the state evolution with time, and what is the best possible
estimation of the state that can be obtained from them. Section 2.5.2 describes the measurement or observation
equations, and also reaches the best possible state estimate from the information they contain. Both approaches
are combined in section 2.5.3, which describes the extended Kalman filter or EKF, the most widely used nonlinear
state estimation algorithm.

2.5.1 Sampled Data Systems

A state space system is a mathematical representation of a physical process in which the variables (both state
and input) are related by first order differential equations (for continuous systems) or difference equations (for
discrete ones). If the state of the system (the value of the state variables) is known at a given time, and so are
all the present and future inputs (the evolution with time of the input variables), it is then possible to obtain the
evolution with time of all the state variables.

A sampled data system is one whose dynamics are described by continuous time differential equations, but whose
inputs only change at discrete time instants. Additionally, it is only necessary to estimate the state variables, or
to be precise its mean and covariance13, at those same discrete time instants [6]. A continuous time nonlinear
state system can be written as


ẋ (t) = f x (t) , u (t) , w (t) , t (2.81)

where x ∈ Rm is the state vector, u ∈ Rn is the known control or input vector, and w ∈ Rp is the process noise.
These three vectors may have different sizes. Consider also that the process noise w (t) can be modeled by a zero
mean continuous time white noise random process14 of covariance Qc (sections 2.1.1 and 2.1.4):

w (t) ∼ (0, Qc ) (2.82)

 
Rww (t, τ ) = E w (t) wT (τ ) = Qc δ (t − τ ) (2.83)

2.5.1.1 Linearization of Continuous Time Systems

The dynamics represented by (2.81) can be linearized by performing a Taylor expansion around an unknown
nominal state xN (t) and process noise wN (t)15 , assuming without loss of generality that wN (t) = 0. If it is not,
it can be written as the sum of a zero mean part and a known deterministic part, which can then be added to the
control vector. The expansion is truncated so only the first order terms remain, introducing linearization errors;
these are higher the more nonlinear that f (x, u, w, t) is with respect to x and w, and the farther away that
x (t) lies from xN (t) and w (t) from wN = 0 [6].
 
∂f ∂f ∂f ∂f ∂f
ẋ (t) ≈ f |N + (x − xN ) + w= x + f |N − xN + w (2.84)
∂x N ∂w N ∂x N ∂x N ∂w N

where |N stands for evaluation at xN (t) , u (t) , 0, t . The state system is now continuous time but linear:

ẋ (t) ≈ A (t) x (t) + B (t) u e (t) + w

e (t) (2.85)
∂f

A (t) = xN (t) , u (t) , 0, t (2.86)
∂x
B (t) = I (2.87)
∂f

L (t) = xN (t) , u (t) , 0, t (2.88)
∂w

e (t)
u = f xN (t) , u (t) , 0, t − A (t) xN (t) (2.89)
   
e (t)
w = L (t) w (t) ∼ 0, L Qc LT = 0, Q e (t) (2.90)
c

13
Although the term covariance is traditionally employed in state estimation, it is in fact referring to the state random vector
autocovariance provided by (2.24), or to the state random process autocovariance given by (2.29), depending on context.
14
Note that the process noise does not need to be Gaussian.
15
As the input vector u (t) is known, there is no need to expand around it.

13
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

h i
Rw
ewe (t, τ ) = E we (t) w e c (t) δ (t − τ )
e T (τ ) = Q (2.91)

The above linear state system is based on a unitary input matrix B ∈ Rmxm and a system matrix A (t) ∈ Rmxm
that is the Jacobian of the nonlinear system with respect to the state vector evaluated at the unknown nominal
state. It also employs modified input ue (t) ∈ Rm and process noise w
e (t) ∈ Rm vectors.

2.5.1.2 Comparison of Integrated Continuous and Discrete White Noise Processes

Before continuing, this section compares the behavior of an integrated continuous white noise process with
that of a discrete one, as the result is essential to the discretization of the continuous time state system
(2.85). According to section 2.1.4, a continuous zero mean white noise is defined by w (t) ∼ (0, Qc ) and
 
E w (t) wT (τ ) = Qc δ (t − τ ), while a zero mean discrete time white noise process responds to wk ∼ (0, Qd )
 
and E wk wT l = Qd δk−l . The variation with time of the mean µz (t) and covariance Czz (t) of the noise z (t)
resulting from the integration of the continuous white noise with ż (t) = w (t) , z (0) = 0 are the following:
Z t  Z t
µz (t) = E [z (t)] = E w (α) dα = E [w (α)] dα = 0 (2.92)
0 0
Z t Z t 
 
Czz (t) = E z (t) zT (t) − µz (t) µT z (t) = E w (α) dα w T
(β) dβ
0 0
Z tZ t Z tZ t Z t
 
= E w (α) wT (β) dα dβ = Qc δ (α − β) dα dβ = Qc dβ = Qc t (2.93)
0 0 0 0 0

This expression shows that the mean of an integrated continuous white noise is always zero, but its covariance
grows linearly with time. Integrating now the difference equation zk = zk−1 + wk−1 , z0 = 0, the variation with
time of the mean µk and covariance Czz,k of the integrated noise zk are the following:
"k−1 # k−1
X X
µk = E [zk ] = E wl = E [wl ] = 0 (2.94)
l=0 l=0
"k−1 k−1
# k−1 k−1
  X X X X  
Czz,k = E zk zT T
k − µk µk = E wl wT
m = E wl wT
m
l=0 m=0 l=0 m=0
k−1
X k−1
X k−1
X
= Qd δl−m = Qd 1 = Qd k (2.95)
l=0 m=0 l=0

The covariance of the integrated discrete white noise process also grows linearly with time. Considering a sampling
period of ∆t, t = k · ∆t, a discrete zero mean white noise process can be considered equivalent [6] to a continuous
one if their covariances are related by:

Qd = Qc · ∆t (2.96)

2.5.1.3 Discretization of Linear Continuous Time Systems

Returning to the main argument, and considering that the state vector needs to be known only at a series of
discrete time points, it is possible to discretize the (2.85) linear continuous time system if A (t), B (t), and
e (t) are considered constant during the integration interval, which starts at tk−1 = (k − 1) · ∆t and concludes
u
at tk = k · ∆t. The introduced discretization errors are higher the farther away this assumption is from reality.
Introducing (2.96), the state system is now discrete and linear [6]:

xk e k−1 + w
≈ Fk−1 xk−1 + Gk−1 u e k−1 (2.97)
xk = x (tk ) = x (k ∆t) (2.98)


Fk = exp (Ak ∆t) = exp A (k ∆t) ∆t (2.99)
Z ∆t


Gk = Fk exp − A (τ ) τ dτ B (k ∆t)
0

14
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .



= Fk I − exp − A (k ∆t) ∆t A−1 (k ∆t) B (k ∆t) (2.100)
ek
u e (tk ) = u
= u e (k ∆t) (2.101)
     
ek
w e (k ∆t) ∼ 0, Q
= w e (k · ∆t) · ∆t = 0, Lk Q LT ∆t = 0, Q
e (2.102)
c c k d,k
h i
Rw
ewe ,kj = E w ek weT =Q e δk−j (2.103)
j d,k

Note that both the system state transition matrix Fk ∈ Rmxm and the input transition matrix Gk ∈ Rmxm
make use of the matrix exponential function, although computing the later is not required, as shown in sec-
tion 2.5.3.2.

2.5.1.4 Mean and Covariance of State Vector

It is possible to evaluate the mean µx,k and covariance Cxx,k = Pk of the state vector given by (2.97), which
provide their variation with time16 :

µx,k e k−1
= E [xk ] = Fk−1 µx,k−1 + Gk−1 u (2.104)
h

T i
Cxx,k = Pk = E xk − µx,k xk − µx,k
  T 



= E Fk−1 xk−1 − µx,k−1 + w e k−1
e k−1 Fk−1 xk−1 − µx,k−1 + w

= Fk−1 Cxx,k−1 FT e T e
k−1 + Qd,k−1 = Fk−1 Pk−1 Fk−1 + Qd,k−1 (2.105)

Based on (2.97), xk is a linear combination of a series of known real vectors u e0 , . . . , u

e k−1 plus a series of in-
dependent random vectors x0 , w e 0, . . . , w
e k−1 . According to the central limit theorem stated in section 2.1.1,



xk ∼ N µx,k , Cxx,k = N µx,k , Pk is a normal or Gaussian random vector completely characterized by its
mean and covariance.

The summary of this section is that given a continuous time nonlinear state space system such as (2.81), it is
possible, with some linearization and discretization errors, to transform it into an equivalent discrete time linear
system (2.97) that can be integrated to obtain the estimated value of the state vector x (t) at a series of discrete
times tk = k ∆t characterized by its mean µx,k (2.104) and covariance Cxx,k = Pk (2.105). Without further
assistance, (2.105) shows that the uncertainty of the results grows with time because of the accumulation of the
white noise present in the system [6]. The next section shows how the addition of measurements can solve this
problem.

2.5.2 Sampled Observations

Given the sampled data system of section 2.5.1, it is possible to consider that there exist a series of sensors
capable of measuring certain variables related to the state vector at the same time points at which the state
system is discretized in section 2.5.1.3:

yk = h (xk , vk , tk ) (2.106)

where yk = y (tk ) ∈ Rq is the measurement or observation vector provided by the sensors, xk = x (tk ) ∈ Rm is
the state vector, tk = t (k ∆t) is the discrete time at which the measurements are taken, and vk = v (tk ) ∈ Rq is
the measurement or observation noise, which can be modeled by a zero mean white noise random process17 of
covariance R (sections 2.1.1 and 2.1.4):

vk ∼ (0, R) (2.107)
 
Rvv,kj = E vk vTj = R δk−j (2.108)
16


To compute the covariance, note that there is no correlation between xk−1 − µxk−1 and w
e k−1 .
17
Note that the measurement noise does not need to be Gaussian.

15
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Consider also that the measurement noise and the process noise of section 2.5.1 are orthogonal:
 
Rvw,kj = E vk wT
j =0 (2.109)

2.5.2.1 Linearization of Observations

The discrete observations represented by (2.106) can be linearized by performing a Taylor expansion around an
unknown nominal state xNk = xN (tk ) and observation noise vNk = vN (tk ), assuming without loss of generality
that vNk = 0. If it is not, it can be written as the sum of a zero mean part and a known deterministic part,
which can then be included in the nonlinear function h. The expansion is truncated so only the first order terms
remain, introducing linearization errors; these are higher the more nonlinear that h (xk , vk , tk ) is with respect
to xk and vk , and the farther away that xk is from xNk and vk from vNk = 0 [6].
 
∂h ∂h ∂h ∂h ∂h
yk ≈ h|N + (xk − xNk ) + vk = xk + h|N − xNk + vk (2.110)
∂xk N ∂vk N ∂xk N ∂xk N ∂vk N

where |N stands for evaluation at (xNk , 0, tk ). The observations system is now discrete time and linear:

yk ≈ Hk xk + zk + v ek (2.111)
∂h
Hk = H (tk ) = (xNk , 0, tk ) (2.112)
∂xk
zk = z (tk ) = h (xNk , 0, tk ) − Hk xNk (2.113)
∂h
Mk = M (tk ) = (xNk , 0, tk ) (2.114)
∂vk
   
ek
v e (tk ) = Mk vk ∼ 0, Mk R MT
= v ek
= 0, R (2.115)
k
h i
Revev,kj = E v ek veT =R e k δk−j (2.116)
j

The above observation system is based on an output matrix Hk ∈ Rqxm that is the Jacobian of the nonlinear
system with respect to the state vector evaluated at the unknown nominal state, and a vector zk ∈ Rq that
e k ∈ Rq . It is
depends exclusively of the nominal state. It also employs a modified observation noise vector v
worth noting that computation of zk is not necessary to obtain the solution, as shown in section 2.5.3.2.

2.5.2.2 Constant State Vector Estimation based on Observations

The objective of this section is to obtain the best possible estimate x̂k of a constant18 state vector x based on
the observations yk provided by (2.111) and the previous estimate x̂k−1 . It is possible to employ an expression
like (2.117), where Kk ∈ Rmxq is called the gain matrix and rk ∈ Rq the innovations vector :

x̂k = x̂k−1 + Kk rk = x̂k−1 + Kk (yk − Hk x̂k−1 − zk ) (2.117)

The estimation error εx,k and its mean can then be computed based on (2.117) and (2.111):

εx,k ek
= x − x̂k = (I − Kk Hk ) εx,k−1 − Kk v (2.118)
µεx,k = E [εx,k ] = (I − Kk Hk ) µεx,k−1 (2.119)

As the linearized discrete noise ve k is zero mean per (2.115), (2.117) is called an unbiased estimator [6], because


if the initial estimate x̂0 is set equal to the expected value of the state vector x̂0 = µx → µεx,0 = 0 , then
µεx,k = E [εx,k ] = 0 ∀ k, this is, the expected value of x̂k is equal to µx = E [x] for all tk . This is regardless of the
value of the gain matrix Kk .

A similar process is followed to compute the covariance of the estimation error Cxx,k = Pk . To do so, note that
18
Note that this is the only section where the state vector xk is required to be constant, this is, x = x0 = xk ∀ k.

16
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

 

the observation noise is independent from the estimation error E ve k εT
x,k−1 = 0 :

  T
Cxx,k = Pk = E εx,k εT T e T
x,k − µεx,k µεx,k = (I − Kk Hk ) Pk−1 (I − Kk Hk ) + Kk Rk Kk (2.120)

This expression guarantees that Cxx,k = Pk is positive definite (as all covariance matrices) given that so are Pk−1
e k . The criterion to obtain the gain matrix Kk and hence fill up (2.117) and (2.120) to obtain the estimation
and R
of the state vector as well as the covariance of its error, is the minimization of the sum of the variances of the
estimation errors. That way, the estimation error is not only zero mean but it is also consistently as close as
possible to zero [6].
h i  
2 2
Jk = E (x1 − x̂1,k ) + . . . + (xm − x̂m,k ) = E ε2x1,k + . . . + ε2xm,k
  

= E εT T
x,k εx,k = E Tr εx,k εx,k = Tr Cxx,k = Tr Pk (2.121)
∂Jk  
= 2 (I − Kk Hk ) Pk−1 −HT e
k + 2 Kk Rk (2.122)
∂Kk

where Tr () stands for trace of a matrix, and some not so obvious matrix algebra properties have been employed.
Setting the (2.122) derivative to zero provides the optimum gain matrix:
 −1
Kk = Pk−1 HT T e
k Hk Pk−1 Hk + Rk (2.123)

2.5.3 Extended Kalman Filter

Provided with discrete time and linear state dynamics (2.97) and observations (2.111), the goal of state estimation
is to obtain the best possible estimate of the state vector xk = x (tk ) based on the knowledge of the system
provided by the state dynamics and the availability of observations [6]. At a given time tk = k ∆t, the a priori
estimation x̂−
k is defined as the estimation of xk , this is, the estimation of the state vector at time tk , making
use of all measurements taken before tk but not including those at tk . The a posteriori estimation x̂+k is defined
as the estimation of xk that makes use of all measurements up and including tk . In the same way, it is possible
to define the a priori and a posteriori covariances of the estimation error P− +
k and Pk :
h

T i    
− T
P−k = E xk − x̂− k xk − x̂−
k − E xk − x̂−
k E xk − x̂k (2.124)
h

T i    
+ T
P+k = E xk − x̂+ k xk − x̂+
k − E xk − x̂+
k E xk − x̂k (2.125)

The process starts with an initial estimation of the state vector x̂+
0 before any measurements are available (they
start at k = 1). Since no measurements are available, it is reasonable to form x̂+
0 as the expected value of the
initial state x0 [6]:
 
x̂+
0 = µx,0 = E [x0 ] = E x (t0 ) (2.126)

The covariance of the initial estimation error P+

0 is also required, representing the uncertainty in the initial
estimation x̂+
0
19
:
h

T i     h

T i
+ T
P+
0 =E x0 − x̂+
0 x0 − x̂+
0 − E x0 − x̂+
0 E x0 − x̂0 = E x0 − µx,0 x0 − µx,0 (2.127)

2.5.3.1 Time Update and Measurement Update Equations

−
The next step is to propagate the state estimation without the use of any observations from x̂+
0 to x̂1 , with
the objective of obtaining an estimation that coincides with the state vector mean, this is, x̂−
1 = µx,1 = E [x1 ].
Recalling the evolution of the state vector expected value provided by (2.104), and extending the same reasoning
19
If the initial state is known with exactitude, use P+
0 = 0. Otherwise, use higher values the less confidence the user has in the
accuracy of x̂+
0 .

17
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

to all steps, it makes sense intuitively to propagate the state estimate the same way that the mean of the state
propagates [6]. Hence, the time propagation for the state estimate results in:

x̂− +
e k−1
k = Fk−1 x̂k−1 + Gk−1 u (2.128)

A similar reasoning is employed for the propagation of the covariance of the estimation error in the absence of
observations. Recalling the evolution of the state vector covariance provided by (2.105) and extending the same
reasoning to all steps, the time propagation of the covariance results in [6]:

P− + T e
k = Fk−1 Pk−1 Fk−1 + Qd,k−1 (2.129)

The above equations are called the time update equations. Once the a priori estimation and error covariance have
been computed, it is possible to update them with the information contained in the observation. This is done
−
with the expressions derived in section 2.5.2.2, replacing x̂k−1 with x̂− +
k , x̂k with x̂k , Pk−1 with Pk , and Pk with
P+
k [6]. These are called the measurement update equations:
 −1
Kk = P− T − T e
k Hk Hk Pk Hk + Rk (2.130)


x̂+
k = x̂− − −
k + Kk rk = x̂k + Kk yk − Hk x̂k − zk (2.131)
T e k KT
P+
k = (I − Kk Hk ) P−
k (I − Kk Hk ) + Kk R k (2.132)

2.5.3.2 Introduction of the Nominal Trajectory

The time and measurement update equations developed in the previous section provide the means to com-


pute the variation with time of the estimated state vector x̂− +
k , x̂k as well as that of the covariance of
− +


the estimation errors Pk , Pk . However, to do so, it is necessary to define what is the nominal point
xNk = xN (tk ) , wNk = wN (tk ) = 0, vNk = vN (tk ) = 0 around which the dynamics system is linearized in sec-
tion 2.5.1.1 and the observations in section 2.5.2.1.

The extended Kalman filter (EKF) provides a solution to this problem that is simple but not too intuitive. The
EKF considers its own a priori state estimate as the nominal trajectory, this is, the nonlinear state system and
observations are linearized around the EKF estimate, and simultaneously that same estimate depends on the
linearized system [6]:

xNk = xN (tk ) = x̂−

k (2.133)

This assumption can be introduced into the expressions for the observations output matrix Hk and observations
input vector zk provided by (2.112) and (2.113), with the results introduced into the state estimation measurement
update equation (2.131):
 
− ∂h −

− −

∂h −

−
x̂+
k = x̂ k + K k y k − x̂ , 0, tk x̂ − h x̂ , 0, tk + x̂ , 0, tk x̂
∂xk k k k
∂xk k k


= x̂− −
k + Kk yk − h x̂k , 0, tk (2.134)

Note that, in contrast with (2.131), it is no longer necessary to compute the observations input vector zk . In
order to diminish the state system linearization errors described in section 2.5.1.1, it is also possible to replace
the state estimate time update equation (2.128) with a zeroth order forward integration of the continuous time
state system (2.81), with the time derivative evaluated at x̂+
k−1 :

x̂− + +
k = x̂k−1 + ∆t · f x̂k−1 , uk−1 , 0, tk−1 (2.135)

An extra benefit of this approach compared with (2.128) is that it is no longer necessary to perform the expensive

18
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

computations required to evaluate Gk−1 (2.100).

2.5.3.3 EKF Summary

Given a continuous time nonlinear state system (2.81) with process noise provided by (2.82) and (2.83), together
with a series of discrete time nonlinear observations (2.106) with measurement noise given by (2.107) and (2.108),
and considering no correlation between both noises (2.109), it is possible to compute estimations of the state at
the same time points at which the observations are provided, in such a way that their errors (difference with
respect to the true state) are zero mean and have a covariance that is also computed by means of the following
equations:

x̂+
0 = µx,0 = E [x0 ] (2.136)
h

T i
P+
0 = E x0 − µx,0 x0 − µx,0 (2.137)


x̂−
k = x̂+ +
k−1 + ∆t · f x̂k−1 , uk−1 , 0, tk−1 (2.138)
P− = Fk−1 P+ FT +Qe (2.139)
k k−1 k−1 d,k−1
 −1
Kk = − T − T
Pk Hk Hk Pk Hk + R ek (2.140)


x̂+
k = x̂− −
k + Kk yk − h x̂k , 0, tk (2.141)
P+ (I − Kk Hk ) P−
T e T
k = k (I − Kk Hk ) + Kk Rk Kk (2.142)

In the discussion that follows, x̂k is employed to refer to both the a priori and a posteriori state vector estimations


x̂− +
k , x̂k , and εk = xk − x̂k for the state estimation errors. The above equations show that x̂k is a linear
combination of a random vector x+ 0 plus a series of random processes ue k , so it is itself a random process, and so
is εk .

Leaving aside for the time being the linearization and discretization errors of sections 2.5.1.1, 2.5.1.3, and 2.5.2.1,
results in a problem composed by a discrete time linear state system (2.97) and discrete time linear observations
(2.111). Provided with any user defined positive definite weighting matrix Sk , it can be proven that the solution
provided verifies (2.143), this is, results in a state estimation that always minimizes the weighted sum of squared
estimation errors [6], as long as the process and observations noises are Gaussian zero mean uncorrelated white
noise processes. If they are not Gaussian, then x̂k provides the best linear (in the sense of the previous paragraph)
solution to the (2.143) minimization, although there may be a better nonlinear solution.
 
x̂k = arg min E εT
k Sk ε k (2.143)

The errors induced by the discretization of the linear continuous time dynamics system in section 2.5.1.3 generally
do not result in significant errors as modern systems are capable of running the estimation algorithms at elevated
frequencies. The system matrix A (t) and input vector u e (t) in the continuous time system (2.85) generally do
not vary much during the integration interval, and hence the discretization errors are small.

The linearization errors of sections 2.5.1.1 and 2.5.2.1 are a different story, and can induce the EKF to provide
unreliable estimates or even to diverge in case the nonlinearities are severe [6].

19
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Chapter 3

Introduction to Lie Algebra

This chapter begins with some basic abstract and linear algebra concepts in sections 3.1 and 3.2, and then
introduces Lie algebra in section 3.3, followed by the derivation of some useful Lie Jacobians in section 3.4. Its
application to the discrete integration of states is discussed in section 3.5, to gradient descent optimization in
section 3.6, and to state estimation in section 3.7. The contents of this chapter are generic to any Lie group
without making further mention to rigid bodies. It is only in chapters 4 and 5 where the Lie theory concepts are
applied first to rotational motion and then to the more generic rigid body motion.

3.1 Algebraic Structures, Maps, and Metric Spaces

In algebra, a set is a well defined collection of objects, named elements, while an operation ∗
is a uniquely defined rule that assigns to each ordered pair of elements exactly a third element
{∗ : A × B → C | a ∗ b = c ∈ C, ∀ a ∈ A, ∀ b ∈ B} [19]. Although an operation may involve up to three differ-
ent sets (A, B, C), often two or even the three of them coincide. A set A is a subset of a set B if all elements of
A are also elements of B. An algebraic structure is a combination of a set and one or multiple operations that
complies with certain axioms.

A set A has group structure under operation {∗ : A × A → A} if it complies with the following four axioms
∀ a, b, c ∈ A [19]:

1. Closure: a ∗ b ∈ A

2. Associativity: (a ∗ b) ∗ c = a ∗ (b ∗ c)

3. Identity: ∃ e ∈ A | e ∗ a = a ∗ e = a

4. Inverse: ∃ f ∈ A | f ∗ a = a ∗ f = e

An abelian group is that which in addition also complies with commutativity {a ∗ b = b ∗ a}. A set A has ring
structure under two operations, usually named addition {+ : A × A → A} and multiplication {· : A × A → A},
if, in addition to being an abelian group under addition, complies with the following four axioms ∀ a, b, c ∈ A
[19]:

1. Closure of · : a · b ∈ A

2. Associativity of · : (a · b) · c = a · (b · c)

3. Distributivity of · with respect to + : a · (b + c) = a · b + a · c, (a + b) · c = a · c + b · c

4. Identity of · : ∃ 1 ∈ A | 1 · a = a · 1 = a

An abelian ring is that which in addition also complies with commutativity over multiplication {a · b = b · a}.
Note that by convention, the identity and inverse of addition are denoted 0 and −a, respectively, while those of
20
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

multiplication are denoted 1 and a−1 . A set A has field structure under operations + and · if A is an abelian
group under + and A − {0} (the set A without the additive identity 0) is an abelian group under · [19]. In an
ordered field, the implementation of the addition and multiplication operations enables determining if one element
is greater, equal, or lower than a second element. The set of real numbers R endowed with the operations of
addition + and multiplication · forms an ordered field, known as the field of real numbers hR, +, ·i, nearly always
abbreviated to simply R.

A topological space is an ordered pair (A, τ ), where A is a set and τ is a collection of subsets of A, satisfying the
following axioms [20]:

1. The empty set and A itself belong to τ .

2. Any arbitrary (finite or infinite) union of members of τ still belongs to τ .

3. The intersection of any finite number of members of τ still belongs to τ .

The elements of τ are called open sets and the collection τ is called a topology on A. Topological spaces comprise
the most general notion of a mathematical space; all other spaces defined below are specializations with extra
structure or constraints.

A vector space (linear space) over a field F is a set V together with two operations, addition {+ : V × V → V} and
scalar multiplication {· : F × V → V} that, in addition of V being an abelian group under +, satisfies the following
axioms ∀ u, v ∈ V and ∀ a, b ∈ F [21, 22]. Elements of F are called scalars, while those of V vectors.

1. Closure of · : a · u ∈ V

2. Compatibility of · with field · : a · (b · v) = (a · b) · v

3. Identity of · : 1 · v = v, where 1 denotes the field · identity.

4. Distributivity of · with respect to + : a · (u + v) = a · u + a · v

5. Distributivity of · with respect to field + : (a + b) · v = a · v + b · v

A map or morphism is a rule that to every element in a set A assigns a unique element in a different set B
{f : A → B | f (a) = b ∈ B, ∀ a ∈ A} [19]. A map is injective if each element in B is the image or map output of
no more than one element of A, surjective if each element in B is the image of at least one element of A, and
bijective is the map is simultaneously injective and surjective.

A homomorphism is a structure preserving map between two algebraic structures of the same type (groups, rings,
fields, vector spaces, etc.) [19]. Note that neither the sets nor the operations of the structures need to coincide,
and that a homomorphism preserves every operation contained in the algebraic structures. In the case of groups,
considering a homomorphism {f : hA, +i → hB, ·i}, it complies with all group axioms ∀ a, b, c ∈ A:

1. Closure: f (a + b) = f (a) · f (b)

2. Associativity: f (a + b) + c = f a + (b + c) = f (a) · f (b) · f (c) = f (a) · f (b) · f (c)

3. Identity: f (0) = 1 → f (a) = f (a + 0) = f (0 + a) = f (a) · f (0) = f (0) · f (a) = f (a) · 1 = 1 · f (a)

4. Inverse: f (−a) = f −1 (a) → f (0) = f (a − a) = f (a) · f (−a) = f (a) · f −1 (a) = f −1 (a) · f (a) = 1

Homomorphisms for other algebraic structures are defined similarly. A bijective homomorphism is known as an
isomorphism [19]. A metric is an operation between two elements of the same set onto a field {d : V × V → F}.
It defines the concept of distance between any two members of the set and complies with the following axioms
∀ u, v, w ∈ V:

1. Identity of indiscernibles: d (u, v) = 0 ⇔ u = v

2. Symmetry: d (u, v) = d (v, u)

3. Subadditivity: d (u, w) ≤ d (u, v) + d (v, w)

21
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Based on these three axioms, it is straightforward to prove that d (u, v) ≥ 0 ∀ u, v ∈ V. The most common metric
is the inner product {h· , ·i : V × V → F}, usually associated to a vector space, which satisfies the following three
axioms ∀ u, v, w ∈ V and ∀ a, b ∈ F [21]:

1. Commutativity: hu, vi = hv, ui

2. Linearity with respect to + and · : hu, a · v + b · wi = a · hu, vi + b · hu, wi

3. Positive definiteness: hv, vi ≥ 0 and hv, vi = 0 ⇔ v = 0

A metric space is a combination of a set with a metric on that same set [23], while an inner product space restricts
the definition to the case of a vector space V over a field F endowed with an inner product metric over the same
field F [24]. An Euclidean space is a finite-dimensional inner product space over the field of the real numbers R
[25].

A group action on a space is a group homomorphism of a given group hA, ∗i into the group of transfor-
mations of the space {g () : A × V → V | ga (u) = v ∈ V, ∀ a ∈ A, ∀ u ∈ V}, and needs to verify two axioms
∀ a, b ∈ A, ∀ u ∈ V:

1. Identity: ge (u) = u, where e is the identity of G.

2. Compatibility: ga∗b (u) = ga gb (u)

Returning to the case of inner product spaces, two vectors are orthogonal if their inner product is zero, while the
p
length or norm of a vector is kvk = hv, vi. A unit vector is that whose norm is one. An inner product space
can also be endowed with an additional operation, the cross product {× : V × V → V}, which complies with the
following axioms [21]:

1. Anti commutativity: u × v = −v × u

2. Compatibility with · : (a · u) × v = u × (a · v) = a · (u × v)

3. Distributivity with + : (u + v) × w = (u × w) + (v × w)

It can also be quickly derived that hu × v, ui = hu × v, vi = 0.

3.2 Points, Vectors, and Axes

Note that in the abstract discussion above it has not yet been defined what a vector is. This section focuses
on the three-dimensional Euclidean space E3 [26], which can be represented by a Cartesian frame, where every
T
point p ∈ E3 can be identified by its three coordinates p = [p1 p2 p3 ] ∈ R3 . A vector in E3 is defined by a
pair of points p, q ∈ E3 with a directed arrow connecting p to q, where the vector v = [v1 v2 v3 ]T ∈ R3 is a
triplet of numbers, each one being the difference between the corresponding coordinates of the two points q and
p (v = q − p ∈ R3 ). Although they share notation, points and vectors are different geometric objects. A free
vector is one that does not depend on its starting or base point.

The set of all free vectors in R3 form an inner product space with cross product over the field of real numbers R,
with both products defined as follows by making use of matrix notation:

hu, vi = u · v = uT v = u1 v1 + u2 v2 + u3 v3 (3.1)
    
0 −u3 +u2 v1 u2 v3 − u3 v2
    
u×v = b v = +u3
u 0 −u1  v2  = u3 v1 − u1 v3  = −v × u = −b
vu (3.2)
−u2 +u1 0 v3 u1 v2 − u2 v1

where vT is the transpose of v and v b its skew-symmetric form1 . The inner product or Euclidean metric can
measure distances and angles, while the cross product defines orientation.
1
An skew-symmetric matrix is one whose negative equals its transpose.

22
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Any vector v in R3 can be written as v = v1 e1 + v2 e2 + v3 e3 , where e1 , e2 , and e3 are the three linearly
independent basis vectors and v1 , v2 , v3 the coordinates or components of v with respect to that basis [27]. The
basis is called orthogonal if ei T ej = 0 when i 6= j, orthonormal if additionally ei T ej = 1 when i = j, and right
handed if additionally ǫijk = ei T bej ek is 1 for ǫ123 , ǫ231 , and ǫ312 , -1 for ǫ132 , ǫ213 , and ǫ321 , and 0 in all other
cases [21].

An axis or line is defined by its direction n (provided by a free vector) and a point p that it passes through.
Its coordinates are (n, m) ∈ R6 , where m = p b n is called the moment of the line. The coordinates (n, m) are
independent of p. The moment m is normal to the plane through the line and the origin with norm equal to
the distance from the line to the origin. The point belonging to the line that is closest to the origin responds to
p⊥ = nb m. A line has four degrees of freedom and hence two redundancies, provided by n being a direction and
hence a unit vector (knk = 1) and n being orthogonal to m by definition (nT m = 0) [28].

It is important to remark that although this is the formal definition of an axis, in this document unless otherwise
specified an axis is synonymous with just a direction n with two degrees of freedom (knk = 1), passing through
the origin (p = 0 → m = 0). The reason is that in most occasions it is convenient to consider that a rigid body
rotates about the origin of the reference frame representing it.

3.3 Lie Groups and Lie Algebras

A manifold is a topological space that locally resembles Euclidean space near each element, so each element of
an m dimensional manifold has a neighborhood that is homeomorphic2 to the m dimensional Euclidean space
[29]. Manifolds, which are embedded in spaces of higher dimension, are curved, smooth (hyper) surfaces with no
edges or spikes; they are defined by the constraints imposed on the state [1], this is, the state vector is restricted
to moving within the manifold.

A Lie group hG, ◦i is a smooth manifold whose elements satisfy the group axioms. They combine the local
properties of smooth manifolds, enabling the use of calculus, with the global properties of groups, allowing the
nonlinear composition of distant objects [1]. Elements of G are denoted with X , the identity with E, and the
inverse with X −1 . As in any other group, Lie groups are capable of transforming elements of other sets by
means of their actions. In this sense, the group operation {◦ : G × G → G}, generally called composition, can be
considered as an action of the group on itself.

If X (t) is an element or point of the Lie group moving on the manifold, its derivative with time belongs to the
space tangent to G at X , denoted by TX G. There exists a unique tangent space at each point, but the structure
of such tangent spaces is the same everywhere [1]. The tangent space at a point is a real vector space of the same
dimension as the manifold that intuitively contains all the possible directions in which one can tangentially pass
through the point.

The Lie algebra m is defined as the tangent space at the identity {m = TE G}, and it is a vector space whose
elements can be identified with vectors in Rm , with m being the number of degrees of freedom of the Lie group
G [1]. Elements of the tangent space are usually denoted v when referring to velocities and τ = v · t for more
general elements. Lie algebras can be defined at any manifold point X , establishing local coordinates for TX G,
and its elements are denoted by the < ·∧ > symbol, such as vX ∧ ∈ TX G or τ E∧ ∈ TE G.

As all tangent spaces or Lie algebras have the same structure no matter the position of the element X within
the Lie group G, it can be considered with no loss of generality3 that all group actions at X ∈ G, noted as gX (),
transform elements viewed in the local or body frame represented by X into the global or space frame represented
−1
by E [1]. The opposite is true for the inverse operations noted as gX () = gX −1 (). When the group action is the
2
A homeomorphism or topological isomorphism is a continuous function between topological spaces that has a continuous inverse
function.
3
This is just a convention, and the opposite one is employed in some texts.

23
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

composition ◦ itself, elements to the right of X belong to the body frame, while those on the left are viewed on
the global frame.

3.3.1 Lie Algebra Velocities, Hat and Vee Operators

The structure of the Lie algebra can be obtained by time derivating the group inverse constraint [1]. Every Lie
group employed in this document (refer to chapters 4 and 5) has a ◦ composition operator realized by some type
of multiplication. If this is the case, the group inverse constraint responds to X ◦ X −1 = X −1 ◦ X = E, and its
derivation with time leads to the Lie algebra velocities viewed in either the body or local frames4 :

X˙ ◦ X −1 + X ◦ X −̇1 = 0 → vE∧ = X˙ ◦ X −1 = −X ◦ X −̇1 (3.3)

X −1
◦ X˙ + X −̇1
◦X =0 → v X∧
=X −1
◦ Ẋ = −X −̇1
◦X (3.4)

The τ ∧ or v∧ elements of the Lie algebra hence do not have trivial structures but can always be expressed as
linear combinations of some base elements ei , which are called the generators of m [1]. It is generally more
convenient to manipulate them as vectors τ ∈ Rm , as they can then be grouped together in larger state vectors
and operated by means of linear algebra.
n The isomorphisms othat linearly convert between them are called hat
∨
{· : R → m | τ → τ } and vee · : m → Rm | (τ ∧ ) → τ .
∧ m ∧ ∨

3.3.2 Exponential and Logarithmic Maps, Plus and Minus Operators

The exponential map {exp () : m → G | X = exp (τ ∧ )} wraps the tangent element around the manifold following
the geodesic or minimum distance line, effectively converting elements of the Lie algebra into those of the mani-
fold or Lie group. The unwrapping or inverse operation is the logarithmic map {log () : G → m | τ ∧ = log (X )}.
The hat and vee operators can be incorporated into these maps, resulting in the capitalized maps
{Exp () : Rm → G | X = Exp (τ )} and {Log () : G → Rm | τ = Log (X )}. Note that the exponential map com-
plies with the following properties ∀ t ∈ R [1]:
t
exp (t τ ∧ ) = exp (τ ∧ ) (3.5)


exp X ◦ τ ∧ ◦ X −1 = X ◦ exp (τ ∧ ) ◦ X −1 (3.6)

The plus and minus operators enable operating with increments of the nonlinear manifold expressed in the
corresponding linear tangent vector space [1]. As the Lie group G is not abelian, there exist right ⊕ and ⊖
operators as well as left ⊞ and ⊟ ones. Note that the addition of (usually) small perturbations ∆τ to a given
manifold X result in a perturbed manifold Y:


Y = X ⊕ ∆τ X = X ◦ Exp ∆τ X ∈ G (3.7)


∆τ X = Y ⊖ X = Log X −1 ◦ Y ∈ TX G (3.8)
E E


Y = ∆τ ⊞ X = Exp ∆τ ◦ X ∈ G (3.9)


∆τ E = Y ⊟ X = Log Y ◦ X −1 ∈ TE G (3.10)

3.3.3 Adjoint Action

The vectors or elements of the tangent space at X can be transformed to the tangent space at the identity E by
means of the adjoint {Ad () : G × m → m} [1]. The adjoint is hence an action of the Lie group that operates on
its own Lie algebra. The adjoint action can be obtained by the equivalence of the perturbed state Y in (3.7, 3.9)
by means of (3.6):


τ E∧ = AdX τ X ∧ = X ◦ τ X ∧ ◦ X −1 (3.11)
4
Note that X −̇1 represents the time derivative of the inverse, not the inverse of the time derivative.

24
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

The adjoint action is a linear homomorphism, and hence complies with the following expressions ∀ X , Y ∈ G,
∀ a, b ∈ R, ∀ τ X ∧ , σX ∧ ∈ TX G, and ∀ τ Y∧ ∈ TY G:




AdX a τ X ∧ + b σ X ∧ = a AdX τ X ∧ + b AdX σ X ∧ (3.12)
Y∧


Y∧


AdX AdY τ = AdX ◦Y τ (3.13)

As the nadjoint is a linear transform, it is always possible to obtain anoequivalent matrix operator, the adjoint

∨
matrix Ad : G × Rm → Rm | AdX · τ = X τ ∧ X −1 , AdX ∈ Rmxm , that maps the Cartesian tangent space
vectors instead of the Lie algebra elements [30]. Both maps share the same symbols but are easily distinguished
by context.

∨
τ E = AdX · τ X = X τ X ∧ X −1 (3.14)

The adjoint matrix complies with the following properties:

X ⊕ τ X = AdX · τ X ⊞ X (3.15)
AdX −1 = Ad−1
X (3.16)
AdX ◦Y = AdX AdY (3.17)

3.3.4 Right and Left Lie Group Derivatives

Given a function {f : G → H | Y = f (X ) ∈ H, ∀X ∈ G} that maps together two manifolds or Lie groups of di-
mensions m and n respectively, it is possible to make use of the plus and minus operators to establish right
and left derivatives (Jacobians) that linearly map their respective Lie algebras or tangent spaces, either locally


TX G → Tf(X ) H if employing the right ⊕ and ⊖ operators, or globally (TE G → TE H) when using the left ⊞
and ⊟ operators [1]. As a tangent space can always be identified to a Euclidean space of the same dimension,
this enables the application of the concepts of random vectors, stochastic processes, and their correlation (section
2.1) to Lie algebras, leading to the section 3.3.5 expressions for Lie covariances. In addition, these derivatives
constitute the basis for the construction of the Lie group Jacobians in section 3.4, which in turn are indispens-
able for the establishment of rigorous solutions for the gradient descent minimization (optimization) and state
estimation in Lie groups, as described in sections 3.6 and 3.7, respectively.

The right Jacobian of f (X ) is defined as the derivative of f (X ) with respect to X when the increments are
viewed in their respective local tangent spaces, this is, tangent respectively at X ∈ G and f (X ) ∈ H [1]. The left
Jacobian of f (X ) is defined similarly, but with the increments viewed in the global tangent spaces for G and H
respectively:
h 
i

−1 X
⊖ f(X ) f X ⊕ ∆τ X
⊖ f(X ) Log f (X ) ◦ f X ◦ Exp ∆τ
J⊕ X = lim = lim ∈ Rnxm (3.18)
∆τ X →0 ∆τ X ∆τ X →0 ∆τ X
h
 −1 i

E
⊟ f(X ) f ∆τ E
⊞ X ⊟ f (X ) Log f Exp ∆τ ◦ X ◦ f (X )
J = lim = lim ∈ Rnxm (3.19)
⊞X ∆τ E →0 ∆τ E ∆τ E →0 ∆τ E

The following first order Taylor expansions can then be directly established:
 

⊖ f (X )
f X ⊕ ∆τ X ≈ f (X ) ⊕ J⊕ X ∆τ X = Y ⊕ ∆τ Y ∈ H (3.20)
 
EG

⊟ f (X ) EG
f ∆τ ⊞ X ≈ J⊞ X ∆τ ⊞ f (X ) = ∆τ EH ⊞ Y ∈ H (3.21)

Note that the ⊕ or ⊞ symbols that appear as Jacobian subindexes in (3.18) and (3.19) indicate that the domain
G is indeed a Lie group, and should be replaced by a standard + operator if this is not the case and the function



f domain is in fact a real or Euclidean space. If this is the case, the expressions X ⊕ ∆τ X and ∆τ EG ⊞ X
within the equations (3.18) through (3.21) shall both be replaced by (X + ∆τ ).
25
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Similarly, the ⊖ and ⊟ symbols that appear as Jacobian superindexes indicate that the function f image or
codomain H is also a Lie group, and should otherwise be replaced by − if the codomain is a Euclidean space. If
this is the case, the ⊖ and ⊟ operators within (3.18) and (3.19) shall be replaced by the standard − operator,
and the f (X ) ⊕ and ⊞ f (X ) expressions within (3.20) and (3.21) shall both be replaced by f (X ) +.

Equations (3.20) and (3.21) lead to the following expressions for the function f propagation of the tangent
spaces:
⊖ f (X )
∆τ Y = ∆τ f(X ) = J⊕ X ∆τ X (3.22)
⊟ f (X )
∆τ EH = J⊞ X ∆τ EG (3.23)

In addition, (3.15) enables establishing a relationship between the right and left Jacobians of f (X ):

⊟ f (X ) ⊖ f (X )
J⊞ X = Adf(X ) J⊕ X Ad−1
X (3.24)

3.3.5 Lie Groups Uncertainty and Covariance

As the ⊕ and ⊖ operators can be employed to define perturbations in the local tangent space TµX G around
a nominal or expected point E [X ] = µX ∈ G, it is possible to employ a local autocovariance definition similar to
the one
 used for Euclidean spaces (2.24), leading to the definition of stochastic variables (vectors) on manifolds
X ∼ µX , CX XX [1]:

X = µX ⊕ ∆τ X ∈G (3.25)
∆τ X = X ⊖ µX ∈ TµX G (3.26)
  h i
T
CX
XX = E ∆τ X ∆τ X ,T = E (X ⊖ µX ) (X ⊖ µX ) ∈ Rmxm (3.27)

The different types of correlation matrices for stochastic processes introduced in section 2.1.3 can also be defined
accordingly. Note that although the notation of CX X X refers to the covariance of the manifold or Lie group X ∈ G,
the definition in fact refers to the covariance of the nominal point local tangent space ∆τ X ∈ TµX G, with its
dimension matching the number of degrees of freedom of the manifold.

A similar process employing the ⊞ and ⊟ operators leads to the global autocovariance:
  h i
T
CEX X = E ∆τ E ∆τ E,T = E (X ⊟ µX ) (X ⊟ µX ) ∈ Rmxm (3.28)
h i
CEX X = E AdX ∆τ X ∆τ X ,T AdT X
X = AdX CX X AdX
T
(3.29)
h i
CXXX = E Ad−1 X ∆τ ∆τ
E E,T
Ad−T
X = Ad−1 E
X CX X AdX
−T
(3.30)

Given a function {f : G → H | Y = f (X ) ∈ H, ∀X ∈ G} as that introduced in section 3.3.4, which maps together

two manifolds or Lie groups of dimensions m and n respectively, it is possible to propagate the covariances CXXX
and CEX X from the domain manifold G to the image one H:
  h i
CYYY = E ∆τ Y ∆τ Y,T = E ∆τ f(X ) ∆τ f(X ),T
 
⊖ f (X ) X X ,T ⊖ f (X ),T ⊖ f (X ) X ⊖ f (X ),T
= E J ∆τ ∆τ J =J CX X J ∈ Rnxn (3.31)
⊕X ⊕X ⊕X ⊕X
  ⊟ f (X ) E ⊟ f (X ),T
CEYY = E ∆τ EH ∆τ EH ,T = J CX X J ∈ Rnxn (3.32)
⊞X ⊞X

The establishment and propagation of covariance matrices of the proper dimensions is key for the application of
state estimation techniques such as Kalman filtering when some of the state vector components belong to Lie
manifolds and their tangent spaces, as described in section 3.7.

26
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

3.4 Euclidean and Lie Jacobians

The definition of the proper derivative matrices or Jacobians is indispensable for all calculus techniques that
rely on linearization, such as optimization by means of the gradient descent method (section 3.6) or state es-
timation through Kalman filtering (section 3.7). Given a function {f (x) : Rm → Rn }, its (Euclidean) Jacobian
− f(x)
J+ x ∈ Rnxm stacks the partial derivatives of each component of the output space with respect to those of the
input space:

− f (x) fi (xj + ∆xj ) − fi (xj )

J+ x,ij = lim (3.33)
∆xj →0 ∆xj

This section relies on the right and left Lie group derivatives introduced in section 3.3.4 to properly define
Jacobians when either the input or output spaces (or both) are not Euclidean but Lie groups. The various
Jacobians listed in table 3.1 have been obtained by means of the expressions that appear on section 3.3 together
with the chain rule, and include instances in which both the domain and codomain of the f (X ) function are either
Euclidean or Lie groups. Some are generic, while others, which rely on group actions, depend on the specific set
on which the action is applied and hence can only be established for a specific Lie group, such as rotational or
rigid body motions (chapters 4 and 5).

Jacobian Result Taylor Expansion

 
−1 −1
J⊖
⊕X
X −AdX (X ⊕ ∆τ )−1 −1
≈ X ⊕ J⊕ X ∆τ ⊖ X ∈G
 
−1
⊟X
J⊞ −Ad−1 (∆τ ⊞ X )
−1
≈ ⊟ X −1 ∆τ ⊞ X −1
J⊞ ∈G
X X X
h i
J⊖
⊕X
X ◦Y Ad−1
Y (X ⊕ ∆τ ) ◦ Y ≈ (X ◦ Y) ⊕ J⊖ ⊕X
X ◦ Y ∆τ ∈G
h i
J⊟ X ◦ Y Imxm (∆τ ⊞ X ) ◦ Y ≈ J ⊟ X ◦ Y ∆τ ⊞ (X ◦ Y) ∈G
⊞X ⊞X
h i
J⊖ X ◦ Y Imxm X ◦ (Y ⊕ ∆τ ) ≈ (X ◦ Y) ⊕ J⊖ X ◦ Y ∆τ ∈G
⊕Y ⊕Y
h i
J⊟ X ◦Y AdX X ◦ (∆τ ⊞ Y) ≈ ⊟
J⊞ Y X ◦ Y ∆τ ⊞ (X ◦ Y) ∈G
⊞Y
⊖ Exp (τ )
J+ τ JR (τ )5 Exp (τ + ∆τ ) ≈ Exp (τ ) ⊕ [JR (τ ) ∆τ ] ∈G
 
J−1
R (τ ) τ + J−1
R (τ ) ∆τ ≈6 Log Exp (τ ) ⊕ ∆τ ∈ Rm
⊟ Exp (τ )
J+ τ JL (τ )7 Exp (τ + ∆τ ) ≈ [JL (τ ) ∆τ ] ⊞ Exp (τ ) ∈G
 
J−1
L (τ ) τ + J−1
L (τ ) ∆τ ≈8 Log ∆τ ⊞ Exp (τ ) ∈ Rm
 
− Log (X )
− Log (X )
J⊕ X J−1
R Log (X ) Log (X ⊕ ∆τ ) ≈ Log (X ) + J⊕ X ∆τ ∈ Rm
 
− Log (X )
− Log (X )
J⊞ X J−1
L Log (X ) Log (∆τ ⊞ X ) ≈ Log (X ) + J⊞ X ∆τ ∈ Rm
h i
J⊖
⊕X
X ⊕τ Ad−1
Exp(τ ) (X ⊕ ∆τ ) ⊕ τ ≈ (X ⊕ τ ) ⊕ J⊖ ⊕X
X ⊕ τ ∆τ ∈G
h i
⊟τ ⊞X
J⊞ AdExp(τ ) τ ⊞ (∆τ ⊞ X ) ≈ ⊟
J⊞ Xτ ⊞ X ∆τ ⊞ (τ ⊞ X ) ∈G
X
h i
J⊖
+τ
X ⊕τ JR (τ ) X ⊕ (τ + ∆τ ) ≈ (X ⊕ τ ) ⊕ J⊖ +τ
X ⊕ τ ∆τ ∈G
h i
⊟τ ⊞X
J+ JL (τ ) (τ + ∆τ ) ⊞ X ≈ J⊟ τ ⊞ X ∆τ ⊞ (τ ⊞ X ) ∈G
τ +τ
h i
J−
⊕X
Y ⊖X −J−1
L (Y ⊖ X ) Y ⊖ (X ⊕ ∆τ ) ≈ (Y ⊖ X ) + J− ⊕X
Y ⊖ X ∆τ ∈ Rm
5
Obtained in sections 4.12 and 5.13 for rotational and rigid body motion, respectively.
6
Obtained by replacing ∆τ by J−1R ∆τ in the expression above.
7
Obtained in sections 4.12 and 5.13 for rotational and rigid body motion, respectively.
8
Obtained by replacing ∆τ by J−1L ∆τ in the expression above.

27
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Jacobian Result Taylor Expansion

h i
J−
⊞X
Y ⊟X −J−1
R (Y ⊟ X ) Y ⊟ (∆τ ⊞ X ) ≈ (Y ⊟ X ) + J− ⊞X
Y ⊟ X ∆τ ∈ Rm
h i
J−
⊕Y
Y ⊖X J−1
R (Y ⊖ X ) (Y ⊕ ∆τ ) ⊖ X ≈ (Y ⊖ X ) + J− ⊕Y
Y ⊖ X ∆τ ∈ Rm
h i
J− Y ⊟ X J−1
L (Y ⊟ X ) (∆τ ⊞ Y) ⊟ X ≈ (Y ⊟ X ) + J− Y ⊟ X ∆τ ∈ Rm
⊞Y  ⊞Y 
− gX (u) − gX (u)
J gX ⊕∆τ (u) ≈ gX (u) + J ∆τ ∈ Ru
⊕X ⊕X
 
− g (u) − gX (u)
J⊞ XX Tables g∆τ ⊞X (u) ≈ gX (u) + J⊞ X ∆τ ∈ Ru
 
− g (u) − gX (u)
J+ uX 4.7 gX (u + ∆u) ≈ gX (u) + J+ u ∆u ∈ Ru
−1  −1 
− gX (u) −1 −1 − gX (u)
J & gX ⊕∆τ (u) ≈ gX (u) + J ∆τ ∈ Ru
⊕X ⊕X
 
− g−1 (u) −1 −1 − g−1 (u)
J⊞ XX 5.8 g∆ τ ⊞X (u) ≈ gX (u) + J⊞ XX ∆τ ∈ Ru
 
− g−1 (u) −1 −1 − gX−1
(u)
J+ uX gX (u + ∆u) ≈ gX (u) + J+ u ∆u ∈ Ru
 
− Ad (v) − Ad (v)
J⊕ X X AdX ⊕∆τ (v) ≈ AdX (v) + J⊕ X X ∆τ ∈ Rm
 
− Ad (v) − Ad (v)
J⊞ X X Ad∆τ ⊞X (v) ≈ AdX (v) + J⊞ X X ∆τ ∈ Rm
 
− Ad (v) − AdX (v)
J+ v X AdX (v + ∆v) ≈ AdX (v) + J+ v ∆v ∈ Rm
 
− Ad−1 (v) −1
− AdX (v)
J⊕ X X Tables Ad−1
X ⊕∆τ (v)
−1
≈ AdX (v) + J⊕ X ∆τ ∈ Rm
 
− Ad−1 (v) − Ad−1X (v) ∆τ
J⊞ X X 4.7 Ad−1
∆τ ⊞X (v) ≈ Ad−1 X (v) + J ⊞X ∈ Rm
 
− Ad−1 (v) − Ad−1X (v) ∆v
J+ v X & Ad−1
X (v + ∆v) ≈ Ad−1 X (v) + J +v ∈ Rm
 
− gExp(τ ) (u) − gExp(τ ) (u)
J+ τ 5.8 gExp(τ +∆τ ) (u) ≈ gExp(τ ) (u) + J+ τ ∆τ ∈ Ru
−1
" −1
#
− gExp(τ ) (u) −1 −1
− gExp(τ ) (u)
J+ τ gExp(τ +∆τ ) (u) ≈ gExp(τ ) (u) + J+ τ ∆τ ∈ Ru

Table 3.1: Lie Jacobians

There are two constructions that appear repeatedly within table 3.1. The first is the adjoint matrix (3.14), which
maps the local and global tangent spaces at a given point on a manifold or Lie group, while the second are the
right and left Jacobians of the capitalized exponential function, also known as simply the right Jacobian JR (τ )
and the left Jacobian JL (τ ). These compare variations in the tangent space of the output Exp (τ ) map (locally
for JR and globally for JL ) with (Euclidean) variations in the input argument τ , and are obtained in sections
4.12 and 5.13 for the specific cases of rotational and rigid body motions, respectively.

AdExp(τ ) = JL (τ ) J−1
R (τ ) (3.34)
JR (−τ ) = JL (τ ) (3.35)

Being located in the tangent spaces, the right and left Jacobians can be related by means of the adjoint, resulting
in (3.34). Expression (3.35) in turn can be obtained by means of the chain rule.

3.5 Discrete Integration in Lie Groups

Following on the discrete integration in Euclidean spaces described in section 2.3, consider now that the state
system is composed by a vector y ∈ Rn , an element of a Lie group X ∈ G, and a vector vX ∈ Rm representing
the velocity of X as it moves along its manifold, contained in the local or body tangent space TX G. Neither X
28
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

nor its components are Euclidean, and hence the section 2.3 integration schemes are not applicable. If treated
so, the resulting element Xk+1 would not be located on the manifold as it would not comply with the Lie group
constraints, and it would be necessary to reproject it back to it, incurring in errors that although small for a
single integration step may become significant when accumulated.

Group these states into a composite state vector made up by n plus m components of an Euclidean space plus
an element of a Lie group. As in the Euclidean case, the initial composite vector value is known:
 T
x = y vX X (3.36)
  T
xk = x (tk ) = yk vX k Xk (3.37)

As in the Euclidean case, the objective is the determination of the composite state vector value at a time
xk+1 = x (tk+1 ) = x (tk + ∆t) by relying on evaluations of the y and vX time derivatives:


ẏ (t) = f y y (t) , vX (t) , X (t) , t (3.38)


v̇X (t) = f v y (t) , vX (t) , X (t) , t (3.39)

The solution consists on employing the Euclidean integration method of choice to obtain yk+1 and vX
k+1 , but rely
on the right plus operator and the capitalized exponential map of the Lie group to determine Xk+1 . In case of
Euler’s method, the solution is the following:

yk+1 ≈ yk + ∆t ẏ(yk , vX
k , Xk , tk ) (3.40)
X
vX
k+1 ≈ vX
k + ∆t v̇ (yk , vX
Xk , tk )
k , (3.41)
 

Xk+1 ≈ Xk ⊕ ∆t vX k = Xk ◦ Exp ∆t vk
X
(3.42)
 T
xk+1 = x (tk+1 ) ≈ yk+1 vXk+1 Xk+1 (3.43)

In case the state vector velocity vE is that of the global or space tangent space TE G, it is necessary to modify
(3.42) to employ the left plus operator:
 

Xk+1 ≈ ∆t vEk ⊞ Xk = Exp ∆t vEk ◦ Xk (3.44)

It is necessary to proceed in a similar manner if a different integration method is selected. In the case of Heun’s
method with local velocity vX , the modified integration scheme is the following:

ẏ1 = ẏ(yk , vX
k , Xk , tk ) (3.45)
X
ẏ2 = ẏ(yk + ∆t ẏ1 , vX X
k + ∆t v̇1 , Xk ⊕ ∆t vk , tk + ∆t) (3.46)
v̇X
1 = v̇ X
(yk , vX
k , Xk , tk ) (3.47)
v̇X
2 = v̇X (yk + ∆t ẏ1 , vX X X
k + ∆t v̇1 , Xk ⊕ ∆t vk , tk + ∆t) (3.48)
∆t
yk+1 ≈ yk + [ẏ1 + ẏ2 ] (3.49)
2
∆t h X X
i
vX
k+1 ≈ vXk + v̇ 1 + v̇ 2 (3.50)
2 h  i  
∆t X ∆t2 X
Xk+1 ≈ Xk ⊕ vX k + v X
k + ∆t v̇ 1 = Xk ⊕ ∆t v X
k + v̇ 1
2 2
 
∆t2 X
= Xk ◦ Exp ∆t vX k + v̇1 (3.51)
2

In case of the 4th order Runge-Kutta integration scheme, it results in the following:


ẏ1 = ẏ yk , vXk , Xk , tk (3.52)


v̇X
1 = v̇X yk , vX k , Xk , tk (3.53)

29
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

 
∆t ∆t X ∆t X ∆t
ẏ2 = ẏ yk + ẏ1 , vXk + v̇ , X k ⊕ v , tk + (3.54)
2 2 1 2 k 2
 
∆t ∆t X ∆t X ∆t
v̇X
2 = v̇X yk + ẏ1 , vX k + v̇ , X k ⊕ v , tk + (3.55)
2 2 1 2 k 2
   
∆t ∆t X ∆t ∆t X ∆t
ẏ3 = ẏ yk + ẏ2 , vXk + v̇ , X k ⊕ v X
+ v̇ , tk + (3.56)
2 2 2 2 k
2 1 2
   
∆t ∆t X ∆t ∆t X ∆t
v̇X
3 = v̇X yk + ẏ2 , vX k + v̇ , X k ⊕ v X
+ v̇ , tk + (3.57)
2 2 2 2 k
2 1 2
   
X ∆t X
ẏ4 = ẏ yk + ∆t ẏ3 , vX k + ∆t v̇3 , Xk ⊕ ∆t vk +
X
v̇ , tk + ∆t (3.58)
2 2
   
∆t X
v̇X
4 = v̇X yk + ∆t ẏ3 , vX k + ∆t v̇ X
3 , X k ⊕ ∆t v X
k + v̇ , tk + ∆t (3.59)
2 2
 
ẏ1 ẏ ẏ ẏ
yk+1 ≈ yk + ∆t + 2+ 3+ 4 (3.60)
6 3 3 6
" #
X X X
v̇1 v̇2 v̇3 v̇X
vX
k+1 ≈ X
vk + ∆t + + + 4
(3.61)
6 3 3 6
     
∆t X ∆t X ∆t X ∆t X ∆t X ∆t  X X
Xk+1 ≈ Xk ⊕ v + vk + v̇ + vk + v̇ + vk + ∆t v̇3
6 k 3 2 1 3 2 2 6
 
∆t2 X ∆t2 X ∆t2 X
= Xk ⊕ ∆t vX k + v̇1 + v̇2 + v̇3
6 6 6
 
X ∆t2 X ∆t2 X ∆t2 X
= Xk ◦ Exp ∆t vk + v̇1 + v̇2 + v̇3 (3.62)
6 6 6

Similar modifications to that of (3.44) are required if the tangent space velocity vE is viewed in the global
space.

3.6 Gradient Descent Optimization in Lie Groups

The Gauss-Newton implementation of the gradient descent optimization method is described in section 2.4 for
the case of Euclidean spaces. Consider now a Lie group element X ∈ G and a nonlinear map based on its tangent
space {f : Rm → Rn | f (τ ) ∈ Rn , τ = Log (X ) ∈ Rm , τ ∧ = log (X ) ∈ m, ∀ X ∈ G}. As in the Euclidean case, it is
possible to evaluate its Jacobian {J : Rm → Rnxm | J (τ ) = ∂f (τ )/∂τ ∈ Rnxm , ∀ τ ∈ Rm }, and the error function
is defined as E (τ ) = E Log(X )) = f (τ ) − f T .

Given an initial state X0 = Exp (τ 0 ), the objective is to determine a Lie group element
E E
X = Exp (τ ) = ∆τ ⊞ X0 = ∆τ ◦ Exp (τ 0 ) in the vicinity of X0 for which the cost function norm kE (τ ) k ∈ R
holds a local minimum. As the τ E∧ perturbation belongs to the spatial tangent space TE G, all gradient descent
methods advance the solution by means of (3.63):

Xk+1 ←− ∆τ Ek ⊞ Xk = ∆τ Ek ◦ Exp (τ k ) (3.63)

A robust formulation is necessary to ensure that the transform vector τ , which is treated as Euclidean by the
functions E and f , is advanced as a member of the tangent space that adheres to the Lie group constraints, as
guaranteed by the use of the left Jacobian inverse J−1
L (τ ) (table 3.1). The Gauss-Newton method first step hence
consists of two first order Taylor expansions, one for the logarithmic map and a second for the function f :

  
E k+1 = f k+1 − f T = f Log ∆τ Ek ◦ Exp (τ k ) − f T ≈ f τ k + J−1 L ∆τ Ek − f T
Exp(τ k )
≈ f k + Jk J−1
L ∆τ Ek − f T = E k + Jk J−1 E
Lk ∆τ k (3.64)
Exp(τ k )

30
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

The remaining Gauss-Newton steps are modified accordingly:

 T    T
kE k+1 k = E T T ET
k+1 E k+1 = E k E k + ∆τ k Jk J−1
Lk Jk J−1 E
Lk ∆τ k + 2 ∆τ k
ET
Jk J−1
Lk Ek (3.65)
∂kE k+1 k h T  i−1  T
= 0 −→ ∆τ Ek = − Jk J−1 Lk Jk J−1
Lk Jk J−1
Lk E k −→
∂∆τ Ek
h i−1
∆τ Ek = − J−T Lk JT
J J
k k Lk
−1
J−T T
Lk Jk E k (3.66)

If the perturbation τ X ∧ instead belongs to the local tangent space TX G, it is necessary to employ the right
Jacobian instead of the left one, resulting in:

Xk+1 ←− Xk ⊕ ∆τ X X
k = Exp (τ k ) ◦ ∆τ k (3.67)
h i−1
∆τ X
k = − J−T T −1
Rk Jk Jk JRk J−T T
Rk Jk E k (3.68)

3.7 State Estimation in Lie Groups

The state estimation EKF discussion of section 2.5 states that given a continuous time nonlinear Euclidean state
system (2.81) with process noise provided by (2.82) and (2.83), together with a series of discrete time nonlinear
observations (2.106) with measurement noise given by (2.107) and (2.108), and considering no correlation between
both noises (2.109), it is possible to compute estimations of the Euclidean state and its covariance at the same
time points at which the observations are provided, in such a way that the estimation errors (difference with
respect to the true state) are zero mean. The estimations are obtained by means of (2.136) through (2.142).

Instead of the Euclidean space time varying state vector x (t) ∈ Rm considered in section 2.5, consider now
that the continuous time nonlinear state system is composed by a vector z (t) ∈ Rn , an element of a Lie group
X (t) ∈ G, and a vector vX (t) ∈ Rm representing the velocity of X as it moves along its manifold, contained in
the local or body tangent space TX G. As X is not Euclidean, the direct application of the EKF state estimation
scheme of section 2.5 would result in the need to continuously reproject the estimated Lie group elements Xk
back to the manifold as otherwise the estimated states would not comply with the Lie group constraints. The
repeated deviations and reprojections from and to the manifold may result in a significant degradation in the
estimation accuracy.

The most rigorous and precise way to adapt the EKF scheme is to exclude the Lie group element X ∈ G from the
state system, replacing it by a local tangent space perturbation ∆τ X ∈ TX G. Each filter step now consists on esti-
 T
mating the Lie group element Xˆ+ = X̂ + (tk ) ∈ G, the state vector x̂+ = x̂+ (tk ) = ∆τ̂ X + v̂X + ẑ+ ∈ R2 m+n ,
k k k k k
and its covariance P+ +
k = P (tk ) ∈ R
(2 m+n)x(2 m+n)
, based on their values at tk−1 = (k − 1) ∆t. For clarity
purposes the Lie velocity vX and the state vector z can be grouped into a bigger Euclidean state vector
 T  X T
p = vX z ∈ Rm+n , so x̂+ k = ∆τ̂ k p̂k .

Considering with no loss of generality that the local tangent state perturbation ∆τ X is located on the first
positions of the combined state vector x, the definition of the covariance matrix is a combination of that of its
Euclidean components (2.105) and its local Lie counterparts (3.27), with additional combined members:
" #
CX X
X X ,k CX p,k
Pk = ∈ R(2 m+n)x(2 m+n) (3.69)
CXpX ,k C pp,k
h i h

T i
X X X ,T
CX X ,k = E ∆τ k ∆τ k = E Xk ⊖ µX ,k Xk ⊖ µX ,k ∈ Rmxm (3.70)
h
T i h

T i
CX
X p,k = E ∆τ k
X
pk − µp,k = E Xk ⊖ µX ,k pk − µp,k ∈ Rmx(m+n) (3.71)
h
i h

T i
CX
pX ,k = E pk − µp,k ∆τ X k
,T
= E pk − µp,k Xk ⊖ µX ,k ∈ R(m+n)xm (3.72)
h

T i
Cpp,k = E pk − µp,k pk − µp,k ∈ R(m+n)x(m+n) (3.73)

The following paragraphs do not describe the full state estimation process, but only the changes with respect to

31
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

the Euclidean case described in section 2.5:

• Initialization. The Lie group element X̂0+ , Lie velocity v̂X + +

0 , and Euclidean state vector ẑ0 are initialized
with their expected values, while the local tangent space perturbation ∆τ̂ X 0
+
is initialized to zero. The
+
covariance of the initial estimation error P0 represents the uncertainty in the initial estimation x̂+0.

X̂0+ = µX ,0 = E [X0 ] (3.74)

    h T i
+ X+ + T + + T
x̂+
0 = µx,0 = E [x0 ] = ∆τ̂ X
0 v̂0 ẑ0 = ∆τ̂ X
0 p̂0 = E 0m v X 0 z0 (3.75)
 h

T i h

T i
E X0 ⊖ µX ,0 X0 ⊖ µX ,0 E X0 ⊖ µX ,0 p0 − µp,0
P+
0 =  h

T i h

T i  (3.76)
E p0 − µp,0 X0 ⊖ µX ,0 E p0 − µp,0 p0 − µp,0

• Time Update Equations. The first EKF step propagates the state estimation without the use of any
observations, and is similar to that described in section 2.5. The (2.81) continuous time nonlinear state
system is however replaced by (3.77):
h iT

ẋ (t) = ∆τ̇ X v̇X ż = f X (t) ⊕ ∆τ X (t) , vX (t) , z (t) , u (t) , w (t) , t (3.77)
∆τ̇ X = vX (3.78)
h iT

ṗ = v̇X ż = f p X (t) ⊕ ∆τ X (t) , vX (t) , z (t) , u (t) , w (t) , t (3.79)

Linearization of the continuous time system results in the (2.86) system matrix A (t) ∈ R(2 m+n)x(2 m+n) ,
where various blocks are likely to be based on the ⊖ and − Jacobians of table 3.1. Its discretization by
means of (2.99) leads to the system state transition matrix Fk ∈ R(2 m+n)x(2 m+n) . With no other differences
with respect to the section 2.5 Euclidean process, the manifold element is left unchanged, while the a priori
state vector and error covariance are obtained by means of (3.81) and (3.82):

X̂k− = +
X̂k−1 (3.80)


x̂−
k = x̂+
k−1 + ∆t · f Xˆk−1
+
⊕ ∆τ̂ X +
k−1 , v̂X +
k−1 , ẑ+
k−1 , uk−1 , 0, tk−1 (3.81)
P− = Fk−1 P+ FT e
+Q (3.82)
k k−1 k−1 d,k−1

• Measurement Update Equations. The second EKF step updates the estimations by means of the
observations, and is very similar to that described in section 2.5. The (2.106) discrete time nonlinear
observation system is however replaced by (3.83):


yk = h Xk ⊕ ∆τ X X
k , vk , zk , vk , tk (3.83)

Its linearization results in the (2.112) output matrix Hk ∈ Rqx(2 m+n) , where it is also likely for various
blocks to be based on the ⊖ and − Jacobians of table 3.1. With no other differences with respect to the
section 2.5 Euclidean process, the manifold element is again left unchanged, while the a posteriori state
vector and error covariance are obtained by means of (3.86) and (3.87):
 −1
Kk = P− H T
Hk P −
H T
+ ek
R (3.84)
k k k k

X̂k+ = X̂k− (3.85)

h  i
x̂+
k = x̂−
k + Kk yk − h Xˆk− ⊕ ∆τ̂ X − X− −
k , v̂k , ẑk , 0, tk (3.86)
e T
P+
k = (I − Kk Hk ) P− T
k (I − Kk Hk ) + Kk Rk Kk (3.87)

• Reset Equations. The third EKF step, which is not necessary for purely Euclidean systems, resets the
a posteriori estimation of the tangent space perturbation ∆τ̂ X
k
+
to zero while modifying the a posteriori
estimations for the Lie group element Xˆ and the error covariance P+ accordingly9. Note that the accuracy
+
k k
+
9
The a posteriori estimations for the Lie velocity v̂X
k and the Euclidean components ẑ+
k are not modified in this step.

32
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

of the linearizations present in the previous two steps, which result in the A (t) and Hk system and output
matrices, is based on the first order Taylor expansions present in table 3.1, which are directly related to the
size of the tangent space perturbations. Although it is not strictly necessary to execute this step in every
EKF cycle, the accuracy of the whole state estimation process is improved by maintaining the perturbations
as small as possible, so it is recommended to never bypass the reset step.

Taking into account that the Lie group element is going to be updated per (3.90), the error covariance is
propagated to the new Lie group element per (3.31) as follows:

P+
k ←− D P+ DT (3.88)
 k + 
⊖ Xˆk ⊕ ∆τ̂ X
k
+

D = J⊕ X 0mx(m+n)  ∈ R(2 m+n)x(2 m+n) (3.89)

0(m+n)xm I(m+n)x(m+n)

Once propagated, the Lie group element is updated with the local tangent space perturbation, which is
itself reset to zero:

X̂k+ ←− Xˆk+ ⊕ ∆τ̂ X

k
+
(3.90)
∆τ̂ X
k
+
←− 0m (3.91)

Although the above process has been described making use of a local tangent space perturbation ∆τ X ∈ TX G
and a vector vX ∈ Rm also viewed in the local tangent space that represents the velocity of X ∈ G as it moves
along its manifold, there exists an equivalent formulation that employs perturbations and velocities viewed in the
manifold global tangent space, this is, ∆τ E ∈ TE G and vE ∈ Rm . In this case, the filter estimates the Lie group
 E+ E+ + T
element X̂k+ ∈ G, the state vector x̂+
k = ∆τ̂ k v̂k ẑk ∈ R2 m+n , and the error covariance P+k:
" #
CEX X ,k CEX p,k
Pk = ∈ R(2 m+n)x(2 m+n) (3.92)
CEpX ,k Cpp,k
h i h

T i
CEX X ,k = E ∆τ Ek ∆τ E,T k = E X k ⊟ µ X ,k Xk ⊟ µ X ,k ∈ Rmxm (3.93)
h
T i h

T i
CEX p,k = E ∆τ Ek pk − µp,k = E Xk ⊟ µX ,k pk − µp,k ∈ Rmx(m+n) (3.94)
h
i h

i
T
CEpX ,k = E pk − µp,k ∆τ E,T k = E pk − µp,k Xk ⊟ µX ,k ∈ R(m+n)xm (3.95)

The only additional changes are the use of (∆τ E ⊞ X ) instead of (X ⊕ ∆τ X ), the fact that the blocks of the A (t)
and Hk matrices are now based on the ⊟ and − Jacobians of table 3.1, and the use of J⊟ τ ⊞X
⊞X ∆τ̂ E+ ˆ+ to
k ⊞ Xk
propagate the covariance.

33
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Chapter 4

Rotation of Rigid Bodies

A rigid body is an object in which the distance between any two of its points is constant, as is the orientation
between any two of its vectors (refer to section 3.2 for the definition of points and vectors in R3 ). Rotational
rigid body motion is that in which one of its points, named the center of rotation OCR , does not move. Rigid
body rotations do not comply with the axioms of an Euclidean space (section 3.1) but with those of Lie groups
(section 3.3), and hence this chapter heavily relies on the concepts of Lie theory discussed in sections 3.3 and
3.4. Table 4.1 provides a comparison between the generic nomenclature employed in chapter 3 and their rotation
equivalents. The different representations discussed in this chapter are summarized in table 4.2.
Concept Lie Theory Rotation Concept Lie Theory Rotation
Lie group G SO (3) Lie group elements X, Y R, S
Concatenation ◦ ◦ Lie algebra m so (3)
−1
Identity E IR Inverse X R−1
Velocity v ω Tangent element τ r
Local frame X B Global frame E N
Point action gX () gR (p) Vector action gX () gR∗ (v)
∧ ∧
Adjoint AdX (τ ) AdR (r ) Adjoint matrix AdX τ AdR r

Table 4.1: Comparison between generic Lie elements and those of rigid body rotations

This chapter begins with an introduction to rotational motion in section 4.1, followed by a description of the
different rotation Lie group representations: the rotation matrix (section 4.2), the rotation vector (section 4.3), the
unit quaternion (section 4.4), the half rotation vector (section 4.5), and the Euler angles (section 4.6). Algebraic
operations on rigid body rotations, such as powers, linear interpolation, and the plus and minus operators, are
introduced in section 4.7. Section 4.8 presents the rotation time derivative that leads to the definition of the
angular velocity in the tangent space. The velocity of the rigid body points is discussed in section 4.9, followed
by the adjoint map in section 4.10, which transforms elements of the tangent space while the rotation advances
on its manifold, and by an analysis of uncertainty and covariances applied to rotational motion (section 4.11).
An extensive analysis of the rotation Jacobians is presented in section 4.12. Sections 4.13, 4.14, and 4.15 apply
the discrete integration of Lie groups, the Gauss-Newton optimization of Lie group functions, and the state
estimation of Lie groups contained in sections 3.5, 3.6, and 3.7 to the case of rotations. Finally, the advantages
and disadvantages of each rotation representation are discussed in section 4.16.

4.1 Special Orthogonal (Lie) Group

A rigid body can be represented with a Cartesian frame attached to any of its points (the origin), with the basis
vectors e1 , e2 , and e3 being simply unit vectors along the main axes. It can be assumed with no loss of generality
that the frame origin coincides with the center of rotation. Rigid body rotations can be combined and reversed,

34
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

complying with the algebraic concept of group, but are not endowed with a metric, so they are not part of a metric
or Euclidean space (section 3.1). They do however comply with the axioms of a Lie group (section 3.3), and hence
the set of rigid body rotations together with the operation of rotation concatenation comprises hSO (3) , ◦i, known
as the rotation group or special orthogonal group of R3 [30], where its elements are denoted by R, the identify
rotation by IR , and the inverse by R−1 . The rotation group has two main actions, which are the rotation of points
 
g() : SO (3) × R3 → R3 | p → gR (p) and that of vectors g∗ () : SO (3) × R3 → R3 | v → gR∗ (v) .

Based on the rigid body definition above, its motion corresponds to an orthogonal transformation, this is, one
that preserves the norm (maintaining distances between points as well as angles between vectors) and the cross
product (maintaining orientation)1 [26]. These are called orthogonality and handedness [30]:

• Norm: kgR∗ (v) k = kvk, ∀ v ∈ R3

• Cross product: gR∗ (u) × gR∗ (v) = gR∗ (u × v) , ∀ u, v ∈ R3

Noting that a vector represents the difference between two points, gR∗ (v) = gR∗ (q − p) = gR (q) − gR (p),
and considering the possibility that one of the points may be the origin, gR (p) = gR (0) = 0, results in the
equivalence between the point and vector rotation maps:

gR∗ (q − 0) = gR∗ (q) = gR (q) − gR (0) = gR (q) → gR () = gR∗ () (4.1)

Representation Symbol Structure Space

Rotation matrix R orthogonal 3x3 matrix SO (3)
∧
Angular velocity ω =ω b skew-symmetric matrix so (3)
ω free 3-vector
Rotation vector r∧ = b
r skew-symmetric matrix SO (3) & so (3)
r = ω t = nφ free 3-vector
Unit quaternion q unit quaternion SO (3)
∧
Half angular velocity Ω pure quaternion so (3)
Ω = ω/2 free 3-vector
Half rotation vector h∧ pure quaternion SO (3) & so (3)
h = Ω t = n θ = r/2 free 3-vector
Euler angles φ 3 angles SO (3)

Table 4.2: Summary of rotational motion representations

The SO (3) analysis below adopts the convention introduced in section 3.3, in which all ac-
tions, including concatenation {◦ : SO (3) × SO (3) → SO (3)}, transform elements viewed in the lo-
cal or body frame FB = {OB , b1 , b2 , b3 } into elements viewed in the global or spatial frame
FN = {ON , n1 , n2 , n3 } = {gR (OB ) , gR∗ (b1 ) , gR∗ (b2 ) , gR∗ (b3 )}2 :

pN = gRNB (pB ) (4.2)

vN = gR∗NB (vB ) (4.3)

4.2 Rotation Matrix

The three basis vectors of the output frame can be stacked side by side into a matrix
R = RNB = [n1 n2 n3 ] ∈ R3x3 , called the rotation matrix. Since its columns form a right handed orthonormal
basis, it complies with the orthogonality and handedness conditions, and it can be proven that the rotation
1
An orthogonal transformation can also be defined as one that preserves both the inner and cross products.
2
The spatial and local bases are denoted N and B as they usually correspond to the NED and body frames, respectively. The North
- East - Down or NED frame is centered at the object center of mass, with axes aligned with the geodetic North, East, and Down
directions. The body frame is also centered at the object center of mass, with iB
1 contained in its plane of symmetry pointing forward
along a fixed direction, iB B B
3 also located in the object plane of symmetry, normal to i1 and pointing downward, and i2 orthogonal to
both in such a way that they form a right hand system.

35
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

matrix R is an special orthogonal matrix3 . Rotation matrices hence represent rigid body rotations, and
their space SO (3) = {R ∈ R3x3 | RT R = I3 , det (R) = +1} has group structure under matrix multiplication
{R3x3 × R3x3 → R3x3 | Ra Rb ∈ R3x3 , ∀ Ra , Rb ∈ R3x3 } [19]. While having dimension nine, the special orthog-
onal group SO (3) defined by means of rotation matrices constitutes a three dimensional manifold to Euclidean
space E3 . Note that in this group
 the identity
 element is given by the identity matrix (I = I3 ), and the inverse
−1 T
coincides with the transpose R = R .

Concept SO (3) Rotation Matrix Concept SO (3) Rotation Matrix

Lie group element R R Concatenation ◦ Matrix product
Identity IR I3 Inverse R−1 RT
Point rotation gR (p) Rp Vector rotation gR∗ (v) Rv

Table 4.3: Comparison between generic SO (3) and rotation matrix

The rotation matrix R represents the actual coordinate transformation from the local to the global frame:

gR∗ (v) = R v (4.4)

The inverse rotation (from the global to the local frame), is simply the transpose:

R−1 = RT (4.5)

The concatenation of rotations is also straight forward as it coincides with matrix multiplication. Note that
SO (3) as defined above is not an abelian group, so the order of the factors is important.

REB = REN RNB (4.6)

4.3 Rotation Vector as Tangent Space

As discussed in section 3.3.1, the structure of the Lie algebra associated to SO (3) can be obtained by time derivat-
ing the Lie group inverse constraint, RT (t) R (t) = R (t) RT (t) = I3 , resulting in the following particularizations
of (3.3) and (3.4):
T
ω N∧
NB
b NNB
=ω = ṘNB RT
NB
= −RNB ṘNB (4.7)
B∧ B T T
ω NB
b
=ω NB = RNB ṘNB = −ṘNB RNB (4.8)

The Lie algebra velocity v∧ of SO (3) is known as the angular velocity ω ∧ , and as shown in (4.7) and (4.8),
has the structure of a skew-symmetric matrix because its negative coincides with its transpose, so it is generally
b An alternative definition of the angular velocity is presented in section 4.8. Inverting the previous
denoted as ω.
equations results in the rotation matrix time derivative, which is linear:

ṘNB = ω b BNB
b NNB RNB = RNB ω (4.9)

b (t0 ), and hence the skew-symmetric matrix ω

Notice that if R (t0 ) = I3 , then Ṙ (t0 ) = ω b (t0 ) provides a first order
approximation of the rotation matrix around the identity matrix I3 :

b (t0 ) ∆t
R (t0 + ∆t) ≈ I3 + ω (4.10)

b ∈ R3x3 | ω ∈ R3 , −ω
The space of skew-symmetric matrices so (3) = {ω b T } is hence the tangent space
b =ω
3
Orthogonal means that the transpose equals the inverse, while special or proper means that the determinant is positive one.

36
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .


of SO (3) at the identity I3 [26], denoted as TI3 R. The hat ·∧ : R3 → so (3) | ω → ω ∧ = ω b and vee
∨ 3 ∨

· : so (3) → R | ωb → ω operators convert the Cartesian vector form of the angular velocity into its skew-
symmetric form, and vice versa.

If R (t0 ) 6= I3 , the tangent space needs to be transported right multiplying by RNB (t0 ) (in the case of space
angular velocity), or left multiplying for the local based velocity:
 N   
RNB (t0 + ∆t) ≈ RNB (t0 ) + ω b NB (t0 ) ∆t RNB (t0 ) = I3 + ωb NNB (t0 ) ∆t RNB (t0 ) (4.11)
 B   
RNB (t0 + ∆t) ≈ RNB (t0 ) + RNB (t0 ) ω b NB (t0 ) ∆t = RNB (t0 ) I3 + ω b BNB (t0 ) ∆t (4.12)

b , x (t) ∈ R3 , where ω
Note that the solution to the ordinary differential equation ẋ (t) = x (t) ω b is constant, is
b
ω t
x (t) = x (0) e . Based on it, assuming R (0) = I3 as initial condition, and considering for the time being that
b is constant,
ω
2 n
bt = I + ω b
(ωt) (b
ω t)
R (t) = eω 3 bt + + ··· + + ... (4.13)
2! n!

which is indeed a rotation matrix as it complies with the SO (3) conditions of orthogonality and handedness
[26].

Concept Lie Theory SO (3)

∧
Tangent space element τ r∧ = b
r
Velocity element v∧ ∧
ω =ω b
Structure ∧ skew symmetric matrix

Table 4.4: Comparison between generic SO (3) and rotation vector as tangent space
b t can be realized by maintaining
b is constant, (4.13) means that any rotation R (t) = eω
Remembering that so far ω
a constant angular velocity ω for a given time t. This is analogous to stating that any angular displacement
b φ can be achieved by rotating an angle φ about a fixed unitary rotation axis n, which enables the
R (φ) = en
definition of the rotation vector r, also known as the exponential coordinates of the R rotation, as

r = ω t = n φ ∈ R3 (4.14)

Note that the rotation vector r belongs to the tangent space as it is a multiple of the angular ve-
locity ω ∈ so (3), and hence tends to coincide with it as time tends to zero. The exponential map
∧
 3
{exp () : so (3) → SO (3) | R = exp (r )} and its capitalized form Exp () : R → SO (3) | R = Exp (r) wrap
the rotation vector around the rotation group. In the case of the rotation matrix, the exponential map can be
r2 = r rT − I3 and b
obtained from (4.13) based on the fact that all skew-symmetric matrices verify that b r3 = −b
r,
converting skew symmetric matrices into orthogonal ones:

b
r r2
b
r) = Exp (r) = er = I3 +
b
R (r) = exp (b sin krk + (1 − cos krk) (4.15)
krk krk2

Geometrically, the skew symmetric matrix corresponds to an axis of rotation (via the mapping n → nb ) and the
exponential map generates the rotation corresponding to rotating about that axis by an amount φ [31].

The angular velocity ω however is in fact not required to be constant. Given a rotation matrix R ∈ SO (3), it
can be proven that there exists a not necessarily unique vector r ∈ R3 such that R = er . The logarithmic map
b

{log () : SO (3) → so (3) | r∧ = log (R)} and its capitalized version Log () : SO (3) → R3 | r = Log (R) hence

37
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

convert rigid body rotations into rotation vectors.

   
R11 R12 R13   R32 − R23
  trace (R) − 1 krk  
R = RNB = R21 R22 R23  → krk = arccos , r= R13 − R31  (4.16)
2 2 sin krk
R31 R32 R33 R21 − R12

Any rotation matrix can hence be realized by rotating a certain angle about a given axis, as indicated in (4.14).
The vector n = r/krk indicates the rotation direction while φ = krk represents the turn angle. The exponential
map described by (4.15) is thus surjective (there is at least one rotation vector for every rotation matrix) but not
injective, as a rotation of (krk + 2 k π) ∀ k ∈ Z about r/krk or a rotation of (−krk + 2 k π) about −r/krk produce
exactly the same rotation matrix.

Although inverting the rotation by means of the rotation vector is straightforward,

rBN = rNB −1 = −rNB (4.17)

the different SO (3) actions (concatenation, point rotation, vector rotation), as well as the relationship between
the rotation vector derivative with time and the angular velocities, are complex and rarely used.

4.4 Unit Quaternion

The quaternions with unity norm, known as unit quaternions, comprise an additional representation of the
rotation group SO (3), as shown below. Quaternions in turn are generalizations of complex numbers in the same
way that these are generalizations of real ones [26]. It is hence necessary to first describe the complex numbers in
section 4.4.1 and the quaternions in section 4.4.2 before focusing on the unit quaternions in section 4.4.3.

4.4.1 Complex Numbers


The set of complex numbers C is composed of two real numbers C = R + R i | i2 = i · i = −1 . Given two
complex numbers c1 = x1 + y1 i ∈ C, c2 = x2 + y2 i ∈ C, ∀ x1 , y1 , x2 , y2 ∈ R, it is possible to define the operations
of addition {+ : C × C → C} and multiplication {· : C × C → C}.

c1 + c2 = (x1 + y1 i) + (x2 + y2 i) = (x1 + x2 ) + (y1 + y2 ) i (4.18)

c1 · c2 = c1 c2 = (x1 + y1 i) · (x2 + y2 i) = (x1 x2 − y1 y2 ) + (x1 y2 + y1 x2 ) i (4.19)

∗
The conjugate is defined as c∗ = x − y i ∈ C and verifies that (c1 · c2 ) = c∗1 · c∗2 , while the norm
√ √ p
kck = c · c∗ = c∗ · c = x2 + y2 ∈ R satisfies that kc1 · c2 k = kc1 k · kc2 k. The set of complex numbers C en-
dowed with the operations of addition + and multiplication · forms a field (not ordered), known as the field of
complex numbers hC, +, ·i, nearly always abbreviated to simply C. The additive identity is 0 = 0 + 0 i and the
inverse −c = −x − y i, while the multiplication identity is 1 = 1 + 0 i and the inverse c−1 = c∗ /kck2 .
 
Complex numbers can always be written in polar form c = r (cos φ + sin φ i) = r ei φ , and as such are valid
representations of the circle group or plane rotations group SO (2), similarly to the case of rotation vectors in
SO (3) described in section 4.3.

4.4.2 Quaternions
The set of quaternions H is defined as {H = C + C j | j2 = −1, i · j = −j · i}. A quaternion q ∈ H has the form
q = q0 + q1 i + q2 j + q3 i j, with qi ∈ R. Pure quaternions q = q1 i + q2 j + q3 i j ∈ Hp are those defined in the
tridimensional imaginary subspace of H, and verify that q = − q∗ .

There are many different conventions for the quaternion found in the literature [30]. This document adopts the

38
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Hamilton convention, characterized by locating the real part first (instead of last), being right handed (left),
passive (rotates frames and not vectors as in active), and local to global rotations (global to local). Any variation
to these choices would result in different expressions below, although the physical concepts do not vary.

The real plus imaginary notation {1, i, j, i j} is not always the most convenient. A quaternion can also be ex-
pressed as the sum of a scalar plus a vector in the form q = q0 + qv , where q0 is the real or scalar part and
qv = q1 i + q2 j + q3 i j is the imaginary or vector part. Quaternions are however mostly represented as 4-vectors
T T
q = [q0 qv ] = [q0 q1 q2 q3 ] , which enables the usage of matrix algebra for quaternion operations. It is also
convenient to abuse the equal operator by combining general, real, and pure quaternions as in q = q0 + qv , where
T T
q0 = [q0 0v ] and qv = [0 qv ] .

The following expressions define the addition {+ : H × H → H} and inner product {h· , ·i : H × H → R} of two
quaternions, which commute, as well as the scalar multiplication {· : R × H → H}:

q+p = [q0 qv ]T + [p0 pv ]T = [q0 + p0 q1 + p1 q2 + p2 q3 + p3 ]T = [q0 + p0 qv + pv ]T (4.20)

hq, pi = q · p = qT p = q0 p0 + q1 p1 + q2 p2 + q3 p3 (4.21)
T T
a·q = a · [q0 qv ] = [a q0 a · qv ] (4.22)

The multiplication of quaternions {⊗ : H × H → H} is not commutative as it includes the cross product:

 
q0 · p0 − q1 · p1 − q2 · p2 − q3 · p3
  " #
q1 · p0 + q0 · p1 − q3 · p2 + q2 · p3  q 0 p0 − q T
p
q⊗p= 
q · p + q · p + q · p − q · p  = q p + p q + q
v v
(4.23)
 2 0 3 1 0 2 1 3 0 v 0 v b v pv
q3 · p0 − q2 · p1 + q1 · p2 + q0 · p3

It is also bilinear [30]:

     
+q0 −q1 −q2 −q3 p0 +p0 −p1 −p2 −p3 q0
     
+q1 +q0 −q3 +q2  p1  +p1 +p0 +p3 −p2   q1 
q ⊗ p = [q]L p = 
+q
   = [p]R q =    (4.24)
 2 +q3 +q0 −q1   
 p2 
+p
 2 −p3 +p0 +p1   
  q2 
+q3 −q2 +q1 +q0 p3 +p3 +p2 −p1 +p0 q3

These expressions can be simplified for the common case of the multiplication of a quaternion q by a pure
quaternion pv , this is, {⊗ : H × Hp → H}:
 
−q1 · p1 − q2 · p2 − q3 · p3
  " #
+q0 · p1 − q3 · p2 + q2 · p3 
  −qv T pv
q ⊗ pv =  = q p +q (4.25)
+q3 · p1 + q0 · p2 − q1 · p3  0 v b v pv
−q2 · p1 + q1 · p2 + q0 · p3
    
−q1 −q2 −q3   0 −p1 −p2 −p3 q0
  p   
+q0 −q3 +q2   1  +p1 0 +p3 −p2  q1 
q ⊗ pv = [q]L3 pv =  
+q +q −q  p2  = [pv ]R3 q = +p
   (4.26)
 3 0 1   2 −p3 0 +p1   
 q2 
p3
−q2 +q1 +q0 +p3 +p2 −p1 0 q3

∗
The conjugate quaternion is defined as q∗ = q0 − qv ∈ H and verifies that (q ⊗ p) = p∗ ⊗ q∗ ,
p p √ √
while the quaternion norm kqk = hq , q∗ i = hq∗ , qi = q ⊗ q∗ = q∗ ⊗ q ∈ R satisfies that
kq ⊗ pk = kp ⊗ qk = kqk kpk. Quaternions endowed with ⊗ form the non-commutative group hH, ⊗i,
where q1 = 1 + 0v is the identity and q−1 = q /kqk2 the inverse [30]. Additionally, quaternions endowed with
∗

addition + and multiplication ⊗ form the ring hH, +, ⊗i where q0 = 0 + 0v is the addition identity and −q the
addition inverse or negative.

The quaternion rotation operator or double product is defined as {H × R3 → R3 | q ∈ H,

39
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

v ∈ R3 → q ⊗ v ⊗ q∗ ∈ R3 }4 , while the natural power of a quaternion qn , n ∈ N is obtained by multiply-

ing the quaternion by itself n − 1 times.

4.4.3 Unit Quaternion

Unit quaternions verify that kqk = 1, which implies that q−1 = q∗ . They can always be written as

q = cos θ + n sin θ (4.27)

where n is a unit vector and θ is a scalar. The exponential of a quaternion eq is defined analogously to that of real
numbers [30]. For pure quaternions q = qv , if abusing notation with qv = v = n θ where θ = kvk and knk = 1,
it can be proven that eqv = cos θ + n sin θ, which can be considered an extension of the eiθ = cos θ + i sin θ
expression for complex numbers introduced in section 4.4.1 [30]. Notice that since keqv k = 1, the exponential


of a pure quaternion is a unit quaternion. If kqk = 1, it is easy to verify that log (q) = log en θ = q , so the v
logarithm of a unit quaternion is a pure quaternion [30].

Unit quaternions endowed with ⊗ constitute a subgroup that represents a 3-sphere, this is, the three di-
mensional surface of the unit sphere of R4 , and is commonly noted as S3 . They comply with the or-
thogonality and handedness conditions required in section 4.1 for rigid body rotations, and hence their
space SO (3) = {q ∈ S3 | q∗ ⊗ q = q ⊗ q∗ = q1 } possesses group structure under quaternion multiplication
{⊗ : S3 × S3 → S3 | qa ⊗ qb ∈ S3 , ∀ qa , qb ∈ S3 } [19]. While having dimension four, the special orthogonal group
SO (3) defined by means of unit quaternions constitutes a three dimensional manifold to Euclidean space E3 . Note
that in this group q1 constitutes the identity and q∗ the inverse.
Concept SO (3) S3 Concept SO (3) S3
Lie group element R q Concatenation ◦ ⊗
−1
Identity IR q1 Inverse R q∗
Point rotation gR (p) q ⊗ p ⊗ q∗ Vector rotation gR∗ (v) q ⊗ v ⊗ q∗

Table 4.5: Comparison between generic SO (3) and unit quaternion

Coordinate transformation (point or vector rotation) and rotation concatenation are both linear:

gR∗ (v) = q ⊗ v ⊗ q∗ (4.28)

qEB = qEN ⊗ qNB (4.29)

4.5 Half Rotation Vector as Tangent Space

It is interesting to point out that in the case of rotation matrices (section 4.2), the orthogonality (RT R = I3 )
and handedness (det (R) = +1) constraints constitute two different expressions, while in the case of quaternions
both are contained within (q∗ ⊗ q = q ⊗ q∗ = q1 ). As in other Lie groups, the time derivation of this constraint
∗
results in the structure of the Lie algebra. Derivating leads to q∗ ⊗ q̇ = − (q∗ ⊗ q̇) , which indicates that q∗ ⊗ q̇
is in fact a pure quaternion, as is q̇ ⊗ q∗ . This results in the following particularizations of (3.3) and (3.4):

ΩN∧
NB = q̇NB ⊗ q∗NB = −qNB ⊗ q̇∗NB (4.30)
ΩB∧
NB = q∗NB ⊗ q̇NB = −q̇∗NB ⊗ qNB (4.31)

The Lie algebra velocity v∧ of S3 is known as the half angular velocity Ω∧ [30], and as shown in (4.30) and (4.31),
has the structure of a pure quaternion because its negative coincides with its conjugate:

Ω∧ (t) = [0, Ω (t)]T ∈ Hp (4.32)

4
It is easily proven that the double quaternion product results in R3 and not R4 .

40
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Inverting the previous equations results in the unit quaternion time derivative, which is linear:

q̇NB = ΩN∧
NB
⊗ qNB = qNB ⊗ ΩB∧
NB
(4.33)

Notice that if q (t0 ) = q1 , then q̇ (t0 ) = Ω (t0 ), and hence the pure quaternion Ω∧ (t0 ) provides a first order
approximation of the unit quaternion around the identity q1 :

q (t0 + ∆t) ≈ q1 + Ω∧ (t0 ) ∆t (4.34)

The space of pure quaternions so (3) = {Ω∧ ∈ Hp | Ω ∈ R3 } is hence the tangent space of the unit sphere

S3 of quaternions at the identity q1 , denoted as Tq1 R. The hat ·∧ : R3 → so (3) | Ω → Ω∧ and vee
n
∨ o
·∨ : so (3) → R3 | Ω∧ → Ω operators convert the half angular velocity vector into its pure quaternion
form, and vice versa.

If q (t0 ) 6= q1 , the tangent space needs to be transported right multiplying by qNB (t0 ) (in the case of space
tangent space), or left multiplying for the local space:

qNB (t0 + ∆t) ≈ qNB (t0 ) + [ΩN∧

NB
(t0 ) ∆t] ⊗ qNB (t0 ) = [q1 + ΩN∧
NB
(t0 ) ∆t] ⊗ qNB (t0 ) (4.35)
B∧ B∧
qNB (t0 + ∆t) ≈ qNB (t0 ) + qNB (t0 ) ⊗ [Ω NB (t0 ) ∆t] = qNB (t0 ) ⊗ [q1 + Ω NB (t0 ) ∆t] (4.36)

Note that the solution to the ordinary differential equation ẋ (t) = x (t) ⊗ Ω∧ , x (t) ∈ R4 , where Ω∧ is constant,
∧
is x (t) = x (0) eΩ t . Based on it, assuming q (0) = q as initial condition, and considering for the time being
1
that Ω is constant,

2
n
Ω ∧
t ∧ Ω∧ t Ω∧ t
q (t) = e = q1 + Ω t + + ···+ + ... (4.37)
2! n!

which is indeed a unit quaternion [30].

Concept Lie Theory SO (3)

∧ ∧ T
Tangent space element τ h = [0 h]
T
Velocity element v∧ Ω∧ = [0 Ω]
Structure ∧ pure quaternion

Table 4.6: Comparison between generic SO (3) and half rotation vector as tangent space
∧
Remembering that so far Ω∧ is constant, (4.37) means that any rotation q (t) = eΩ t can be realized by main-
taining a constant half angular velocity Ω∧ in Hp for a given time t. This is analogous to stating that any angular
∧
displacement q (θ) = en θ can be achieved by rotating an angle θ about a fixed unitary rotation axis n∧ ∈ H . p
It is customary to absorb t into Ω or θ into n, resulting in the half rotation vector h:

h = Ω t = n θ ∈ R3 (4.38)

Expression (4.37) represents the exponential map {exp : so (3) → SO (3) | h∧ ∈ Hp → exp h∧ ∈ S3 } [30], which
transforms pure quaternions into unit quaternions. Before continuing, it is possible to compare the similarities
and differences between the exponential maps when applied to rotation matrices versus quaternions, as both
represent maps between the tangent space so (3) and the special orthogonal group SO (3). In the case of rotation
matrices, this translates to a map between skew-symmetric matrices and orthogonal ones, while for quaternions
the exponential map converts pure quaternions into unitary ones. There is one additional difference, however.
In the case of rotation matrices, the map encodes through the rotation vector r = ω t = n φ, this is, the axis of
rotation n and the rotated angle φ. In the case of quaternions, the encoding is through the half rotation vector
h = Ω t = n θ. Since the rotation is accomplished by the double product q ⊗ v ⊗ q∗ as noted in (4.28), the vector

41
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

v experiences a rotation that is twice that encoded in q, which means that q encodes half the intended rotation
on v. This implies that the space of unit quaternions S3 is in fact a double covering of SO (3), not SO (3) itself
[30].

r = 2·h (4.39)
ω = 2·Ω (4.40)
φ = 2·θ (4.41)

Taking these relationships into consideration, it is possible to obtain a more practical expression


for the exponential map exp () : so (3) → SO (3) | R = exp h∧ = exp (r∧ /2) and its capitalized form

Exp () : R3 → SO (3) | R = Exp (h) = Exp (r/2) than (4.37) [30]:

∧ ∧ ∧ φ φ
q (h) = q (r/2) = Exp (h) = Exp (r/2) = eΩ t = en θ = en φ/2 = cos + n sin (4.42)
2 2

When regarded as a pure quaternion in Hp , the angle θ between a unit quaternion q and the identity q1 is
cos θ = q1 T q = q0 . At the same time, the angle φ rotated by the unit quaternion q on objects in R3 satisfies
(4.42), so the angle between a unit quaternion vector and the identify in Hp is half the angle rotated by the unit
quaternion in R3 space, as depicted by (4.41).

So far the exponential map has been obtained based on the assumption of constant angular velocity, but this does
not need to be the case. Given a unit quaternion q ∈ S3 , there exists a not necessarily unique rotation vector
r = 2 · h ∈ R3 such that q = Exp (r/2) = Exp (n φ/2):
 
kqv k
φ = 2 arctan (4.43)
q0
qv
n = (4.44)
kqv k

This expression represents the capitalized logarithmic map {log : SO (3) → so (3) | q ∈ S3 → h = r/2 ∈ R3 } [30].
As in the case of the rotation matrix described in section 4.3, the exponential map described by (4.42) is surjective
but not injective, as a rotation of (krk + 2 k π) ∀ k ∈ Z about r/krk produces exactly the same unit quaternion.
In contrast with the case of rotation matrices, a rotation of (−krk + 2 k π) about −r/krk produces the opposite
(negative) unit quaternion, although both represent the same rotation.

This shows that the map from rotation matrix to unit quaternion {SO (3) → SO (3) | R ∈ R3x3 → q ∈ S3 } is
also surjective but not injective, as there are two and only two quaternions corresponding to the same rotation
matrix (R (q) = R (−q)). The reason is again the double covering of SO (3) by the unit quaternion [30]. One
quaternion induces a rotation in R3 that follows the shortest direction to the final angle (φ < π/2), while the
opposite quaternion rotates the opposite way reaching the same final angle after rotating (φ > π/2).

As the vector Ω represents half the angular velocity ω, it is possible to adjust expressions (4.30), (4.31), and
(4.33):

ω N∧
NB
= 2 q̇NB ⊗ q∗NB (4.45)
ω B∧
NB = 2 q∗NB ⊗ q̇NB (4.46)
1 N∧ 1
q̇NB = ω ⊗ qNB = qNB ⊗ ω B∧ (4.47)
2 NB 2 NB

4.6 Euler Angles

All the previous representations have some type of redundancy as their dimension is higher than three. It is
always possible, however, to pick three unitary vectors (n1 , n2 , n3 ) forming a basis5 and perform three consecutive
5
The base vectors do not need to be orthogonal, just linearly independent.

42
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

rotations to define a map {R3 → SO (3) | (β1 , β2 , β3 ) ∈ R3 → R = en b 3 β3 ∈ SO (3)} [26].

b 2 β2 e n
b 1 β1 e n

In case the selected basis is orthonormal there are only twelve possible combinations or Euler angles, of
which 3 − 2 − 1 is the one employed in this document6 . The rotation from the spatial or global frame
FN = {OCR , n1 , n2 , n3 } to the local or body frame FB = {OCR , b1 , b2 , b3 } is performed by first rotating a yaw
angle (y) about n3 , followed by rotating a pitch angle (p) about n′2 , and finally rotating a bank or roll angle (r)
about n′′1 , where n′2 is the result of applying the first rotation to n2 and n′′1 , which coincides with b1 , that of
applying the first two rotations to n1 .
     
1 0 0 cp 0 sp cy −sy 0
b 1 r = 0 cr
R1 (r) = en
 b2 p =  0
−sr R2 (p) = en 1

0  R3 (y) = e b
n 3 y 
= sy cy

0 (4.48)
 
0 sr cr −sp 0 cp 0 0 1

where s and c stand for sine and cosine respectively. The complete map for the three rotations is:
 
+cp · cy −cr · sy + sr · sp · cy +sr · sy + cr · sp · cy
 
RNB = R3 (y) R2 (p) R1 (r) = +cp · sy +cr · cy + sr · sp · sy −sr · cy + cr · sp · sy (4.49)
−sp +sr · cp +cr · cp

T
In this document the Euler angles are denoted by φNB = [y, p, r] . They can also be obtained from the rotation
matrix, but there are singular instances (p = ±π/2) where the angles can not be uniquely determined.

R21 R32
y = arctan p = arcsin (−R31 ) r = arctan (4.50)
R11 R33

4.7 Rotational Motion Algebraic Operations

The basic algebraic operations of addition, subtraction, multiplication, division, and exponentiation are not
defined for objects of the special orthogonal group SO (3), no matter if they are represented by a rotation matrix
R ∈ R3x3 , a rotation vector r ∈ R3 , or a unit quaternion q ∈ S3 . However, as members of the special orthogonal
group SO (3), all rotation representations are closed under a given operation that represents the concatenation of
rotations, and define not only an identity rotation that represents the lack of rotation, but also an inverse operation
representing the opposite rotation. The concatenation of rotations and the identity and inverse operations enable
the definition of the power, exponential and logarithmic operators (section 4.7.1), the spherical linear interpolation
(section 4.7.2), and the perturbations together with the plus and minus operators (section 4.7.3).

4.7.1 Powers, Exponentials and Logarithms

Any rotation can be executed by rotating a given angle φ about a fixed rotation axis n, resulting in
the rotation vector r = n φ (section 4.3) or its half h = n φ/2 (section 4.5). Taking a multiple or a
fraction of a rotation vector is hence straightforward, as t r = t n φ = n (t φ) ∀ t ∈ R, r ∈ so (3). The ex-
ponential maps defined in (4.15) and (4.42) are named that way because they comply with the behav-
ior of the real exponential function expb (a) = exp (a · b) ∀ a, b ∈ R. As such, the exponential function
{exp() : R3 × R → SO (3) | r ∈ R3 , t ∈ R → Rt = Exp (t r) ∈ SO (3)} is defined as:

b
r r2
b
Rt (r) = R (t r) = Exp (t r) = I3 + sin kt rk + (1 − cos kt rk) (4.51)
krk krk2
tφ tφ
qt (h = r/2) = q (t h = t r/2) = Exp (t r/2) = cos + n sin (4.52)
2 2

In a similar way, the logarithmic maps defined in (4.16), (4.43), and (4.44) also comply with the be-
6
This means to first rotate about the 3rd axis, then about the resulting 2nd axis, and finally about the ensuing 1st axis.

43
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

havior of the real logarithmic function b · log (a) = log ab ∀ a, b ∈ R. As such, the logarithmic function


{log() : SO (3) × R → R3 | R ∈ SO (3) , t ∈ R → t r = Log Rt ∈ R3 } is defined as:
 
log Rt (r) = Log (Exp (t r)) = t Log (R (r)) = t Log (Exp (r)) = t r (4.53)
 
log qt (h = r/2) = Log (Exp (t r/2)) = t Log (q (r/2)) = t Log (Exp (r/2)) = t r/2 (4.54)

4.7.2 Spherical Linear Interpolation

Given two rotations R0 , R1 ∈ SO (3), spherical linear interpolation (SLERP) seeks to obtain a rotation function
R (t) , t ∈ R that linearly interpolates from R (0) = R0 to R (1) = R1 in such a way that the rotation occurs at
constant angular velocity along a fixed axis [30].

If employing unit quaternions, ∆q is according to (4.29) the full rotation required to go from q0 to q1 , such that
q1 = q0 ⊗ ∆q, from where ∆q = q∗0 ⊗ q1 . The corresponding rotation vector is then ∆r = n ∆φ = 2 Log (∆q).
The quaternion corresponding to a fraction of the full rotation δφ = t ∆φ is the following:
     
n δφ n ∆φ ∆r
δq = Exp = Exp t = Exp t
2 2 2


t
= Exp t Log (∆q) = Exp t Log (q0 ⊗ q1 ) = (q∗0 ⊗ q1 )
∗
(4.55)

The interpolated unit quaternion is hence the following:

t
q (t) = q0 ⊗ (q∗0 ⊗ q1 ) (4.56)

Because of the double covering of SO (3) by the quaternion, only the interpolation between quaternions at acute
angles (∆θ = ∆φ/2 ≤ π/2), is executed following the shortest path, which occurs if cos ∆θ = q0 T q1 < 0. If this
is not the case, just replace q1 by −q1 and repeat the process.

A similar result is obtained when employing rotation matrices instead of unit quaternions:
 t
R (t) = R0 RT
0 R1 (4.57)

4.7.3 Plus and Minus Operators

A perturbed rigid body rotation R e ∈ SO (3) can always be expressed as the composition of the unperturbed
rotation R with a (usually) small perturbation ∆R. Perturbations can be specified either at the local or body
frame FB , this is, at the local vector space tangent to SO (3) at the actual orientation, in which case they are
known as local perturbations. They can also be specified at the global frame FN , which coincides with the vector
space tangent to SO (3) at the origin; in this case they are known as global perturbations [30]. Local perturbations
appear on the right hand side of the rotation composition, resulting in R e = R ◦ ∆RB , while global ones appear
e = ∆R ◦ R.
to the left, hence R N

The plus and minus operators are introduced in section 3.3.2 and enable operating with increments of the
nonlinear SO (3) manifold expressed in the linear tangent vector space so (3). There exist right (⊕, ⊖) or left
(⊞, ⊟) versions depending on whether the increments are viewed in the local frame (right) or the global one
(left). It is important to remark that although perturbations and the plus and left operators are best suited to
work with small rotation changes (perturbations), the expressions below are generic and work just the same no
matter the size of the perturbation.
e = R ⊕ ∆rB = R ◦ Exp (∆rB )} produces a rotation el-
The right plus operator {⊕ : SO (3) × so (3) → SO (3) | R
ement Re resulting from the composition of a reference rotation R with an often small rotation ∆rB = nB ∆φ,
contained in the tangent space to the reference rotation R, this is, in the local space [30]. The left plus op-
e = ∆rN ⊞ R = Exp (∆rN ) ◦ R} is similar but the often small rotation
erator {⊞ : so (3) × SO (3) → SO (3) | R

44
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

∆rN = nN ∆φ is contained in the tangent space at the identify or global space. The expressions shown below are
valid up to the first coverage of SO (3), this is, φ < π. In the cases of rotation matrix and unit quaternion, the
plus operator is defined as:
e
R = R ⊕ ∆rB = R Exp (∆rB ) = R ∆RB (4.58)
e
q = q ⊕ ∆rB = q ⊗ Exp (∆rB /2) = q ⊗ ∆qB (4.59)
e
R N N
= ∆r ⊞ R = Exp (∆r ) R = ∆R R N
(4.60)
e
q = ∆rN ⊞ q = Exp (∆rN /2) ⊗ q = ∆qN ⊗ q (4.61)

The right minus operator {⊖ : SO (3) × SO (3) → so (3) | ∆rB = Re ⊖ R = Log R−1 ◦ R e }, as well as the left


e ⊟ R = Log R
{⊟ : SO (3) × SO (3) → so (3) | ∆rN = R e ◦ R−1 }, represent the inverse operations, returning the
rotation vector difference ∆r between two rotations R and R e expressed in either the local or global tangent
spaces to R.
 
∆rB = e ⊖ R = Log RT R
R e = Log (∆RB ) (4.62)
B ∗
∆r = e ⊖ q = 2 Log (q ⊗ q
q e) = 2 Log (∆q ) B
(4.63)
 
∆rN = e ⊟ R = Log R
R e RT = Log (∆RN ) (4.64)
N ∗
∆r = e ⊟ q = 2 Log (e
q q ⊗ q ) = 2 Log (∆q ) N
(4.65)

If the ∆r perturbation is small, the (4.13) and (4.37) Taylor expansions can be truncated, resulting in the following
expressions, valid for both the body frame (∆rB ) or the global one (∆rN ):

∆R = Exp (∆r) ≈ I3 + ∆r∧ = I3 + ∆φ n

b (4.66)
∧ T
∆q = Exp (∆r/2) ≈ q1 + ∆r /2 = [1, n ∆φ/2] (4.67)

4.8 Rotational Motion Time Derivative and Angular Velocity

Let R (t) ∈ SO (3) , t ∈ R be a rotating rigid body, and compute its derivative with time, which belongs to neither
SO (3) nor so (3) but to the Euclidean space of the chosen rotation representation, R3x3 for the rotation matrix
and H for the unit quaternion:

R (t + ∆t) − R (t)
Ṙ (t) = lim (4.68)
∆t→0 ∆t

Considering the time modified rotation R (t + ∆t) as the perturbed state (section 4.7.3), the resulting time
derivatives for the rotation matrix and unit quaternion representations are the following:


R ∆RB − R R I3 + ∆φ n b B − I3 ∆φ nbB
Ṙ (t) = lim ≈ lim = R lim (4.69)
∆t→0 ∆t ∆t→0 ∆t ∆t→0 ∆t
h i
T
q ⊗ ∆qB − q q ⊗ [1 n ∆φ/2] − q1
B
[0 nB ∆φ/2]
T
q̇ (t) = lim ≈ lim = q ⊗ lim (4.70)
∆t→0 ∆t ∆t→0 ∆t ∆t→0 ∆t

Similar expressions based on rN = ∆φ nN can be obtained if left multiplying by the perturbation instead of right
multiplying. The Ṙ (t) and q̇ (t) expressions (4.9) and (4.47) are then directly obtained when defining the body
angular velocity ωBNB as the time derivative of the rotation vector rB = nB φ when viewed in local or body frame
FB , and the spatial angular velocity ω NNB as the time derivative of the rotation vector rN = nN φ when viewed in
global or spatial frame FN :
∆rB nB ∆φ
ω BNB (t) = ∆ṙB (t) = lim = lim (4.71)
∆t→0 ∆t ∆t→0 ∆t
∆r N
nN ∆φ
ω NNB (t) = ∆ṙN (t) = lim = lim (4.72)
∆t→0 ∆t ∆t→0 ∆t

45
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Note that the rotation of the angular velocity (relationship between ω NNB and ω BNB ) is not given by the rotation
action gR∗ (4.3) but by the adjoint map AdR described in section 4.10, although in the case of the SO (3) rotation
group, both maps coincide.

4.9 Rotational Motion Point Velocity

There exists a direct relationship between the velocity of a point belonging to a rigid body and the elements
of its tangent space, this is, the angular velocity in so (3) in the case of rotational motion. This relationship is
independent of the SO (3) representation, although the rotation matrix is employed in the expressions below. As
discussed in section 4.1, the rotation actions have the same form for points as for vectors (4.1). Hence, if pB are
the fixed coordinates of a point belonging to the FB rigid body, the point spatial coordinates pN can be obtained
by means of (4.4):

pN (t) = gRNB (t) (pB ) = RNB (t) pB (4.73)

The velocity of a point is the time derivative of its spatial or global coordinates. As p is fixed to FB , its time
derivative is zero (ṗB = 0), so its velocity viewed in the spatial frame responds to:

vNp (t) = ṗN (t) = ṘNB (t) pB (4.74)

Although ṘNB maps the point body coordinates to its spatial velocity per (4.74), its high dimension makes it
ω NNB , ω
inefficient [31]. By making use of the spatial and body instantaneous angular velocities (b b BNB ) introduced
in (4.9), the velocity of a point pB viewed in FN can be obtained as follows:

vNp (t) = b NNB (t) RNB (t) pB = ω

ω b NNB (t) pN (t) (4.75)
B
vNp (t) = b
RNB (t) ω NB (t) pB (4.76)

The velocity of pB viewed in FB can then be obtained by means of the vector action map:


vBp (t) = g−1 N T N
b BNB (t) pB
RNB(t) ∗ vp (t) = RNB (t) vp (t) = ω (4.77)

The point velocity is hence the result of the cross product between the angular velocity and the point coor-
dinates (4.76, 4.77). Similar expressions are obtained if employing the unit quaternion (vNp (t) = ωNNB∧ ⊗ pN (t),
∧
vBp (t) = ω BNB ⊗ pB ).

4.10 Rotational Motion Adjoint

The adjoint map of a Lie group is defined in section 3.3.3 as an action of the Lie group on its own
Lie algebra that converts between the local tangent space and that at the identity. In the case of
rotational motion, both the rotation vector and the angular velocity belong to the tangent space, so

Ad () : SO (3) × so (3) → so (3) | AdR (r∧ ) = R ◦ r∧ ◦ R−1 , AdR (ω∧ ) = R ◦ ω ∧ ◦ R−1 . This is equivalent
to q ⊗ ω∧ ⊗ q∗ for unit quaternions or R ωb RT for rotation matrices, which represents the congruency transfor-
mation7 between the spatial and body angular velocities ω b NNB and ω
b BNB :

ω b BNB RT
b NNB = RNB ω NB
(4.78)

The application of the vee operator results in the adjoint matrix coinciding with the rotation matrix itself
7
Two square matrices A and B are called congruent if B = PT A P for some invertible square matrix P.

46
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

{AdR ω = R ω}8 , implying that elements of the SO (3) tangent space (both rotation vectors and angular veloc-
ities) can be transformed by means of the rotation action as any other free vector. Note that this result only
applies to rotational motion, as for example the vector action and adjoint matrix of SE (3) discussed in chapter
5 do not coincide:

ω NNB = AdRNB ω BNB = RNB ωBNB (4.79)

A similar process leads to the inverse adjoint matrix (Ad−1 T

R ω = AdR−1 ω = R ω):

ω BNB = Ad−1 N T N
RNB ω NB = RNB ω NB (4.80)

4.11 Rotational Motion Uncertainty and Covariance

Following the analysis of uncertainty on Lie groups presented in section 3.3.5, the definitions of local and global
autocovariances for SO (3) elements around a nominal or expected rotation E [R] = µR ∈ SO (3) are the follow-
ing:
  h i
T
CBRR = E ∆rB ∆rBT = E (R ⊖ µR ) (R ⊖ µR ) ∈ R3x3 (4.81)
  h i
T
CNRR = E ∆rN ∆rNT = E (R ⊟ µR ) (R ⊟ µR ) ∈ R3x3 (4.82)

Note that although the notation refers to the covariance of the rotation manifold R ∈ SO (3), the definition in
fact refers to the covariance of the rotation vectors ∆rB or ∆rN located in the tangent space, with its dimension
(3) matching the number of degrees of freedom of the SO (3) manifold. The relationship between the local and
global autocovariances responds to:

CNRR = AdRNB CBRR AdT B T

RNB = RNB CRR RNB (4.83)

Given a function {f : R → S | S = f (R) ∈ SO (3) , ∀R ∈ SO (3)} between two rotations, the covariances are prop-
agated as follows:
⊖ f (R) B ⊖ f (R),T
CBSS = J CRR J ∈ R3x3 (4.84)
⊕R ⊕R
⊟ f (R) N ⊟ f (R),T
CNSS = J CRR J ∈ R3x3 (4.85)
⊞R ⊞R

4.12 Rotational Motion Jacobians

Lie group Jacobians are introduced in section 3.4 based on the right and left Lie group derivatives of section
3.3.4, and in this section are customized for the SO (3) case, with table 4.7 representing the particularization of
table 3.1 to the case of rigid body rotations. The various Jacobians listed in table 4.7 have been obtained by
means of the chain rule, the expressions already introduced in this document, and those of section 3.3. Note that
although in many cases the results include the rotation matrix, all Jacobians are generic and do not depend on
the specific SO (3) parameterization.

In addition to the adjoint matrix, two other Jacobians are of particular importance as they appear repeatedly in
table 4.7. These are the right and left Jacobians of the capitalized exponential function, also known as simply
the right Jacobian JR (r) and the left Jacobian JL (r), and they evaluate the variation of the so (3) tangent space
provided by the output of the Exp (r) map (locally for JR and globally for JL ) while moving along the SO (3)
manifold with respect to the (Euclidean) variations within the original tangent space provided by r. Their closed
8
The adjoint matrix is generic and applicable to all SO (3) representations.

47
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

forms as well as those of their inverses are included in table 4.7, and have been obtained from [32]; they verify
−1 −T
that JL (r) = JT
R (r), and JL (r) = JR (r).

− gExp(r)∗ (v)
It is also worth noting the special importance of the J+ r Jacobian present at the bottom of table 4.7,
which represents the derivative of a rotated vector with respect to perturbations in the Euclidean tangent space
(not on the curved manifold) that generates the rotation, as it enables tangent space optimization by calculus
methods designed exclusively for Euclidean spaces.

Jacobian Table 3.1 Expression Size

−1
J⊖
⊕R
R = −AdR = −R ∈ R3x3

J⊟ R−1 = −Ad−1 = −RT ∈ R3x3

⊞R R

J⊖
⊕R
R◦S = Ad−1
S = RT
S ∈ R3x3

J⊟ R◦S = I = I3x3 ∈ R3x3

⊞R
J⊖
⊕S
R◦S = I = I3x3 ∈ R3x3

J⊟ R◦S = AdR = RR ∈ R3x3

⊞S
⊖ Exp (r) 1 − cos krk krk − sin krk 2
J+ r = JR (r) = I3 − 2
b
r+ b
r ∈ R3x3
krk
 krk3 
b
r 1 1 + cos krk
J−1
R (r) = I3 + + 2
− r2
b ∈ R3x3
2 krk 2 krk sin krk
⊟ Exp (r) 1 − cos krk krk − sin krk 2
J+ r = JL (r) = I3 + 2
b
r+ b
r ∈ R3x3
krk
 krk3 
b
r 1 1 + cos krk
J−1
L (r) = I3 − + − r2
b ∈ R3x3
2 krk2 2 krk sin krk
− Log (R)

J = J−1
R Log (R) ∈ R3x3
⊕R
− Log (R)

J⊞ R = JL−1 Log (R) ∈ R3x3

J⊖
⊕R
R⊕r = Ad−1
Exp(r) = RT (r) ∈ R3x3

J⊟ r⊞R = AdExp(r) = R (r) ∈ R3x3

⊞R
J⊖
+r
R⊕r = JR (r) ∈ R3x3

J⊟ r⊞R = JL (r) ∈ R3x3

+r
J−
⊕R
S ⊖R = −J−1
L (S ⊖ R) ∈ R3x3

J−
⊞R
S ⊟R = −J−1
R (S ⊟ R) ∈ R3x3

J−
⊕S
S ⊖R = −1
JR (S ⊖ R) ∈ R3x3

J−
⊞S
S ⊟R = JL−1 (S ⊟ R) ∈ R3x3
− gR∗ (v)
J = b
−R v ∈ R3x3
⊕R
− gR∗ (v) ∧
J⊞ R = − (R v) ∈ R3x3
− g (v)
J+ vR∗ = R ∈ R3x3
− g−1 (v)  ∧
J⊕ RR∗ = RT v ∈ R3x3
−1
− gR∗ (v)
J = RT v ∈ R3x3
⊞R
− g−1 (v)
J+ vR∗ = RT ∈ R3x3

48
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Jacobian Table 3.1 Expression Size

− Ad (ω)
J⊕ R R = b
−R ω ∈ R3x3
− Ad (ω) ∧
J⊞ R R = − (R ω) ∈ R3x3
− Ad (ω)
J+ ω R = AdR = R ∈ R3x3
− Ad−1  ∧
J R (ω) = RT ω ∈ R3x3
⊕R
− Ad−1 (ω)
J⊞ R R = RT ω ∈ R3x3
− Ad−1 (ω)
J+ ω R = Ad−1
R = RT ∈ R3x3
− gExp(r)∗ (v)
∧
J+ r = b JR (r) = − R (r) v
−R (r) v JL (r) ∈ R3x3
−1
− gExp(r)∗ (v)
∧
J+ r = −RT (r) v
b JL (r) = RT (r) v JR (r) ∈ R3x3

Table 4.7: Rotational motion Jacobians

4.13 Rotational Motion Discrete Integration

The discrete integration with time of an element of a Lie group based on its Lie algebra is discussed in detail in
section 3.5, which includes expressions for the Euler, Heun and Runge-Kutta methods. In the case of rotational
motion, the state vector includes the rotation element R ∈ SO (3) and its angular velocity ω ∈ R3 contained
in the tangent space, viewed either in the local (ω BNB ) or global (ω NNB ) frames. The Euler method expressions
equivalent to (3.42) and (3.44) are shown below. Expressions for other integration schemes can easily be derived
from those in section 3.5:

Rk+1 ≈ Rk ⊕ [∆t ω BNBk ] = Rk ◦ Exp (∆t ωBNBk ) (4.86)

Rk+1 ≈ [∆t ω NNBk ] ⊞ Rk = Exp (∆t ω NNBk ) ◦ Rk (4.87)

4.14 Rotational Motion Gauss-Newton Optimization

The minimization by means of the Gauss-Newton iterative method of the Euclidean norm of a nonlinear function
whose input is a Lie group element is presented in section 3.6. In the case of rotational motion, the resulting
expressions for perturbations ∆rNNB ∈ so (3) to an input rotation R ∈ SO (3) viewed in the global frame FN are
shown in (4.88) and (4.89), which are equivalent to the generic (3.63) and (3.66). Refer to section 3.6 for the
meaning of the function Jacobian J and to section 4.12 for that of the left Jacobian JL .

Rk+1 ←− ∆rNNBk ⊞ Rk = ∆rNNBk ◦ Exp (rNBk ) (4.88)

h i−1
∆rNNBk = − J−T
Lk J T
k Jk J−1
Lk J−T T
Lk Jk E k (4.89)

If the perturbation is viewed in the local frame FB , (3.67) and (3.68) are customized as follows, making use of
the right Jacobian JR defined in section 4.12:

Rk+1 ←− Rk ⊕ ∆rBNBk = Exp (rNBk ) ◦ ∆rBNBk (4.90)

h i−1
∆rBNBk = − J−T J T
J
Rk k k RkJ−1
J−T T
Rk Jk E k (4.91)

4.15 Rotational Motion State Estimation

The adaptation of the EKF state estimation introduced in section 2.5 to the case in which Lie group elements and
their velocities are present is discussed in detail in section 3.7. For rotational motion with local perturbations, it

49
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

is necessary to replace X ∈ G by R ∈ SO (3), ∆τ X ∈ TX G by ∆rB ∈ so (3), vX ∈ Rm by ω B ∈ R3 , CX XX ∈ R

mxm

by CBRR ∈ R3x3 , and J⊖ ⊕X

X ⊕ τ by J⊖ R ⊕ r . The particularizations for global perturbations are similar.
⊕R

4.16 Applications of the Various Rotation Representations

This chapter discusses five different representations of the rotation or special orthogonal group SO (3): the rotation
matrix, the rotation vector, the unit quaternion, the half rotation vector, and the Euler angles. Although in theory
all of them can be employed for each of the purposes described in this chapter, and the required expressions
derived, each parameterization has its own advantages and disadvantages, being suited for certain purposes but
not recommended for others.

• The rotation matrix R is the most natural parameterization, possesses an easy to obtain inverse, and linear
expressions for composition and rotation. It provides a clear connection with the tangent space, together
with the exponential and logarithmic maps, SLERP, and plus and minus operators, which are not complex.
Its main drawbacks are the storage costs associated with its high dimension (9) and the expense involved
in maintaining orthogonality if allowed to deviate from the manifold [21, 33]. Its high cost precludes its
use to track the rotation over its manifold, although most implementations continuously compute it if the
adjoint matrix or the Jacobian blocks are required.

• The unit quaternion q is the preferred representation to track the rotation over its manifold, even if it is
necessary to obtain the rotation matrix for the adjoint and Jacobian blocks. Its advantages with respect
to the rotation matrix are its small dimension (4) and ease to maintain unitary if allowed to deviate from
the manifold. Unit quaternions are the least natural of the rotation parameterizations, being necessary to
convert to a different SO (3) representation for visualization [21]. While the inverse and concatenation are
linear, the rotation action is bilinear, which presents a disadvantage with respect to the rotation matrix.
Unit quaternion expressions are slightly more complex than those of the rotation matrix, and present a
slightly less obvious connection with the tangent space because in fact they represent a double covering of
SO (3) instead of SO (3) itself.

• The main advantage of the rotation vector r is that it belongs to the so (3) tangent space while simultane-
ously being an SO (3) parameterization. It is hence indicated for those uses related with incremental rotation
changes by means of the exponential map together with the plus and minus operators (periodically adding
the perturbations to the unit quaternion tracking the rotation), such as discrete integration, optimization,
and state estimation. The rotation vector norm is the most adequate metric for evaluating the rotated dis-
tance (or estimation error) between two rigid bodies. Although it benefits from its straightforward inverse,
its geometric appeal, and its small dimension (3+1)9 , its usage for other applications is discouraged by its
complex nonlinear kinematics, rotation action, and composition [21], which are not shown in this chapter.

• The half rotation vector h is so similar (half) to the rotation vector that its usage is not recommended in
order to avoid confusion. Its only real application as the tangent space of the unit quaternion is in practice
solved by dividing the rotation vector by two when necessary.

• The Euler angles φ have a long history and a clear physical meaning, which makes them the best choice
for attitude visualization, and constitute the only representation in which its dimension (3) coincides with
that of the manifold. However, they are not recommended for any other usage because of the presence of
discontinuities, together with complex and nonlinear expressions for inversion, composition, and rotation
action [21].

9
Strictly speaking the dimension is 3, although any usage requires computing the norm, which is often stored to accelerate the
transformations.

50
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Chapter 5

Motion of Rigid Bodies

This chapter can be considered as a continuation of the analysis of the rotational motion of rigid bodies contained
in chapter 4, in which its center of rotation OCR is not stationary but moves in the Euclidean space E3 . It follows
a similar scheme, relying on Lie theory concepts discussed in sections 3.3 and 3.4. Table 5.1 provides a comparison
between the generic nomenclature employed in chapter 3 and their rigid body motion equivalents. The different
representations discussed in this chapter are summarized in Table 5.2.
Concept Lie Theory Motion Concept Lie Theory Motion
Lie group G SE (3) Lie group element X, Y M, N
Concatenation ◦ ◦ Lie algebra m se (3)
Identity E IM Inverse X −1 M−1
Velocity v ξ Tangent element τ τ
Local frame X B Global frame E E
Point action gX () gM (p) Vector action gX () gM∗ (v)
Adjoint AdX (τ ∧ ) AdM (τ ∧ ) Adjoint matrix AdX τ AdM τ

Table 5.1: Comparison between generic Lie elements and those of rigid body motions

This section begins with an introduction to rigid body motion in section 5.1, followed by a description of the
different rigid body motion Lie group representations: the affine representation (section 5.2), the homogeneous
matrix (section 5.3), the transform vector (section 5.4), the unit dual quaternion (section 5.5), the half transform
vector (section 5.6), and the screw (section 5.7). Algebraic operations on rigid body motions are introduced in
section 5.8, such as powers, linear interpolation, and the plus and minus operators. Section 5.9 presents the
motion time derivative that leads to the definition of the twist or motion velocity in the tangent space. The
velocity of the rigid body points is discussed in section 5.10, followed by the adjoint map in section 5.11, which
transforms elements of the tangent space while the motion progresses on its manifold, and by an analysis of
uncertainty and covariances applied to rigid body motion (section 5.12). An extensive analysis of the rigid body
motion Jacobians is presented in section 5.13. Sections 5.14, 5.15, and 5.16 apply the discrete integration of Lie
groups, the Gauss-Newton optimization of Lie group functions, and the state estimation of Lie groups contained
in sections 3.5, 3.6, and 3.7 to the case of rigid body motions. Finally, the advantages and disadvantages of each
motion representation are discussed in section 5.17.

5.1 Special Euclidean (Lie) Group

A rigid body can be represented with a Cartesian frame attached to any of its points (the origin), with the basis
vectors e1 , e2 , and e3 being simply unit vectors along the main axes. Rigid body motions can be combined and
reversed, complying with the algebraic concept of group, but are not endowed with a metric, so they are not part
of a metric or Euclidean space (section 3.1). They do however comply with the axioms of a Lie group (section

51
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

3.3), and hence the set of rigid body motions together with the operation of motion concatenation comprises
hSE (3) , ◦i, known as the special Euclidean group of R3 [26], where its elements are denoted by M, the identify
motion by IM , and the inverse by M−1 . The group has two main actions, which are the motion of points
 
g() : SE3 × R3 → R3 | p → gM (p) and that of vectors g∗ () : SE3 × R3 → R3 | v → gM∗ (v) .

Representation Symbol Structure Space

Affine representation (R, T) SO (3) & free 3-vector SE (3)
Homogeneous matrix M 4x4 matrix (5.9) SE (3)
Twist ξ∧ 4x4 matrix (5.15) se (3)
ξ = [ν ω]T free 6-vector
Transform vector τ∧ 4x4 matrix (5.15) SE (3) & se (3)
τ = ξ t = [s r]T = [k ρ n φ]T free 6-vector
Unit dual quaternion ζ unit dual quaternion SE (3)
∧
Half twist Υ pure dual quaternion se (3)
Υ = ξ/2 free 6-vector
Half transform vector Ψ∧ pure dual quaternion SE (3) & se (3)
Ψ = Υ t = ξ t/2 = τ /2 free 6-vector
Screw S∧ = (φ⋄ /2, nm⋄ ) dual number & dual vector SE (3) & se (3)
T
S = [n m h φ] 8-vector

Table 5.2: Summary of rigid body motion representations

The movements of rigid bodies are introduced in section 4.1 as orthogonal transformations, this is, those that
preserve orthogonality and handedness.

• Norm: kgM∗ (v) k = kvk, ∀ v ∈ R3

• Cross product: gM∗ (u) × gM∗ (v) = gM∗ (u × v) , ∀ u, v ∈ R3

It is also worth noting the relationship between the motions of vectors and points:

gM∗ (v) = gM∗ (q − p) = gM (q) − gM (p) (5.1)

The SE (3) analysis below adopts the convention introduced in section 3.3, in which all actions, in-
cluding concatenation {◦ : SE (3) × SE (3) → SE (3)}, transform elements viewed in the local or body
frame FB = {OB , b1 , b2 , b3 } into elements viewed in the global or spatial frame FE = {OE , e1 , e2 , e3 }
= {gM (OB ) , gM∗ (b1 ) , gM∗ (b2 ) , gM∗ (b3 )}1 , which overlap each other before the motion takes place.

5.2 Affine Representation

The motion of a rigid body can always be divided into a rotation plus a translation, in which the point motion
action responds to:

gM (p) = gR (p) + T (5.2)

where gR is the point rotation action discussed in chapter 4, the point p is viewed in the local or body frame, and
T represents the vector going from the origin of the global or spatial frame to that of the body frame, viewed in
the global frame. Any SO (3) representation can be employed for the above expression, but the rotation matrix
1
In contrast with the case of rotational motion described in chapter 4, the spatial frame is now named E as it usually corresponds
to the ECEF frame. The NED case does not apply to this case as it shares origin with the body frame. The Earth Centered Earth
Fixed or ECEF frame is centered at the Earth center of mass, with iE 3 pointing towards the geodetic North along the Earth rotation
axis, iE E E E
1 contained in both the Equator and zero longitude planes, and i2 orthogonal to i1 and i3 forming a right handed system.

52
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

(section 4.2) and the unit quaternion (section 4.4) are the most common:

pE = REB pB + TEEB (5.3)

E B ∗ E
p = qEB ⊗ p ⊗ qEB + T EB (5.4)


The set of all possible rigid body motions SE (3) = M = (R, T) | R ∈ SO (3) , T ∈ R3 , coupled with the
motion concatenation defined below, is a valid representation of the special Euclidean group. The inverse motion,
as well as the concatenation operation, get slightly more complex because of the affine nature of the point
action:
−1 −1


(R, T) = R−1 , −gR∗ (T) (5.5)


(REB , TEEB ) = (REN , TEEN ) ◦ (RNB , TNNB ) = REN ◦ RNB , gREN ∗ (TNNB ) + TEEN (5.6)

Concept SE (3) Affine Concept SE (3) Affine

Lie group element M (R, T) Concatenation ◦ ◦
−1 −1
Identity IM (IR , 0) Inverse M (R, T)
Point motion gM (p) gR (p) + T Vector motion gM∗ (v) gR∗ (v)

Table 5.3: Comparison between generic SE (3) and motion affine representation

As indicated in (5.1), the effect of a rigid body motion on a vector requires a different map than that of points
because the translational part has the same influence on both the vector initial and final points:

gM∗ (v) = gM (q) − gM (p) = (gR (q) + T) − (gR (p) + T) = gR (q − p) = gR∗ (v) 6= gM (v) (5.7)

5.3 Homogeneous Matrix

Homogeneous coordinates are introduced with the objective of replacing the affine transformation (5.3) represent-
T
ing the rigid body motion with a linear transformation. Given a point p = [p1 p2 p3 ] ∈ R3 , its homogeneous
T T
representation is obtained adding a “1” as a fourth coordinate, so that p̄ = [p 1] = [p1 p2 p3 1] ∈ R4 . In the
T T
case of a vector v = q − p ∈ R3 , its homogeneous coordinates are v̄ = [v 0] = q̄ − p̄ = [v1 v2 v3 0] ∈ R4 .
The affine coordinate transformation (5.3) can then be converted into a linear transformation:
" # " # " #
gM (p) R T p
gM (p̄) = = = M p̄ (5.8)
1 0T
3 1 1

where M ∈ R4x4 is the homogeneous representation of (R, T) ∈ SE (3).

Concept SE (3) Homogeneous Concept SE (3) Homogeneous

Lie group element M M Concatenation ◦ Matrix product
Identity IM I4 Inverse M−1 M−1
Point motion gM (p) M p̄ Vector motion gM∗ (v) M v̄

Table 5.4: Comparison between generic SE (3) and homogeneous matrix

This enables a natural matrix representation of the special Euclidean group [26]:
( " # )
R T 3x3 3
SE (3) = M= T R∈R , T∈R ⊂ R4x4 (5.9)
03 1

which has group structure under matrix multiplication {R4x4 × R4x4 → R4x4 | Ma Mb ∈ R4x4 , ∀ Ma , Mb ∈ R4x4 }
[19]. While having dimension sixteen, the special Euclidean group SE (3) defined by means of homogeneous
matrices constitutes a six dimensional manifold to Euclidean space E6 . Note that in this group the identity

53
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

element is given by the identity matrix (I = I4 ).

The inversion and concatenation of transformations are linear when using homogeneous coordinates [26]:
" #−1 " #
−1 R T RT −RT T
M = = (5.10)
0T
3 1 0T3 1
" # " #" # " #
REB TEEB REN TEEN RNB TNNB REN RNB REN TNNB + TEEN
MEB = = MEN MNB = = (5.11)
0T
3 1 0T
3 1 0T
3 1 0T3 1

An advantage of the homogeneous representation is that the motion actions on points and vectors share the same
expression:

gM∗ (v̄) = gM (q̄) − gM (p̄) = M q̄ − M p̄ = M (q̄ − p̄) = M v̄ = gM (v̄) (5.12)

5.4 Transform Vector as Tangent Space

As discussed in section 3.3.1, the structure of the Lie algebra associated to SE (3) can be obtained by time
derivating the Lie group inverse constraint, M−1 (t) M (t) = M (t) M−1 (t) = I4 , resulting in the following par-
ticularizations of (3.3) and (3.4):
−1
ξE∧
EB
= ṀEB M−1
EB
= −MEB ṀEB (5.13)
B∧ −1 −1
ξ EB
= MEB ṀEB = −ṀEB MEB (5.14)

The Lie algebra velocity v∧ of SE (3) is known as the twist ξ ∧ , and has the following structure, derived from
(5.13) and (5.14):
" #
∧ b (t)
ω ν (t)
ξ (t) = ⊂ R4x4 (5.15)
0T
3 0

The twist ξ represents the motion velocity and is composed by the angular velocity ω defined in section 4.8 and
the linear velocity ν, defined in section 5.9. Inverting (5.13) and (5.14) results in the homogeneous matrix time
derivative, which is linear:

ṀEB = ξE∧
EB
MEB = MEB ξ B∧
EB
(5.16)

Notice that if M (t0 ) = I4 , then Ṁ (t0 ) = ξ ∧ (t0 ), and hence the twist matrix ξ∧ (t0 ) provides a first order
approximation of the homogeneous matrix around the identity matrix I4 :

M (t0 + ∆t) ≈ I4 + ξ ∧ (t0 ) ∆t (5.17)

 " # 
∧ b ν
ω 4x4 3
The space of matrices with the (5.15) structure, se (3) = ξ = T ⊂R b ∈ so (3) , ν ∈ R is hence
|ω
03 0
the tangent space of SE (3) at the identity I4 [26], denoted as TI4 M. With the twist Cartesian coordinates de-
 n
∨ o
T
fined as ξ = [ν ω] ∈ R6 , the hat ·∧ : R6 → se (3) | ξ → ξ ∧ and vee ·∨ : se (3) → R6 | ξ∧ → ξ operators
convert the Cartesian vector form of the twist into its matrix form, and vice versa.

If M (t0 ) 6= I4 , the tangent space needs to be transported right multiplying by MEB (t0 ) (in the case of space
twist), or left multiplying for the local based twist:

MEB (t0 + ∆t) ≈ MEB (t0 ) + [ξ E∧

EB
(t0 ) ∆t] MEB (t0 ) = [I4 + ξE∧
EB
(t0 ) ∆t] MEB (t0 ) (5.18)
MEB (t0 + ∆t) ≈ MEB (t0 ) + MEB (t0 ) [ξB∧ B∧
EB (t0 ) ∆t] = MEB (t0 ) [I4 + ξ EB (t0 ) ∆t] (5.19)

54
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Note that the solution to the ordinary differential equation ẋ (t) = ξ ∧ x (t) , x (t) ∈ R6 , where ξ ∧ is constant, is
∧
x (t) = eξ t x (0). Based on it, assuming M (0) = I4 as initial condition, and considering for the time being that
ξ ∧ is constant,

2
n
ξ ∧
t ∧ ξ∧ t ξ∧ t
M (t) = e = I4 + ξ t + + ···+ + ... (5.20)
2! n!

It can be proven by means of (4.15) and the matrix exponential properties that (5.20) is indeed an homogeneous
matrix that hence represents the special Euclidean SE (3) transformations.

Concept Lie Theory SE (3)

" #
∧ ∧ b
r s
Tangent space element τ τ = T
" 03 0#
b
ω ν
Velocity element v∧ ξ∧ = T
03 0
Structure ∧ (5.15)

Table 5.5: Comparison between generic SE (3) and transform vector as tangent space

Remembering that so far ξ ∧ is constant, which is equivalent to both ω b and ν within (5.15) also being constant,
∧
(5.20) means that any rigid body motion M (t) = e ξ t can be realized by maintaining a constant twist ξ ∧ for
a given time t [26]. The vectors n = ω t/kω tk = r/krk and k = ν t/kν tk = s/ksk indicate the twist directions,
while φ = krk and ρ = ksk represent the twist magnitudes, respectively. This enables the definition of the
transform vector τ , also known as the exponential coordinates of the M motion, as

T T T
τ = ξ t = [ν t ω t] = [s r] = [k ρ n φ] ∈ R6 (5.21)

Note that the transform vector τ belongs to the tangent space as it is a multiple of the twist ξ ∈ se (3), and hence
tends to coincide with it as time tends to zero. The exponential map {exp () : se (3) → SE (3) | M = exp (τ ∧ )}

and its capitalized form Exp () : R6 → SE (3) | M = Exp (τ ) wrap the transform vector around the special
Euclidean group. However, the twist ξ ∧ (t) in fact is not required to be constant. Given a rigid body motion
represented by its homogeneous matrix M ∈ SE (3), it can be proven that there exists a not necessarily unique
transform vector τ = [s r] = [k ρ n φ] such that M = eτ [26, 31]. The exponential map has the following
T T ∧

form [26]:
 
r s + r rT s
r)] b
[I3 − exp (b
exp (b
r) 
M (τ ) = exp (τ ∧ ) =  krk2  r 6= 0 (5.22)
0 1
" #
∧ I3 s
M (τ ) = exp (τ ) = r=0 (5.23)
0 1

The exponential map described above is thus surjective but not injective, as in general there are infinitely many
solutions to the map. The logarithmic map {log () : SE (3) → se (3) | τ ∧ = log (M)} and its capitalized version

Log () : SE (3) → R6 | τ = Log (M) hence convert rigid body motions into transform vectors [26]. The rotation
vector r is provided by (4.16); if r = 0, s coincides with T, while otherwise it is obtained by solving for s from
the following linear system [26], taken from (5.22):
h i
(I3 − er ) b
b
r + r rT s = krk2 T (5.24)

Unlike the case of the rotation vector (4.17), the transform vector inverse coincides with its negative only if the
motion is very small. The different SE (3) actions (concatenation, point motion, vector motion), the inverse,
and the relationship between the transform vector derivative with time and the twist, are complex and rarely

55
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

used.

5.5 Unit Dual Quaternion

The dual quaternions with unity norm, known as unit dual quaternions, comprise an additional representation of
the special Euclidean group SE (3), as shown below. Dual quaternions in turn are generalizations of quaternions
in the same way as dual numbers are generalization or real ones [28, 34, 35]. For this reason, it is necessary to
first describe the dual numbers and dual vectors in sections 5.5.1 and 5.5.2 before discussing the dual quaternions
in section 5.5.3 and finally the unit dual quaternions in section 5.5.4.

5.5.1 Dual Numbers


The set of dual numbers D is defined as D = R + R ǫ | ǫ2 = ǫ · ǫ = 0 . Given two dual numbers
d⋄1
= x1 + y1 ǫ ∈ D, d⋄2
= x2 + y2 ǫ ∈ D, ∀ x1 , y1 , x2 , y2 ∈ R, it is possible to define the operations of addition
{+ : D × D → D} and multiplication {· : D × D → D} [28]:

d⋄1 + d⋄2 = (x1 + y1 ǫ) + (x2 + y2 ǫ) = (x1 + x2 ) + (y1 + y2 ) ǫ (5.25)

d⋄1 · d⋄2 = (x1 + y1 ǫ) · (x2 + y2 ǫ) = (x1 x2 ) + (x1 y2 + y1 x2 ) ǫ (5.26)

The set of dual numbers D endowed with the operations of addition + and multiplication · forms a ring, known as
the ring of dual numbers hD, +, ·i, nearly always abbreviated to simply D. The additive identity is 0⋄ = 0 + 0 ǫ and
the inverse −d⋄ = −x − y ǫ, while the multiplication identity is 1⋄ = 1 + 0 ǫ and the inverse d⋄−1 = 1/x − y ǫ/x2 .
Note that D is a ring instead of a field as the multiplicative inverse d⋄−1 is not defined when x = 0. The conjugate
of a dual number is obtained by switching the sign of its dual part (d∗ = x − y ǫ ∈ D).

The most useful property of dual numbers is the explicit relationship that exists between the value of any function
evaluated at a dual number f (d⋄ ) = f (x + y ǫ) and its value when evaluated exclusively at its real part f (x) [34].
The Taylor expansion of f (x + y ǫ) around x reads:

∂f 1 ∂2f 2 1 ∂3f 3
f (d⋄ ) = f (x + y ǫ) = f (x) + (x) (d⋄
−x) + (x) (d⋄
−x) + (x) (d⋄ −x) + · · · (5.27)
∂d⋄ 2! ∂d⋄2 3! ∂d⋄3

n
As (d⋄ − x) = yn ǫn is zero when n > 1, this translates into:

∂f
f (d⋄ ) = f (x + y ǫ) = f (x) + (x) y ǫ (5.28)
∂d⋄

5.5.2 Dual Vectors

T
Dual vectors in three dimensions are formed by grouping three dual numbers {d⋄ = [d⋄1 d⋄2 d⋄3 ] ∈ D3 ,
∀ d⋄1 , d⋄2 , d⋄3 ∈ D}. It is then possible to define, ∀ d⋄ ∈ D, d⋄ , e⋄ ∈ D3 , the scalar multiplication of a double num-
 
ber by a double vector · : D × D3 → D3 , the inner product between two double vectors h· , ·i : D3 × D3 → D ,

and the cross product between two double vectors × : D3 × D3 → D3 . The results are similar to those of real
numbers shown in section 3.1 [28]:
T
d⋄ · d⋄ = [d⋄ d⋄1 d⋄ d⋄2 d⋄ d⋄3 ] (5.29)
⋄ ⋄ ⋄ ⋄ ⋄T ⋄ T
hd , e i = d ·e = d e = [d⋄1 e⋄1 d⋄2 e⋄2 d⋄3 e⋄3 ] (5.30)
 ⋄
  ⋄  ⋄ ⋄ 
0 −d⋄3 +d⋄2 e1 d2 e3 − d⋄3 e⋄2
⋄ c⋄ ⋄  ⋄ ⋄  ⋄  ⋄ ⋄ 
d ×e ⋄
= d e = +d3 0 ⋄
−d1  e2  = d3 e1 − d⋄1 e⋄3  = −e⋄ × d⋄ = −eb⋄ d⋄ (5.31)
−d⋄2 +d1 ⋄
0⋄ e⋄3 d⋄1 e⋄2 − d⋄2 e⋄1

56
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

5.5.3 Dual Quaternions

The set of dual quaternions Hd is defined as {H = H + H ǫ | ǫ2 = −1}. A dual quaternion ζ ∈ Hd has the form
ζ = qr + qd ǫ, with qr , qd ∈ H. The real plus dual notation {1, ǫ} is not always the most convenient. A dual
quaternion can also be expressed as the sum of a dual number plus a dual vector in the form ζ = d⋄0 + d⋄v , where d⋄0
is the scalar part and d⋄v = d⋄1 i + d⋄2 j + d⋄3 i j is the vector part. Dual quaternions are however mostly represented
T T T
as 8-vectors ζ = [qr qd ] = [q0r qvr q0d qvd ] = [q0r q1r q2r q3r q0d q1d q2d q3d ] , which enables the use
of matrix algebra for quaternion operations [35]. It is also convenient to abuse the equal operator as required to
combine general, real, and pure dual quaternions.

The dual quaternion addition {+ : Hd × Hd → Hd } and the scalar product {· : D × Hd → Hd } are both straight-
forward and commutative:
" #
qra + qrb
ζa + ζb = (qra + qda ǫ) + (qrb + qdb ǫ) = (qra + qrb ) + (qda + qdb ) ǫ =
qda + qdb
= (d⋄0a + d⋄va ) + (d⋄0b + d⋄vb ) = (d⋄0a + d⋄0b ) + (d⋄va + d⋄vb ) (5.32)
⋄
d ·ζ = (x + y ǫ) · (qr + qd ǫ) = x · qr + (x · qd + y · qr ) ǫ (5.33)

The multiplication of dual quaternions {⊗ : Hd × Hd → Hd } is not commutative as it includes the dual vector
cross product (5.31). Depending on how it is expressed, the similarities with the multiplication of dual numbers
(5.26) or that of quaternions (4.23) are obvious [28, 35, 36]:

ζa ⊗ ζb = (qra + qda ǫ) ⊗ (qrb + qdb ǫ) = (qra ⊗ qrb ) + (qra ⊗ qdb + qda ⊗ qrb ) ǫ
" #
qra ⊗ qrb
= = (d⋄0a + d⋄va ) ⊗ (d⋄0b + d⋄vb )
qra ⊗ qdb + qda ⊗ qrb
   
=
T
d⋄0a d⋄0b − d⋄va d⋄vb + d⋄0a d⋄vb + d⋄0b d⋄va + db ⋄ d⋄ (5.34)
va vb

Dual quaternion multiplication is also bilinear [35], based on the operators defined in (4.24):
" #" # " #" #
[ar ]L O4x4 br [br ]R O4x4 ar
ζ a ⊗ ζ b = [ζ a ]L ζ b = = [ζ b ]R ζ a = (5.35)
[ad ]L [ar ]L bd [bd ]R [br ]R ad

It is possible to define three different conjugates for a dual quaternion [28], based on whether it only switches
the sign of the dual part as in the case of dual numbers (5.36), it employs the conjugates of the real and dual
quaternion components (5.37), or a combination of both (5.38):
◦
ζ ◦ = qr − qd ǫ → (ζ a ⊗ ζ b ) = ζ ◦a ⊗ ζ ◦b (5.36)
∗
ζ ∗ = q∗r + q∗d ǫ → (ζ a ⊗ ζ b ) = ζ ∗b ⊗ ζ ∗a (5.37)
• •
ζ = q∗r − q∗d ǫ → (ζ a ⊗ ζ b ) = ζ •b ⊗ ζ •a (5.38)

Pure dual quaternions ζ = 0⋄ + d⋄v ∈ Hdp are those in which its dual number is zero (0⋄ ), or in which
∗
both its real and dual parts are pure quaternions
p q (qr , qd ∈ Hp ), and verify that ζ = − ζ . The dual
quaternion norm is defined as kζk = ζ ⊗ ζ ∗ = qr ⊗ q∗r + (qr ⊗ q∗d + qd ⊗ q∗r ) ǫ ∈ D [36]. Dual quater-
nions endowed with ⊗ do not form a group, because although ζ1 = q1 + 0 ǫ is the identity, the inverse


ζ −1 = qr −1 − qr −1 ⊗ qd ⊗ qr −1 ǫ = qr −1 ⊗ q1 − qd ⊗ qr −1 ǫ is not defined when qr = 0. Dual quaternions en-
dowed with addition + and multiplication ⊗ however do form the non-abelian ring hHd , +, ⊗i.

As in the case of quaternions described in section 4.4.2, the natural power of a dual quaternion ζ n , n ∈ N is
obtained by multiplying the dual quaternion by itself n − 1 times. The double product of a dual quaternion by a
vector {Hd × R3 → R3 } is defined as the product ⊗ of the dual quaternion by the vector by the dual quaternion
conjugate, resulting in three different versions based on the conjugate definition (5.36, 5.37, 5.38).

57
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

5.5.4 Unit Dual Quaternion

Unit dual quaternions are those dual quaternions in which ζ ⊗ ζ ∗ = ζ ∗ ⊗ ζ = ζ1 , which implies that the inverse
and the conjugate coincide as ζ −1 = q∗r − q∗r ⊗ qd ⊗ q∗r ǫ = q∗r + q∗d ǫ = ζ ∗ . Note that the norm kζk has a unity
real part and a zero dual part [36]. Based on (5.34), this translates into the following two conditions:

qr ⊗ q∗r = 1 → kqr k = 1 (5.39)

qr ⊗ q∗d + qd ⊗ q∗r = 0 → hqr , qd i = 0 (5.40)

In other words, unit dual quaternions are those in which the real part qr is a unit quaternion that is also
orthogonal to the dual part qd .

The rigid body motion between a body frame FB and a spatial frame FE represented by the unit quaternion
qEB and the translation TEEB (section 5.2) can always be represented by the following unit dual quaternion ζ EB
T
[28, 35], where the notation is abused to consider the quaternion TEEB = [0 TEEB ] :

ǫ E
ζ EB = qEB + T ⊗ qEB (5.41)
2 EB

(5.41) is indeed a unit dual quaternion as ζ EB ⊗ ζ ∗EB = ζ 1 = q1 = 1 based on TEEB∗ = −TEEB as it is a pure quater-
nion. The opposite map providing the affine representation based on the unit dual quaternion is the follow-
ing:

qEB = ζ EBr (5.42)

E ∗
T EB
= 2 ζ EBd ⊗ ζ EBr (5.43)

Concept SE (3) Hd Concept SE (3) Hd

Lie group element M ζ Concatenation ◦ ⊗
Identity IM ζ1 Inverse M −1
ζ∗
Point motion gM (p) ζ ⊗ ζp ⊗ ζ• Vector motion gM∗ (v) ζ ⊗ ζv ⊗ ζ•

Table 5.6: Comparison between generic SE (3) and unit dual quaternion

The unit dual quaternion endowed with the double product can be employed to transform both points and
vectors, verifying that it complies with the rigid body motion orthogonality and handedness conditions de-
T
scribed in section 4.1. Given a point p = [p1 p2 p3 ] ∈ R3 , its dual quaternion representation ζ p is obtained
by combining the unit quaternion q1 as the real part and the point coordinates as the dual part, result-
ing in ζ p = q1 + ǫ p ∈ R8 [28]. In the case of a vector v = q − p ∈ R3 , its dual quaternion representation is
ζ v = ζ q − ζ p = (q1 + ǫ q) − (q1 + ǫ p) = ǫ (q − p) = ǫ v ∈ R8 . It is then possible, based on (5.4) and (5.7), to
employ the double product to transform both points and vectors between different frames:
 ǫ   ǫ 
ζ pE = ζ EB ⊗ ζ pB ⊗ ζ •EB = qEB + TEEB ⊗ qEB ⊗ (q1 + ǫ pB ) ⊗ q∗EB + q∗EB ⊗ TEEB
2 2
= q1 + ǫ (qEB ⊗ pB ⊗ q∗EB + TEEB ) = q1 + ǫ pE (5.44)
•
ζ pE = ζ EB ⊗ ζ pB ⊗ ζ EB = (qr + qd ǫ) ⊗ (q1 + ǫ p ) ⊗ B
(q∗r − q∗d ǫ)
= q1 + ǫ (qr ⊗ pB ⊗ q∗r + qd ⊗ q∗r − qr ⊗ q∗d ) = q1 + ǫ pE (5.45)
 ǫ   ǫ 
•
ζ vE = ζ EB ⊗ ζ vB ⊗ ζ EB = qEB + TEEB ⊗ qEB ⊗ ǫ vB ⊗ q∗EB + q∗EB ⊗ TEEB
2 2
= ǫ (qEB ⊗ vB ⊗ q∗EB ) = ǫ vE (5.46)
•
ζ vE = ζ EB ⊗ ζ vB ⊗ ζ EB = (qr + qd ǫ) ⊗ ǫ v ⊗ B
(q∗r − q∗d ǫ) = ǫ (qr ⊗ v ⊗ B
q∗r ) = ǫv E
(5.47)

A disadvantage of the unit dual quaternion as an SE (3) representation is that a different expression is required
for the inverse transformation:
 ǫ ∗   ǫ 
ζ pB = ζ ∗EB ⊗ ζ pE ⊗ ζ ◦EB = q∗EB − qEB ⊗ TEEB ⊗ (q1 + ǫ pE ) ⊗ qEB − TEEB ⊗ qEB
2 2

58
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

= q1 + ǫ (q∗EB ⊗ pE ⊗ qEB − q∗EB ⊗ TEEB ⊗ qEB ) = q1 + ǫ pB (5.48)

ζ pB = ζ ∗EB ⊗ ζ pE ⊗ ζ ◦EB = (q∗r + q∗d ǫ) ⊗ (q1 + ǫ pE ) ⊗ (qr − qd ǫ)
= q1 + ǫ (q∗r ⊗ pE ⊗ qr + q∗d ⊗ qr − q∗r ⊗ qd ) = q1 + ǫ pB (5.49)
 ǫ   ǫ 
ζ vB = ζ ∗EB ⊗ ζ vE ⊗ ζ ◦EB = q∗EB − q∗EB ⊗ TEEB ⊗ ǫ vE ⊗ qEB − TEEB ⊗ qEB
2 2
= ǫ (q∗EB ⊗ vE ⊗ qEB ) = ǫ vB (5.50)
ζ vB = ζ ∗EB ⊗ ζ vE ⊗ ζ ◦EB = (q∗r + q∗d ǫ) ⊗ ǫ vE ⊗ (qr − qd ǫ) = ǫ (q∗r ⊗ vE ⊗ qr ) = ǫ vB (5.51)

The inverse transformation coincides with the dual quaternion conjugate provided by (5.37):
ǫ B ǫ
ζ BE = ζ −1
EB
= ζ ∗EB = qBE + T ⊗ qBE = q∗EB − (q∗EB ⊗ TEEB ⊗ qEB ) ⊗ q∗EB
2 BE 2
ǫ
= q∗EB − q∗EB ⊗ TEEB = q∗r + q∗d ǫ (5.52)
2

The concatenation of transformations is straightforward based on (5.6):

 ǫ   ǫ 
ζ EB = ζ EN ⊗ ζ NB = qEN + TEEN ⊗ qEN ⊗ qNB + TNNB ⊗ qNB
2 2
ǫ
= qEN ⊗ qNB + (qEN ⊗ TNB ⊗ qNB + TEEN ⊗ qEN ⊗ qNB )
N

2
ǫ ǫ
= qEB + (qEN ⊗ TNNB ⊗ q∗EN + TEEN ) ⊗ qEB = qEB + TEEB ⊗ qEB
2 2
= (qENr + qENd ǫ) ⊗ (qNBr + qNBd ǫ)
= (qENr ⊗ qNBr ) + (qENr ⊗ qNBd + qENd ⊗ qNBr ) ǫ (5.53)

Unit dual quaternions comply with the orthogonality and handedness conditions required in section 4.1 for
rigid body motions, and hence their space SE (3) = {ζ = qr + qd ǫ ∈ Hd |kqr k = 1, hqr , qd i = 0} possesses group
structure under dual quaternion multiplication {⊗ : Hd × Hd → Hd | ζ a ⊗ ζ b ∈ Hd , ∀ ζ a , ζ b ∈ Hd }. Because of
the (5.39) and (5.40) constraints, although they have dimension eight, the special Euclidean group SE (3) defined
by means of unit dual quaternions constitutes a six dimensional manifold to Euclidean space E6 called the
image space of spatial displacements, which can be visualized in R4 as follows. Expression (5.39) defines a unit
hypersphere of three dimensions, while (5.40) defines the three dimensional hyperplane orthogonal to the normal
at the point qr on the hypersphere. Thus, the image space consists of the hypersphere and all of its tangent
spaces, which have been translated to contain the origin. Note that in this group ζ1 constitutes the identity and
ζ ∗ the inverse.

The map from the affine representation (or homogeneous matrix) to the unit dual quaternion is surjective but not
injective for the same reasons as that between the rotation matrix and the unit quaternion described in section
4.4.3, this is, the double covering of the SO (3) by the unit quaternion.

5.6 Half Transform Vector as Tangent Space

As indicated in section 3.3.1, the structure of the Lie algebra se (3) can be obtained by time derivating its SE (3)

∗
Lie group constraint ζ ⊗ ζ ∗ = ζ ∗ ⊗ ζ = ζ1 , leading to ζ ∗ ⊗ ζ̇ = − ζ ∗ ⊗ ζ̇ , which indicates that ζ ∗ ⊗ ζ̇ is in fact
a pure dual quaternion, as is ζ̇ ⊗ ζ ∗ . This results in the following particularizations of (3.3) and (3.4):
∗
ΥE∧
EB
= ζ̇ EB ⊗ ζ ∗EB = −ζ EB ⊗ ζ̇ EB (5.54)
B∧ ∗ ∗
Υ EB = ζ EB ⊗ ζ̇ EB = −ζ̇ EB ⊗ ζ EB (5.55)

The Lie algebra velocity v∧ is known as the half twist Υ∧ , and as shown in (5.54) and (5.55), has the structure
of a pure dual quaternion because its negative coincides with its conjugate:

Υ∧ (t) = [0⋄ + Υ⋄v (t)] ∈ Hdp (5.56)

59
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Inverting the previous equations results in the unit dual quaternion time derivative, which is linear:

ζ̇ EB = ΥE∧
EB
⊗ ζ EB = ζ EB ⊗ ΥB∧
EB
(5.57)

Notice that if ζ (t0 ) = ζ1 , then ζ̇ (t0 ) = Υ (t0 ), and hence the pure dual quaternion Υ∧ (t0 ) provides a first order
approximation of the unit dual quaternion around the identity ζ1 :

ζ (t0 + ∆t) ≈ ζ 1 + Υ∧ (t0 ) ∆t (5.58)

Concept Lie Theory SE (3)

Tangent space element τ ∧
Ψ = [0⋄ + Ψ⋄v ]
∧

Velocity element v ∧
Υ∧ = [0⋄ + Υ⋄v ]
Structure ∧ pure dual quaternion

Table 5.7: Comparison between generic SE (3) and half transform vector as tangent space

The space of pure dual quaternions se (3) = {Υ∧ ∈ Hdp | Υ⋄ ∈ D3 } is hence the tangent space of the unit

dual quaternions at the identity ζ 1 , denoted as Tζ M. The hat ·∧ : R6 → D3 → se (3) | Υ → Υ∧ and vee
n
∨ o 1

·∨ : se (3) → D3 → R6 | Υ∧ → Υ operators convert the half twist vector into its pure dual quaternion
form, and vice versa.

If ζ (t0 ) 6= ζ 1 , the tangent space needs to be transported right multiplying by ζ EB (t0 ) (in the case of space tangent
space), or left multiplying for the local space:

ζ EB (t0 + ∆t) ≈ ζ EB (t0 ) + [ΥE∧ E∧

EB (t0 ) ∆t] ⊗ ζ EB (t0 ) = [ζ 1 + ΥEB (t0 ) ∆t] ⊗ ζ EB (t0 ) (5.59)
B∧ B∧
ζ EB (t0 + ∆t) ≈ ζ EB (t0 ) + ζ EB (t0 ) ⊗ [Υ EB
(t0 ) ∆t] = ζ EB (t0 ) ⊗ [ζ 1 + Υ
EB
(t0 ) ∆t] (5.60)

Note that the solution to the ordinary differential equation ẋ (t) = x (t) ⊗ Υ∧ , x (t) ∈ R8 , where Υ∧ is constant,
∧
is x (t) = x (0) eΥ t . Based on it, assuming ζ (0) = ζ1 as initial condition, and considering for the time being
that Υ is constant,

2
n
Υ∧
t ∧ Υ∧ t Υ∧ t
ζ (t) = e = ζ1 + Υ t + + ···+ + ... (5.61)
2! n!

which is indeed a pure dual quaternion and coincides with half the twist defined in section 5.4, as proven next
based on (4.47), (5.41), (5.54), (5.112), and TEEB being a pure quaternion:
h ǫ i h ǫ i
∗
ΥE∧
EB
= ζ̇ EB ⊗ q∗EB + (TEEB ⊗ qEB ) = ζ̇ EB ⊗ q∗EB − q∗EB ⊗ TEEB
 2  2 E∧  
ωE∧
EB ǫ E
E ωE∧
EB ωE∧
EB ω ǫ E ω E∧
= + ṪEB + TEB ⊗ − ⊗ TEB = EB +
E
ṪEB − EB ⊗ TEEB
2 2 2 2 2 2 2
E∧
1 ξ EB
= (ω E∧ E∧
EB + ǫ ν EB ) = (5.62)
2 2

A similar process employing (5.55) and (5.111) leads to:

1 ξ B∧
ΥB∧
EB = (ωB∧ B∧
EB + ǫ ν EB ) =
EB
(5.63)
2 2

∧
Remembering that so far Υ∧ is constant, (5.61) means that any rigid body motion ζ (t) = eΥ t can be realized
by maintaining a constant half twist Υ∧ ∈ Hdp for a given time t. The vectors n = ω t/kω tk = r/krk and
k = ν t/kν tk = s/ksk indicate the half twist directions, while θ = φ/2 = krk/2 and ρ/2 = ksk/2 represent the
half twist magnitudes, respectively. This enables the definition of the half transform vector Ψ, also known as the

60
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

exponential coordinates of the M motion, as

1 T 1 T 1 T τ
Ψ = Υt = [ν t ω t] = [s r] = [k ρ n φ] = ∈ R6 (5.64)
2 2 2 2

Note that the half transform vector Ψ belongs to the tangent space as it is a multiple of the half
twist Υ ∈ se (3), and hence tends to coincide with it as time tends to zero. The exponential map
 ∧

 6
exp () : se (3) → SE (3) | M = exp Ψ and its capitalized form Exp () : R → SE (3) | M = Exp (Ψ) wrap
the half transform vector around the special Euclidean group. However, the half twist Υ∧ (t) in fact is not re-
quired to be constant. Given a rigid body motion represented by its unit dual quaternion ζ ∈ SE (3), it can
be proven that there exists a not necessarily unique half transform vector Ψ = τ /2 = [s r]T /2 = [k ρ n φ]T /2
∧
such that ζ = eΨ . The exponential map is made up by a combination of (4.42), (5.22), (5.23), and (5.41).

The logarithmic map log () : SE (3) → se (3) | Ψ∧ = log (M) and its capitalized version
 6
Log () : SE (3) → R | Ψ = Log (M) convert unit dual quaternions into half transform vectors. It is
composed by (5.42), (4.43), and (4.44) for the rotation part, and (5.43) together with (5.24) for the translation
part.

As the vector Υ represents half the twist ξ, it is possible to adjust expressions (5.54), (5.55), and (5.57):

ξ E∧
EB
= 2 ζ̇ EB ⊗ ζ ∗EB (5.65)
ξ B∧
EB
= 2 ζ ∗EB ⊗ ζ̇ EB (5.66)
1 E∧ 1
ζ̇ EB = ξ EB ⊗ ζ EB = ζ EB ⊗ ξB∧
EB
(5.67)
2 2

5.7 Screw as Tangent Space

In the chapter 4 analysis of rotational motion there exists two different representations for the tangent space
so (3). The first is the skew-symmetric angular velocity ω ∧ = ω∧ , which converts into the rotation vector r∧
when applied during a certain amount of time (section 4.3), and represents the origin of the exponential map
exp (r∧ ) (4.15) that transforms it into the rotation matrix R (section 4.2). The second is the pure quaternion
T T
half angular velocity Ω∧ = [0 ω/2] , which converts into the half rotation vector h∧ = [0 r/2] (section 4.5),


and represents the origin of the exponential map exp h∧ = exp (r∧ /2) (4.42) that transforms it into the unit
quaternion q (section 4.4). Note that both representations of the tangent space so (3) are so similar that for all
practical purposes they are considered the same, resulting in two versions of the exponential map that convert
the rotation vector r into either the rotation matrix R or the unit quaternion q.

So far the rigid body motion looks similar. There are two se (3) velocities, the twist ξ∧ and the pure dual
quaternion half twist Υ∧ , which convert into the transform vector τ ∧ and the half transform vector Ψ∧ (sections
5.4 and 5.6) when applied during an amount of time, and constitute the origins of the exponential maps that
transform them into the homogeneous matrix M (section 5.3), the affine representation (section 5.2), or the unit
dual quaternion ζ (section 5.5). As in the rotation case, both se (3) representations are so similar that for all
practical purposes they are considered the same and can be interchanged in the various exponential maps.

There exists however an additional SE (3) parameterization, which also belongs to its tangent space se (3), that
enables the definition of a different exponential map into the unit dual quaternion that explicitly separates the
influence of the motion direction from that of its magnitude, and is also indispensable for the rigid body motion
powers, linear interpolation, and perturbations introduced in section 5.8.

The origin of the screw S lies in the fact that every rigid body motion can be realized by a rotation about an
axis combined with a translation parallel to that same axis [31]. The rotation and translation can be executed
simultaneously or one after another without modifying the result. It is however necessary to remark that in
this case the axis, defined in section 3.2, does not necessarily pass though the origin of the frame characterizing
the rigid body, in contrast to previous representations of rigid body motions in which the rotating axis, more

61
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

appropriately called rotating direction, in all cases passed through the origin. Note that according to section 3.2,
an axis is represented by (n, m) and has four degrees of freedom, while if restricted to passing through the origin
m = 0 and the degrees of freedom are two. The line point closest to the origin responds to p⊥ = n b m.
T
A screw {S = [n m h φ] ∈ R8 | n, m ∈ R3 , h, φ ∈ R} consists of an axis (n, m), a pitch h, and a magnitude
φ [31]. As all other SE (3) representations, it contains six degrees of freedom as it shares the line redundancies
described in section 3.2, this is, knk = 1 and nT m = 0. It represents a rotation by an amount φ about the axis
(n, m) combined by a translation by an amount d = h φ parallel to axis (n, m). If h = ∞, the corresponding
screw motion consists of a pure translation along the axis of the screw by a distance φ. Note that n and φ are
indeed the direction and magnitude of the rotation vector r = n φ defined by (4.14).

Given a rigid body motion represented by the combination of rotation and translation vectors (r = n φ, T), the
map converting it into a screw has two versions:

• r 6= 0. If the motion contains both rotation and translation components:

φ = krk = φ (5.68)
r
n = (5.69)
krk
 
1 b φ
m = b⊥ n =
p n T) × n
T n + cot (b (5.70)
2 2
d = TT n (5.71)
d
h = (5.72)
φ

• r = 0. If the motion is only a translation, the screw definition changes so it contains an ∞ pitch, a magnitude
equal to the translation amount, and an axis in the direction of T that passes through the origin. The
displacement definition does not change though.

h = ∞ (5.73)
φ = kTk (5.74)
n = T/φ (5.75)
m = 0 (5.76)
T
d = T n (5.77)

The opposite map, which provides the rotation and translation vectors from a screw, also has two versions:

• h 6= ∞. The screw contains both translation and rotation components:

T = b p⊥ − cos φ p⊥ + d n
p⊥ − sin φ n (5.78)
r = nφ (5.79)

• h = ∞. The screw does not rotate:

T = nφ (5.80)
r = 0 (5.81)

It is however the exponential map between the screw and the unit dual quaternion the one that provides a different
perspective to the motion of a rigid body. It is built based on the expressions for the axis moment (5.70) and the
unit dual quaternion (5.41), first as the sum of two quaternions (5.82) and next as that of a dual number plus a
dual vector (5.83):
   
φ φ d φ φ d φ
ζ = cos + sin n + − sin + sin m + cos n ǫ (5.82)
2 2 2 2 2 2 2
= qr + qd ǫ = [q0r qvr ]T + [q0d qvd ]T ǫ
62
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

     
φ d φ φ φ d φ
= cos − sin ǫ + sin n + sin m + cos n ǫ (5.83)
2 2 2 2 2 2 2

This last expression can be modified based the application of the dual number Taylor expansion (5.28) to the
sine and cosine:

cos d⋄ = cos (x + y ǫ) = cos x − y ǫ sin x (5.84)

⋄
sin d = sin (x + y ǫ) = sin x + y ǫ cos x (5.85)

This results in:

φ + dǫ φ + dǫ φ⋄ φ⋄
ζ = exp (S) = cos + (n + m ǫ) · sin = cos + nm⋄ · sin = cos θ⋄ + nm⋄ · sin θ⋄ (5.86)
2 2 2 2

where nm⋄ = n + m ǫ is a unit dual vector, this is, one in which its real part is a unit vector that its orthogonal
φ⋄ φ+ dǫ
to its dual part, and θ⋄ = = is a dual number. In fact unit dual quaternions can always be written
2 2
as (5.86), which is the equivalent to (4.27) for unit quaternions.

A process in some aspects similar to that described in section 4.5 proves that (5.86) is indeed the exponential
map {exp() : se (3) → SE (3) | S ∈ R8 → exp (S) ∈ Hd }, which transforms screws into unit dual quaternions. Note
that to do so, it is first necessary to represent the screw as the combination of a unit dual vector nm⋄ = n + m ǫ
φ⋄ φ + dǫ
representing the screw axis and a dual number θ⋄ = = containing the rotation angle and translation
2 2
distance about the screw axis. This map algebraically separates the line information of the screw axis from the
pitch and angle values, where the dual vector nm⋄ represents the axis of the screw motion with its direction
φ⋄
vector and the dual angle θ⋄ = contains both the translation length and the angle of rotation [37]. If there is
2
φ
no rotation (h = ∞), the resulting unit dual quaternion responds to ζ = qr + qd ǫ = q1 + n ǫ.
2
Obtainment of the logarithmic map {log() : SE (3) → se (3) | ζ ∈ Hd → S ∈ R8 } is now straightforward, resulting
in expressions (5.87) through (5.90) for the case of rotation plus translation (qr 6= q1 ):
kqvr k
φ = 2 arctan (5.87)
q0r
qvr
n = (5.88)
kqvr k
q0d
d = −2 (5.89)
kqvr k
 
d q0r −1
m = qvd − n kqvr k (5.90)
2

In the no rotation case (qr = q1 ), the logarithmic map changes as described above. Note that both the exponential
and logarithmic maps share the same surjective traits as those between the rotation vector and unit quaternion
described in section 4.4.3.

Although inverting the motion by means of the screw is straightforward,

T
S−1 = [n m h − φ] (5.91)

the different SE (3) actions (concatenation, point rotation, vector rotation) are complex and rarely used.

5.8 Rigid Body Motion Algebraic Operations

As in the case of pure rotations described in section 4.7, the basic algebraic operations of addition, subtraction,
multiplication, division, and exponentiation are not defined for objects of the special Euclidean group SE (3).
However, all rigid body motion representations are closed under a given operation that represents the concate-

63
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

nation of transformations, and define not only an identity transformation that represents the lack of motion, but
also an inverse operation representing the opposite movement. The concatenation of transformations and the
identity and inverse operations enable the definition of the power, exponential and logarithmic operators (section
5.8.1), the screw linear interpolation (section 5.8.2), and the perturbations together with the plus and minus
operators (section 5.8.3).

5.8.1 Powers, Exponentials and Logarithms

Any rigid body motion can be executed by rotating an angle φ about a certain fixed axis (n, m) combined with
T
a translation of a distance d = h · φ along that same axis, resulting in the screw S = [n m h φ] (section 5.7).
As the rotation and translation can be executed simultaneously or one after another, taking a fraction of a screw
T T
results in t S = t [n m h φ] = [n m h tφ] ∀ t ∈ R, S ∈ se (3).

The exponential map defined in (5.86) is named that way because it complies with the behav-
ior of the real exponential function expb (a) = exp (a · b) ∀ a, b ∈ R. As such, the exponential function
{exp() : se (3) × R → SE (3) | S ∈ se (3) , t ∈ R → Mt = exp (t S) ∈ SE (3)} is defined as:

t φ⋄ t θ⋄ tφ + tdǫ tφ + tdǫ
ζ t (S) = ζ (t S) = exp (t S) = cos + nm⋄ · sin = cos + (n + m ǫ) · sin (5.92)
2 2 2 2

In a similar way, the logarithmic map defined in (5.87) through (5.90) also complies with the behav-


ior of the real logarithmic function b · log (a) = log ab ∀ a, b ∈ R. As such, the logarithmic function
t


{log : SE (3) × R → se (3) | M ∈ SE (3) , t ∈ R → t S = log M ∈ se (3)} is defined as:


log ζ t (S) = log (exp (t S)) = t log (ζ (S)) = t log (exp (S)) = t S (5.93)

It is important to remark that although other exponential maps have been defined, with inputs either the
transform vector (5.22) or the half transform vector, they can not be employed in the exponential function, as
the multiple of a transform vector t τ = (t s, t r) does not result in a uniform movement and hence its associated
motion does not coincide with that of the same multiple of the screw t S.

5.8.2 Screw Linear Interpolation

Given two rigid body motions M0 , M1 ∈ SE (3), screw linear interpolation (ScLERP) is an extension of SLERP
(section 4.7.2) that obtains a motion function M (t) , t ∈ R that linearly interpolates from M (0) = M0 to
M (1) = M1 in such a way that the motion occurs with constant rotation and translation velocities [28].

If employing unit dual quaternions, ∆ζ is according to (5.53) the full motion required to go from ζ 0 to ζ 1 , such that
T
ζ 1 = ζ 0 ⊗ ∆ζ, from where ∆ζ = ζ ∗0 ⊗ ζ 1 . The corresponding screw is then ∆S = [n m h ∆φ] = log (∆ζ).
The unit dual quaternion corresponding to a fraction of the full screw magnitude δφ = t ∆φ is the following:



δζ = exp S (n, m, h, δφ) = exp t · S (n, m, h, ∆φ) = exp (t ∆S)


t
= exp t log (∆ζ) = exp t log (ζ ∗0 ⊗ ζ 1 ) = (ζ ∗0 ⊗ ζ 1 ) (5.94)

The interpolated unit dual quaternion is hence the following, which relies on (5.92) for its solution:

t
ζ (t) = ζ 0 ⊗ (ζ ∗0 ⊗ ζ 1 ) (5.95)

The restrictions described in section 4.7.2 intended to ensure that the rotation is executed following the shortest
path are also applicable in this case.

64
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

5.8.3 Plus and Minus Operators

A perturbed rigid body motion M f ∈ SE (3) can always be expressed as the composition of the unperturbed
motion M with a (usually) small perturbation ∆M. Perturbations can be specified either at the local or body
frame FB , this is, at the local vector space tangent to SE (3) at the actual pose, in which case they are known
as local perturbations. They can also be specified at the global frame FE , which coincides with the vector space
tangent to SE (3) at the origin; in this case they are known as global perturbations. Local perturbations appear
on the right hand side of the motion composition, resulting in Mf = M ◦ ∆MB , while global ones appear to the
f = ∆ME ◦ M.
left, hence M

The plus and minus operators are introduced in section 3.3 and enable operating with increments of the nonlinear
SE (3) manifold expressed in the linear tangent vector space se (3). There exist right (⊕, ⊖) or left (⊞, ⊟) versions
depending on whether the increments are viewed in the local frame (right) or the global one (left). It is important
to remark that although perturbations and the plus and left operators are best suited to work with small motion
changes (perturbations), the expressions below are generic and work just the same no matter the size of the
perturbation.
f = M ⊕ ∆τ B = M ◦ Exp (∆τ B )} produces a motion
The right plus operator {⊕ : SE (3) × se (3) → SE (3) | M
element M f resulting from the composition of a reference motion M with an often small motion ∆τ B , con-
tained in the tangent space to the reference motion M, this is, in the local space. The left plus operator
f = ∆τ E ⊞ M = Exp (∆τ E ) ◦ M} is similar but the often small motion ∆τ E is
{⊞ : se (3) × SE (3) → SE (3) | M
contained in the tangent space at the identify or global space. The expressions shown below are valid up to the
first coverage of SE (3), this is, φ < π. In the cases of homogeneous matrix and unit dual quaternion, the plus
operator is defined as:
f
M = M ⊕ ∆τ B = M Exp (∆τ B ) = M ∆MB (5.96)
ζe = ζ ⊕ ∆τ B = ζ ⊗ Exp (∆τ B /2) = ζ ⊗ ∆ζ B (5.97)
f
M E E
= ∆τ ⊞ M = Exp (∆τ ) M = ∆M M E
(5.98)
ζe = ∆τ E ⊞ ζ = Exp (∆τ E /2) ⊗ ζ = ∆ζ E ⊗ ζ (5.99)

The right minus operator {⊖ : SE (3) × SE (3) → se (3) | ∆τ B = Mf ⊖ M = Log M−1 ◦ M f }, as well as the left


f ⊟ M = Log M
{⊟ : SE (3) × SE (3) → se (3) | ∆τ E = M f ◦ M−1 }, represent the inverse operations, returning
the transform vector difference ∆τ between two motions M and M f expressed in either the local or global
tangent spaces to M.
 
∆τ B = Mf ⊖ M = Log M−1 M f = Log (∆MB ) (5.100)
 
∆τ B = ζe ⊖ ζ = 2 Log ζ ∗ ⊗ e
ζ = 2 Log (∆ζ B ) (5.101)
 
∆τ E = Mf ⊟ M = Log M f M−1 = Log (∆ME ) (5.102)
 
∆τ E = ζe ⊟ ζ = 2 Log eζ ⊗ ζ ∗ = 2 Log (∆ζ E ) (5.103)

If the ∆τ perturbation is small, the (5.20) and (5.61) Taylor expansions can be truncated, resulting in the
following expressions, valid for both the body frame (∆τ B ) or the global one (∆τ E ):
∧
∆M = Exp (∆τ ) ≈ I4 + ∆τ ∧ = I4 + [k ∆ρ n ∆φ] (5.104)
 T ∧
∆ζ = exp (∆τ /2) ≈ ζ1 + ∆τ ∧ /2 = [1 n ∆φ/2] + ǫ k ∆ρ/2 (5.105)

5.9 Rigid Body Motion Time Derivative and Twist

Let M (t) ∈ SE (3) , t ∈ R be a moving rigid body and compute its derivative with time, which belongs to neither
SE (3) nor se (3) but to the Euclidean space of the chosen motion representation, R4x4 for the homogeneous

65
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

matrix and Hd for the unit dual quaternion:

M (t + ∆t) − M (t)
Ṁ (t) = lim (5.106)
∆t→0 ∆t

Considering the time modified motion M (t + ∆t) as the perturbed state (section 5.8.3), the resulting time
derivatives for the homogeneous matrix and unit dual quaternion representations are the following:
h i
B ∧

M ∆M − MB M I 4 + [k ∆ρ n B
∆φ] − I 4
Ṁ (t) = lim ≈ lim
∆t→0 ∆t ∆t→0 ∆t
∧
[∆ρ kB ∆φ nB ]
= M lim (5.107)
∆t→0 ∆t  
∧
T B
ζ ⊗ [1 n B
∆φ/2] + ǫ k ∆ρ/2 − ζ1
ζ ⊗ ∆ζ B − ζ
ζ̇ (t) = lim ≈ lim
∆t→0 ∆t ∆t→0 ∆t
B ∧
[n ∆φ/2 + ǫ k ∆ρ/2]
B

= ζ ⊗ lim (5.108)
∆t→0 ∆t
T
Similar expressions based on τ E = [∆ρ kE ∆φ nE ] can be found if left multiplying by the perturbation instead
of right multiplying. The Ṁ (t) and ζ̇ (t) expressions (5.16) and (5.67) are then directly obtained when defining
the body twist ξ BEB as the time derivative of the transform vector τ B when viewed in local or body frame FB , and
the spatial twist ξEEB as the time derivative of the transform vector τ E when viewed in global or spatial frame
FE :
T
∆τ B [kB ∆ρ nB ∆φ]
ξ BEB (t) = ∆τ̇ B (t) = lim = lim (5.109)
∆t→0 ∆t ∆t→0 ∆t
E T
∆τ E
[k ∆ρ nE ∆φ]
ξ EEB (t) = ∆τ̇ E (t) = lim = lim (5.110)
∆t→0 ∆t ∆t→0 ∆t
T
The twist ξ = [ν ω] represents the motion velocity and is composed by the angular velocity ω defined in
section 4.8 and the linear velocity ν. The twist physical meaning is revealed by obtaining its expressions when
viewed in both the local and spatial frames. The body twist ξ B∧
EB
∈ se (3) corresponding to the rigid body motion
MEB (t) ∈ SE (3) responds to (5.14):
" # " #" E
# " E
#
B∧
ωb BEB ν BEB −1 RTEB −RT E
EB TEB ṘEB ṪEB RT
EB ṘEB RT
EB ṪEB
ξ EB = = MEB ṀEB = = (5.111)
0T
3 0 0T
3 1 0T
3 0 0T3 0

Its physical interpretation is that the angular component ωBEB is indeed the (4.8) angular velocity ωEB as viewed
from the body frame, and the linear component ν BEB is the linear velocity of the body frame origin ṪEB also
viewed from the body frame (4.4, 4.5) [31]. The global twist ξE∧EB ∈ se (3) is determined by means of (5.13):
" # " E
#" #
E∧
b EEB ν EEB
ω −1 ṘEB ṪEB RT EB
−RT EB
TEEB
ξ EB = = ṀEB MEB =
0T3 0 0T3 0 0T
3 1
" E
#
ṘEB RT EB ṪEB − ṘEB RT EB TEB
E

= T
(5.112)
03 0

The ξE∧
EB
physical interpretation is not intuitive, however. While the angular component ω EEB is the (4.7) angular
velocity ω EB as viewed from the spatial frame, its linear component ν EEB is not the velocity of the body frame
E
origin ṪEB viewed in the FE frame (ṪEB ), but the velocity, viewed in the spatial frame, of a possibly imaginary
point of the rigid body which at time t is traveling through the origin of the spatial frame [31]. Chapter 6 may
facilitate the understanding of this concept.

Note that the transformation or motion of the twist (relationship between ξ EEB and ξBEB ), and hence that of the
linear velocities ν EEB and ν BEB , is not given by the motion action gM∗ (5.1) but by the adjoint map AdM described

66
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

in section 5.11. Unlike in the case of rotations, these two maps do not coincide.

5.10 Rigid Body Motion Point Velocity

There exists a direct relationship between the velocity of a point belonging to a rigid body and the elements of its
tangent space, this is, the twist in se (3). This relationship is independent of the SE (3) representation, although
T
the homogeneous matrix is employed in the expressions below. If p̄B = [pB 1] are the fixed coordinates of a
T
point belonging to the FB rigid body, the point spatial coordinates p̄E = [pE 1] can be obtained by means of
(5.8):

p̄E (t) = gMEB (t) (p̄B ) = MEB (t) p̄B (5.113)

The velocity of a point is the time derivative of its spatial or global coordinates. As p̄ is fixed to FB , its time
B
derivative is zero p̄˙ = 0 , so its velocity viewed in the spatial frame responds to:

E
v̄Ep (t) = p̄˙ (t) = ṀEB (t) p̄B (5.114)

Although ṀEB maps the point body coordinates to its spatial velocity per (5.114), its high dimension makes it
inefficient. By making use of the spatial and body twists (ξ EEB∧ , ξBEB∧ ) introduced in (5.16), the velocity of a point
p̄B viewed in FE can be obtained as follows:

v̄Ep (t) = ξE∧

EB
(t) MEB (t) p̄B = ξ E∧
EB
(t) p̄E (t) (5.115)
v̄Ep (t) = MEB (t) ξ B∧
EB (t) p̄
B
(5.116)

The velocity of p̄B viewed in FB can then be obtained by means of the vector action map:

−1
v̄Bp (t) = g−1 E E B∧
MEB(t) ∗ v̄p (t) = MEB (t) v̄p (t) = ξ EB (t) p̄
B
(5.117)

Returning to Cartesian coordinates and introducing the angular and linear components of the twist results
in:

vEp (t) = b EEB (t) pE (t) + ν EEB (t)

ω (5.118)
B B B B
v (t) =
p
b
ω EB (t) p + ν EB (t) (5.119)

The point velocity is hence the result of the sum of the linear velocity and the cross product between the angular
velocity and the point coordinates.

5.11 Rigid Body Motion Adjoint

The adjoint map of a Lie group is defined in section 3.3.3 as an action of the Lie group on its
own Lie algebra that converts between the local tangent space and that at the identity. In the
case of rigid body motion, both the transform vector and the twist belong to the tangent space, so


Ad () : SE (3) × se (3) → se (3) | AdM (τ ∧ ) = M ◦ τ ∧ ◦ M−1 , AdM ξ∧ = M ◦ ξ ∧ ◦ M−1 . This is equiv-
alent to ζ ⊗ ξ ∧ ⊗ ζ ∗ for unit dual quaternions or M ξ∧ M−1 for homogeneous matrices, which represents the
similarity transformation2 between the spatial and body twists ξ EEB∧ and ξ BEB∧ :


ω b BEB RT
b EEB = REB ω EB
b BEB
= AdREB ω
−1
ξ E∧ B∧
EB = MEB ξ EB MEB → (5.120)
 νE = R νB − ω b EEB ωEEB
b EEB TEEB = REB ν BEB + T
EB EB EB

2
Two square matrices A and B are called similar if B = P−1 A P for some invertible square matrix P.

67
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

The application of the vee operator results in the adjoint matrix:

" # " #" # " #
E
ν EEB REB b EEB REB
T ν BEB REB b EEB REB
T
ξ EB
= = T = T ξ BEB = AdMEB ξBEB (5.121)
ω EEB 03x3 REB ω BEB 03x3 REB

As stated above, note that the adjoint map (5.121) is different than the vector action gM∗ (5.1), unlike the case
of rotational motion described in section 4.10, in which they coincide.

A similar process leads to the inverse adjoint matrix (Ad−1

M ξ = AdM−1 ξ):

" #
RT −RT bE
T
ξ B
= Ad−1
MEB ξ E
= EB EB EB
ξ EEB (5.122)
RT
EB EB
0T
3x3 EB

5.12 Rigid Body Motion Uncertainty and Covariance

Following the analysis of uncertainty on Lie groups presented in section 3.3.5, the definitions of local and global
autocovariances for SE (3) elements around a nominal or expected rotation E [M] = µM ∈ SE (3) are the follow-
ing:
  h i
CBMM = E ∆τ B ∆τ BT = E (M ⊖ µM ) (M ⊖ µM )T ∈ R6x6 (5.123)
  h i
T
CEMM = E ∆τ E ∆τ ET = E (M ⊟ µM ) (M ⊟ µM ) ∈ R6x6 (5.124)

Note that although the notation refers to the covariance of the rigid body motion manifold M ∈ SE (3), the
definition in fact refers to the covariance of the transform vectors ∆τ B or ∆τ E located in the tangent space, with
its dimension (6) matching the number of degrees of freedom of the SE (3) manifold. The relationship between
the local and global autocovariances responds to:

CEMM = AdMEB CBMM AdT

MEB (5.125)

Given a function {f : M → N | N = f (M) ∈ SE (3) , ∀M ∈ SE (3)} between two rigid body motions, the covari-
ances are propagated as follows:
⊖ f (M) B ⊖ f (M),T
CBN N = J CMM J ∈ R6x6 (5.126)
⊕M ⊕M
⊟ f (M) E ⊟ f (M),T
CEN N = J⊞ M CMM J⊞ M ∈ R6x6 (5.127)

5.13 Rigid Body Motion Jacobians

Lie group Jacobians are introduced in section 3.4 based on the right and left Lie group derivatives of section
3.3.4, and in this section are customized for the SE (3) case, with table 5.8 representing the particularization of
table 3.1 to the case of rigid body motions3 . The various Jacobians listed in table 5.8 have been obtained by
means of the chain rule, the expressions already introduced in this chapter, and those of section 3.3. Note that
although in many cases the results internally include the rotation matrix, all Jacobians are generic and do not
depend on the specific SE (3) parameterization.

In addition to the adjoint matrix, two other Jacobians are of particular importance as they appear repeatedly in
table 5.8. These are the right and left Jacobians of the capitalized exponential function, also known as simply the
right Jacobian JR (τ ) and the left Jacobian JL (τ ), and they evaluate the variation of the se (3) tangent space
provided by the output of the Exp (τ ) map (locally for JR and globally for JL ) while moving along the SE (3)
manifold with respect to the (Euclidean) variations within the original tangent space provided by τ . Their closed
 
3 a b
To save space within table 5.8, <; > within a matrix implies a different row. For example, [a b; c d] is equivalent to .
c d

68
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

forms as well as those of their inverses are included in table 5.8, and have been obtained from [38]; they are based
on the Q (τ ) matrix:
b
s φ − sin φ
Q (τ ) = Q (s, r) = Q (k ρ, n φ) = + rb
(b s+b sbr+b r bs b
r)
2 φ3
1 − φ2 /2 − cos φ  2 2

− b
r b
s + b
s b
r − 3 b
r b
s b
r
φ4
 
1 1 − φ2 /2 − cos φ φ − sin φ − φ3 /6  2 2

− − 3 b
r b
s b
r + b
r b
s b
r ∈ R3x3 (5.128)
2 φ4 φ5

− gExp(τ ) (p)
It is also worth noting the special importance of the J+ τ Jacobian present at the bottom of table 5.8,
which represents the derivative of a transformed point with respect to perturbations in the Euclidean tangent
space (not on the curved manifold) that generates the motion, as it enables tangent space optimization by calculus
methods designed exclusively for Euclidean spaces.

Jacobian Table 3.1 Expression Size

h i
⊖ M−1
J⊕ = −AdM = − R Tb R; 03x3 R ∈ R6x6
M
−1 h i
⊟ M
J⊞ M = −Ad−1 = − RT − RT T; b 03x3 RT ∈ R6x6
M
h i
J⊖
⊕M
M◦N = Ad−1
N = RTN − R T b
N T N ; 0 3x3 R T
N ∈ R6x6

J⊟ M◦N = I = I6x6 ∈ R6x6

⊞M
J⊖
⊕N
M◦N = I = I6x6 ∈ R6x6
h i
J⊟ M◦N = AdM = RM Tb M RM ; 03x3 RM ∈ R6x6
⊞N
⊖ Exp (τ )
J+ τ = JR (τ ) = JL (−τ ) = JL (−s, −r) ∈ R6x6

J−1
R (τ ) = J−1 −1
L (−τ ) = JL (−s, −r) ∈ R6x6
h i
⊟ Exp (τ )
J+ τ = JL (τ ) = JL (r) Q (τ ) ; 03x3 JL (r) ∈ R6x6
h i
J−1
L (τ ) = JL−1 (r) − J−1 −1 −1
L (r) Q (τ ) JL (r) ; 03x3 JL (r) ∈ R6x6
− Log (M)

J⊕ M = J−1
R Log (M) ∈ R6x6
− Log (M)

J = J−1
L Log (M) ∈ R6x6
⊞M
h i
J⊖ M⊕τ T T b T
⊕M = Ad−1
Exp(τ ) = R (r) − R (r) T (s, r) ; 03x3 R (r) ∈ R6x6
h i
J⊟ τ ⊞M = AdExp(τ ) = R (r) Tb (s, r) R (r) ; 03x3 R (r) ∈ R6x6
⊞M
J⊖
+τ
M⊕τ = JR (τ ) ∈ R6x6

J⊟ τ ⊞M = JL (τ ) ∈ R6x6
+τ
J−
⊕M
N ⊖M = −J−1
L (N ⊖ M) ∈ R6x6
−N ⊟M −J−1 ∈ R6x6
J⊞ M = R (N ⊟ M)

J−
⊕N
N ⊖R = J−1
R (N ⊖ M) ∈ R6x6
−N ⊟R J−1 ∈ R6x6
J⊞ N = L (N ⊟ M)
− gM (p)
J⊕ M = [R − R pb] ∈ R3x6
h i
− gM (p)
J = I3x3 − (R p)∧ − T
b ∈ R3x6
⊞M
− g (p)
J+ pM = R ∈ R3x3

69
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Jacobian Table 3.1 Expression Size

− −1
gM (p) h
∧ i
J⊕ M = −I3x3 RT (p − T) ∈ R3x6
−1
− gM (p) h i
J⊞ M = −RT RT p
b ∈ R3x6
− g−1 (p)
J+ pM = RT ∈ R3x3
h i
− AdM (ξ)
J = − (R ω)∧ R − R νb − T b Rω b ; 03x3 − R ω b ∈ R6x6
⊕M
h i
− AdM (ξ) ∧ ∧ b (R ω)∧ + (R ω)∧ T; b 03x3 − (R ω)∧
J⊞ M = − (R ω) − (R ν) − T ∈ R6x6
h i
− AdM (ξ) b R; 03x3 R
J = AdM = R T ∈ R6x6
+ξ 
− Ad−1  ∧  ∧  ∧ 
J M (ξ) = RT ω
b R RT ν − RT T b ω ; 03x3 RT ω ∈ R6x6
⊕M
− Ad−1 (ξ) h i
J⊞ M M = RT ωb RT ν
b − RT Tbωb ; 03x3 RT ω b ∈ R6x6
− Ad−1 h i
J M (ξ) = Ad−1 = R T
− R T b
T; 0 R T
∈ R6x6
M 3x3
+ξ
− gExp(τ ) (p) h i
∧ b JL (r)
J+ τ = JL (r) Q (τ ) − [R p] JL (r) − T ∈ R3x6
−1
− gExp( h h i∧ i
τ ) (p)
J+ τ = − JL (−r) − Q (−τ ) − RT (T − p) JL (−r) ∈ R3x6

Table 5.8: Rigid body motion Jacobians

5.14 Rigid Body Motion Discrete Integration

The discrete integration with time of an element of a Lie group based on its Lie algebra is discussed in detail in
section 3.5, which includes expressions for the Euler, Heun and Runge-Kutta methods. In the case of rigid body
motion, the state vector includes the motion element M ∈ SE (3) and its twist ξ ∈ R6 contained in the tangent
space, viewed either in the local (ξ BEB ) or global (ξ EEB ) frames. The Euler method expressions equivalent to (3.42)
and (3.44) are shown below. Expressions for other integration schemes can easily be derived from those in section
3.5:

Mk+1 ≈ Mk ⊕ [∆t ξ BEBk ] = Mk ◦ Exp (∆t ξ BEBk ) (5.129)

E E
Mk+1 ≈ [∆t ξ EBk ] ⊞ Mk = Exp (∆t ξ EBk ) ◦ Mk (5.130)

5.15 Rigid Body Motion Gauss-Newton Optimization

The minimization by means of the Gauss-Newton iterative method of the Euclidean norm of a nonlinear function
whose input is a Lie group element is presented in section 3.6. In the case of rigid body motion, the resulting
expressions for perturbations ∆τ EEB ∈ se (3) to an input motion M ∈ SE (3) viewed in the global frame FE are
shown in (5.131) and (5.132), which are equivalent to the generic (3.63) and (3.66). Refer to section 3.6 for the
meaning of the function Jacobian J and to section 5.13 for that of the left Jacobian JL .

Mk+1 ←− ∆τ EEBk ⊞ Mk = ∆τ EEBk ◦ Exp (τ EBk ) (5.131)

h i−1
∆τ EEBk = − J−T T −1
Lk Jk Jk JLk J−T T
Lk Jk E k (5.132)

If the perturbation is viewed in the local frame FB , (3.67) and (3.68) are customized as follows, making use of
the right Jacobian JR defined in section 5.13:

Mk+1 ←− Mk ⊕ ∆τ BEBk = Exp (τ EBk ) ◦ ∆τ BEBk (5.133)

h i−1
∆τ BEBk = − J−T
Rk JT
k Jk J−1
Rk J−T T
Rk Jk E k (5.134)

70
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

5.16 Rigid Body Motion State Estimation

The adaptation of the EKF state estimation introduced in section 2.5 to the case in which Lie group elements and
their velocities are present is discussed in detail in section 3.7. For rigid body motion with local perturbations, it is
necessary to replace X ∈ G by M ∈ SE (3), ∆τ X ∈ TX G by ∆τ B ∈ se (3), vX ∈ Rm by ξ B ∈ R6 , CX XX ∈ R
mxm
by
B 6x6
CMM ∈ R , and J⊕ X ⊖ X ⊕ τ ⊖
by J⊕ MM ⊕ τ . The particularizations for global perturbations are similar.

5.17 Applications of the Various Motion Representations

This chapter discusses six different parameterizations of the rigid body motion or special Euclidean group SE (3):
the affine representation, the homogeneous matrix, the transform vector, the unit dual quaternion, the half
transform vector, and the screw. Although in theory all of them can be employed for each of the purposes
described in this chapter, and the required expressions derived, each parameterization has its own advantages
and disadvantages, being suited for certain purposes but not recommended for others.

• The affine representation (R, T) based on either the rotation matrix or the unit quaternion is the most
natural parameterization for rigid body motion. It can be employed to track the motion over its mani-
fold, although other options are preferred. Many difficulties arise from its complex nature as a composition
between an SO (3) rotation and a R3 vector, such as the complex inverse and concatenation, the different na-
ture of the point and vector actions, the lack of simple plus and minus operators to deal with perturbations,
and the need to continuously keep track of both components when moving over the manifold.

• The homogeneous matrix M is a generalization of the rotation matrix for the case of rigid body motion
that not only linearizes the transformation of coordinates at the expense of bigger size, but also adopts
matrix algebra for the inversion and concatenation of transformations. A second advantage is that the
transformations of vectors and points share the same map when employing homogeneous coordinates.
Additionally, it provides a clear connection with the tangent space, together with the exponential and
logarithmic maps, and plus and minus operators, which are not complex. Its main inconvenients are the
huge size (16), the expense of maintaining the internal rotation matrix orthogonal if allowed to deviate from
the manifold, and the need to work with homogeneous coordinates for both points and vectors. Its high
cost precludes its use to track the motion over its manifold, although most implementations continuously
compute its components (the rotation matrix and the translation vector) if the adjoint matrix or the
Jacobian blocks are required.

• The unit dual quaternion ζ is the preferred parameterization to track the motion over its manifold, even if it
is necessary to obtain the rotation matrix and the translation vector for the adjoint and Jacobian blocks. It
possesses a significant size advantage (8) with respect to the homogeneous matrix, although it is not cheap
to recover its structure if allowed to deviate from the manifold. Unit dual quaternions are the least natural
of the rigid body motion representations, being necessary to convert to a different SE (3) representation
for visualization. While the inverse and concatenation are simple and linear, the motion actions for points
and vectors are bilinear and require slightly different expressions when inverting them, which presents a
disadvantage with respect to the the homogeneous matrix. A significant advantage is given by its direct
relationship with the screw and associated ScLERP capabilities. Unit dual quaternion expressions are more
complex than those of the homogeneous matrix, and present a slightly less obvious connection with the
tangent space.

• The main advantage of the transform vector τ is that it belongs to the se (3) tangent space while simul-
taneously being an SE (3) representation. It is hence indicated for those uses related with incremental
motion changes by means of the exponential map together with the plus and minus operators (periodically
adding the perturbations to the unit dual quaternion tracking the motion), such as discrete integration,
optimization, and state estimation. The norms of its angular and linear components (φ, ρ) are the most
adequate metrics for evaluating the distance (or estimation error) between two rigid bodies. Although it

71
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

benefits from its straightforward inverse when used as a perturbation, its geometric appeal, and its small di-
mension (6), its usage for other applications is discouraged by its complex nonlinear kinematics, coordinate
transformation, and composition, which are not shown in this chapter.

• The half transform vector Ψ is so similar (half) to the transform vector that its usage is not recommended
in order to avoid confusion. Its only real application as the tangent space of the unit dual quaternion is in
practice solved by dividing the transform vector by two when necessary.

• The screw S, in addition to simultaneously belonging to the se (3) tangent space and the SE (3) manifold,
has the advantage that it clearly separates the influence of the motion direction from that of its magnitude,
and as such it enables the definition of powers and ScLERP, which can not be obtained with any of the other
representations. The dimension is not big (8) and the inversion is straightforward, but all other possible
expressions, including motion and concatenation, are very complex and not shown in this chapter.

72
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Chapter 6

Relative Motion of Multiple Rigid

Bodies

This chapter develops expressions for the positions, velocities, and accelerations (both linear and angular) that
refer to different reference systems or rigid bodies, which are in continuous motion (translation and rotation)
among themselves. It relies on the analysis of the motion of rigid bodies performed in chapters 4 and 5 to
determine the relationships among these vectors when viewed in different reference frames.

i22
i23 O2
i21 i13
i03 T02
T12

T01 i12
O0 O1

i01 i02 i11

Figure 6.1: Reference system for combination of movements

The below expressions are based on the three reference systems shown in figure 6.1: an inertial reference system
F0 {00 , i01 , i02 , i03 }1 , and two non-inertial systems F1 {01 , i11 , i12 , i13 } and F2 {02 , i21 , i22 , i23 }, where (T01 , v01 , a01 )
are the position, linear velocity, and linear acceleration of O1 with respect to O0 , (T02 , v02 , a02 ) those of O2 with
respect to O0 , and (T12 , v12 , a12 ) those of O2 with respect to O1 . Similarly, (ω01 , α01 ) are the angular velocity
and angular acceleration of F1 with respect to F0 , (ω 02 , α02 ) those of F2 with respect to F0 , and (ω12 , α12 )
those of F2 with respect to F1 . Consider also that R01 , R02 , and R12 are the rotation matrices (section 4.2)
among the three different rigid bodies.

• Composition of Position. The relationship between the linear position vectors T02 , T12 , and T01
can be established by vector arithmetics when expressed in the same reference frame, or by coordinate
transformation (4.4) when not so:

T002 = T012 + T001 = R01 T112 + T001 (6.1)

1
An inertial frame is that in which bodies whose net force acting upon them is zero do not experience any acceleration and remain
either at rest or moving at constant velocity in a straight line. Any reference frame can be considered inertial for the analysis of the
motion of a given object if the accelerations (linear and angular) of that frame with respect to an accepted inertial system can be
discarded when compared with the accelerations characteristic of the movement being studied.

73
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

• Composition of Linear Velocity. The derivation with time of (6.1) results in:

0 1 0
Ṫ02 = Ṙ01 T112 + R01 Ṫ12 + Ṫ01 (6.2)

Replacing the rotation matrix time derivative by (4.9) results in:

0 1 0
b 101 T112 + R01 Ṫ12 + Ṫ01
Ṫ02 = R01 ω (6.3)

Reordering, replacing the position time derivatives with their respective velocities, and employing (4.9),
results in the relationship between the linear velocity vectors v02 , v12 , and v01 expressed in the inertial
frame F0 :

v002 b 101 T112

= R01 v112 + v001 + R01 ω (6.4)
v002 = v012 + v001 + b 001
ω T012 (6.5)

• Composition of Linear Acceleration. The derivation with time of (6.4) results in:

1
v̇002 = Ṙ01 v112 + R01 v̇112 + v̇001 + Ṙ01 ω b 101 Ṫ12
ḃ 101 T112 + R01 ω
b 101 T112 + R01 ω (6.6)

Replacing the rotation matrix time derivative by (4.9) results in:

1
v̇002 = R01 ω b 101 ω
b 101 v112 + R01 v̇112 + v̇001 + R01 ω b 101 T112 + R01 ω b 101 Ṫ12
ḃ 101 T112 + R01 ω (6.7)

Reordering, replacing the position, linear velocity, and angular velocity time derivatives with their respective
velocities, linear accelerations and angular accelerations, and employing (4.9), results in the relationship
between the linear acceleration vectors a02 , a12 , and a01 expressed in the inertial frame F0 :
 
a002 = R01 a112 + a001 + R01 α b 101 T112 + R01 ω b 101 ω b 101 v112
b 101 T112 + 2 R01 ω (6.8)
 
a002 = a012 + a001 + α b 001 T012 + ωb 001 ω
b 001 T012 + 2 ω b 001 v012 (6.9)

The term on the left hand side is called absolute acceleration, while the three right hand side terms are
usually named relative, transport, and Coriolis accelerations, respectively.

• Composition of Angular Velocity. The relationship among the different frames angular velocities is
given by the rotation matrix composition rule (4.6), which can be derivated with respect to time:

R02 = R01 R12 → Ṙ02 = Ṙ01 R12 + R01 Ṙ12 (6.10)

Replacing the rotation matrix time derivatives by (4.9) and employing the rotational motion adjoint (4.78)
results in:

b 202 = ω
R02 ω b 212 = ω
b 001 R01 R12 + R01 R12 ω b 001 R02 + R02 ω b 212
b 201 + R02 ω
b 212 = R02 ω (6.11)

The relationship among the angular velocity vectors ω02 , ω 12 , and ω 01 is hence the following:

ω002 = ω 012 + ω001 = R01 ω112 + ω 001 (6.12)

• Composition of Angular Acceleration. The derivation with time of (6.12) results in:

ω̇002 = Ṙ01 ω 112 + R01 ω̇ 112 + ω̇ 001 (6.13)

74
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Replacing the rotation matrix time derivatives by (4.9) results in:

b 101 ω 112 + R01 ω̇ 112 + ω̇ 001

ω̇002 = R01 ω (6.14)

Reordering, replacing the angular velocity time derivatives with their respective angular accelerations, and
employing (4.9), results in the relationship between the angular acceleration vectors α02 , α12 , and α01
expressed in the inertial frame F0 :

α002 = b 101 ω112

R01 α112 + α001 + R01 ω (6.15)
α002 = α012 + α001 + b 001
ω ω012 (6.16)

The final expressions of the previous compositions (6.1, 6.5, 6.9, 6.12, and 6.16) are all expressed in the inertial
frame F0 , but they are also valid in any other frame as long as all its components are converted into that
frame2 :

T02 = T12 + T01 (6.17)

v02 b 01 T12
= v12 + v01 + ω (6.18)
a02 b 01 T12 + ω
= a12 + (a01 + α b 01 T12 ) + 2 ω
b 01 ω b 01 v12 (6.19)
ω 02 = ω 12 + ω 01 (6.20)
α02 b 01 ω 12
= α12 + α01 + ω (6.21)

2
Note that it is not the same to compute a time derivative (velocity, acceleration, or angular acceleration) in the inertial frame
and then convert it into a different frame, than to directly compute the derivative in a non-inertial frame.

75
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

Bibliography

[1] J. Sola, J. Deray, and D. Atchuthan, “A Micro Lie Theory for State Estimation in Robotics,” tech. rep.,
2018. arXiv:1812.01537v9 [cs.RO], https://doi.org/10.48550/arXiv.1812.01537.

[2] J. L. Blanco, “A Tutorial on SE(3) Transformation Parameterizations and On-Manifold Optimization,” 2020.
arXiv:2103.15980v2 [cs.RO], https://doi.org/10.48550/arXiv.2103.15980.

[3] O. C. Ibe, Fundamentals of Applied Probability and Random Processes. Elsevier, 2nd ed., 2005. ISBN
0-080-49270-3.

[4] A. Papoulis and S. U. Pillai, Probability, Random Variables, and Stochastic Processes. McGraw-Hill, 4th ed.,
2002. ISBN 0-07-122661-3.

[5] J. A. Farrell, Aided Navigation, GPS with High Rate Sensors. McGraw-Hill, 2008. ISBN 0-071-49329-8.

[6] D. Simon, Optimal State Estimation. John Wiley & Sons, 2006. ISBN 0-471-70858-5.

[7] Brian D. Ripley, Stochastic Simulation. John Wiley & Sons, 1987. ISBN 9780471818847.

[8] Shiomo S. Sawilowsky, “You Think You’ve Got Trivials?,” Journal of Modern Applied Statistical Methods,
2003. https://doi.org/10.22237/jmasm/1051748460.

[9] M. H. Shojaeefard, A. Khalkhali, and S. Yarmohammadistri, “An Efficient Sensitivity Analysis Method for
Modified Geometry of Macpherson Suspension based on Pearson Correlation Coefficient,” Vehicle System
Dynamics, 2017. https://doi.org/10.1080/00423114.2017.1283046.

[10] F. Frishman, “On the Arithmetic Means and Variances of Products and Ratios of Random Variables,” Army
Research Office, 1971. https://doi.org/10.1007/978-94-010-1842-5_32.

[11] P. G. Hoel, S. C. Port, and C. J. Stone, Introduction to Stochastic Processes. Waveland Press Inc., 1972.
ISBN 1-478-60899-4.

[12] R. M. Rogers, Applied Mathematics in Integrated Navigation Systems. AIAA, 3rd ed., 2007. ISBN 1-563-
47927-3.

[13] M. Grewal and A. Andrews, “How Good Is Your Gyro?,” IEEE Control Systems Magazine, 2010.
https://doi.org/10.1109/MCS.2009.935122.

[14] P. J. Huber, Robust Statistics. John Wiley & Sons, 1981. ISBN 0-471-65072-2.

[15] J. Fox and S. Weisberg, “Robust regression,” 2013. DOI not available.

[16] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery, Numerical Recipes in C++, The Art of
Scientific Computing. Cambridge University Press, 2nd ed., 2002. ISBN 0-521-75033-4.

[17] R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision. Cambridge University Press,
2nd ed., 2003. ISBN 0-521-54051-8.

[18] S. Baker and I. Matthews, “Lucas-Kanade 20 Years On: A Unifying Framework,” International Journal of
Computer Vision, 2004. https://doi.org/10.1023/B:VISI.0000011205.11775.fd.

76
Eduardo Gallo The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to . . .

[19] C. C. Pinter, A Book of Abstract Algebra. Dover Publications, 2nd ed., 1990. ISBN 0-486-47417-8.

[20] M. A. Armstrong, Basic Topology. Springer, 1983. ISBN 978-0-387-90839-7.

[21] M. D. Shuster, “A Survey of Attitude Representations,” Journal of the Astronautical Sciences, 1993. DOI
not available.

[22] S. Roman, Advanced Linear Algebra. Springer-Verlag, 2005. ISBN 0-387-24766-1.

[23] V. Bryant, Metric Spaces: Iteration and Application. Cambridge University Press, 1985. ISBN 0-521-31897-1.

[24] P. K. Jain, K. Ahmad, and O. P. Ahuja, Functional Analysis. New Age International, 1995. ISBN 8-122-
40801-X.

[25] E. Artin, Geometric Algebra. John Wiley & Sons, 1988 ed. ISBN 0-471-60839-4.

[26] Y. Ma, S. Soatto, J. Kosecka, and S. S. Sastry, An Invitation to 3-D Vision, From Images to Geometric
Models. Springer, 2001. ISBN 978-1-4419-1846-8.

[27] G. Strang, Linear Algebra and its Applications. Thomson Brooks/Cole, 4th ed., 2006. ISBN 0-03-010567-6.

[28] Y. B. Jia, “Dual Quaternions,” 2013. DOI not available.

[29] T. W. Gamelin and R. E. Greene, Introduction to Topology. Courier Corporation, 1999. ISBN 0-486-40680-6.

[30] J. Sola, “Quaternion Kinematics for the Error-State Kalman Filter,” tech. rep., 2017. arXiv:1711.02508v1
[cs.RO], https://doi.org/10.48550/arXiv.1711.02508.

[31] Richard M. Murray and Zexiang li and S. Shankar Sastry, A Mathematical Introduction to Robotic Manipu-
lation. CRC Press, 1994. ISBN 0-849-37981-4.

[32] G. S. Chirikjian, Stochastic Models, Information Theory, and Lie Groups, vol. 2. Birkhauser, 2012. ISBN
978-0-8176-4943-2.

[33] I. Y. Baritzhack and K. A. Fegley, “Orthogonalization Techniques of a Direction Cosine Matrix,” IEEE
Transactions of Aerospace and Electronic Systems, 1969. https://doi.org/10.1109/TAES.1969.309878.

[34] B. Kenwright, “Dual Quaternions: From Classical Mechanics to Computer Graphics and Beyond,” 2012.
DOI not available.

[35] A. Valverde and P. Tsiotras, “Dual Quaternion Framework for Modeling of Spacecraft Mounted Multibody
Robotic Systems,” Frontiers in Robotics and AI, 2018. https://doi.org/10.3389/frobt.2018.00128.

[36] K. Daniilidis, “Hand-Eye Calibration Using Dual Quaternions,” 1999.

https://doi.org/10.1177/02783649922066213.

[37] B. Busam, T. Birdal, and N. Navab, “Camera Pose Filtering with Local Regression Geodesics
on the Riemannian Manifold of Dual Quaternions,” 2017. arXiv:1704.07072v4 [cs.CV],
https://doi.org/10.48550/arXiv.1704.07072.

[38] T. D. Barfoot and P. T. Furgale, “Associating Uncertainty With Three-Dimensional Poses for Use in Esti-
mation Problems,” IEEE Transactions in Robotics, 2014. https://doi.org/10.1109/TRO.2014.2298059.

77