A Primer on the Differential Calculus of 3D Orientations

Michael Bloesch, Hannes Sommer, Tristan Laidlow, Michael Burri, Gabriel Nuetzi, Péter Fankhauser,
Dario Bellicoso, Christian Gehring, Stefan Leutenegger, Marco Hutter, Roland Siegwart

φ⃗(t)=Bφ(t) TISO(3)
Abstract— The proper handling of 3D orientations is a
ϕI
central element in many optimization problems in engineering. exp(φ(t))
ϕ0
Unfortunately many researchers and engineers struggle with the ϕ(t)=ϕ0⊞φ(t)
φ(t)=ϕ(t)⊟ϕ0=-tω
formulation of such problems and often fall back to suboptimal
solutions. The existence of many different conventions further

SO
arXiv:1606.05285v2 [cs.RO] 31 Oct 2016

complicates this issue, especially when interfacing multiple dif- R3

(3)
fering implementations. This document discusses an alternative
approach which makes use of a more abstract notion of 3D
orientations. The relative orientation between two coordinate Bv0=ϕ0(Iv)

ϕ(t) Bv'(0)=Bv0×ω
systems is primarily identified by the coordinate mapping it
ϕ0
induces. This is combined with the standard exponential map v⃗ Bv(t)=ϕ(t)(Iv)
in order to introduce representation-independent and minimal R3
differentials, which are very convenient in optimization based B(
t) ϕ0
ϕ(t)
methods. B Iv

I. I NTRODUCTION I

The primary goal of this document is to derive and sum-

marize the most important identities for handling 3D orienta-
tions. It can readily be used as a look-up document (general
identities are green (Section IV), implementation dependent
identities are red (Section V)). In a compact theoretical
section all equations are derived together with some insights
into their mathematical background (Section III). We believe
however, that the best way to understand these concepts is to
apply the presented findings on an actual system. To this end,
we discuss the modeling of an Inertial Measurement Unit
(IMU) driven kinematic model (Section VI). Furthermore, we
provide the most important proofs and derivations in order
to provide some additional insights and examples. Similar
elaborations on the topic exist in [1], [2], [3].
An understanding of kinematics (including the concept
of coordinate systems) is a prerequisite for understanding
this document. The corresponding conventions and notations
are summarized in Section II. To completely follow the
theoretical sections some higher mathematical concepts are
necessary.
II. V ECTORS AND C OORDINATE S YSTEMS N OTATION
In this document coordinate systems are denoted by cal-
ligraphic capital letters, e.g. A, and coordinate tuples are
represented by bold lower case letters, e.g. A r BC . The left-
hand subscript of a coordinate tuple indicates the coordinate
system the vector is represented in, while the right-hand
subscripts indicate the 3D points related to start and end
points. For instance, the term A r BC denotes the coordinates
of a vector ~r BC (denoted with an arrow) in the Euclidean
space E3 from point B to point C, represented in the coordi-
nate system A. By abuse of notation, we denote the origin
associated with a specific coordinate system by the same
symbol. Furthermore, the term ΦBA ∈ SO(3) is employed
Physical World R3
Fig. 1. This figure depicts various quantities in a setup where a coordinate
system B is rotated by a constant rotational velocity ω w.r.t. an inertial
coordinate system I. Using coordinate systems, physical vectors can be
represented through the corresponding coordinate tuples. Orientations be-
tween coordinate systems can be defined by the mapping they induce on the
coordinate tuples. They are elements of SO(3). Differences and derivatives
of orientations can be represented in the tangential space TΦI SO(3), which
can be associated with R3 by means of a basis B.
for representing the relative orientation of a coordinate sys-
tem B w.r.t. a coordinate system A. Its definition is coupled
to the (distance preserving) mapping of coordinate tuples and
we employ the notation B r BC = ΦBA (A r BC ). We define the
mapping C : SO(3) → R3×3 such that Φ(r) , C(Φ)r
(corresponding to the rotation matrix). A more complete
overview of coordinate systems and rotations is given in [3].
Furthermore, the vectors ~v B and ~aB denote the absolute
(w.r.t. an inertial coordinate system) velocity and acceleration
of the point B. The vector ω ~ AB denotes the relative angular
velocity of the coordinate system B w.r.t. the coordinate
system A. The skew symmetric matrix of a coordinate tuple
v ∈ R3 is denoted as v × ∈ R3×3 and has the property
v × r = v × r ∀r ∈ R3 , where × denotes the Euclidean
cross-product. The term v × fulfills the following identities
(I ∈ R3×3 is the identity matrix):
(v × )T = −v × , (1)
× 2 T T
(v ) = vv − v vI, (2)
×
(C(Φ)v) = C(Φ)v C(Φ) . × T
(3)
 III. T HEORY
The following contemplations are independent of the
choice of parametrization for 3D orientations. As will follow
in the next definition, 3D orientations are first only thought
of as mapping.
Given a 3D rigid body with attached body-fixed coordinate
system B, its orientation ΦBA w.r.t. a reference coordinate
system A can be defined as the mapping of coordinates of
any fixed vector ~r from A to B, that is,
Br = ΦBA (A r). (4)
We will see later, that by using this definition of the
exponential exp, its argument ϕ can be interpreted as the
rotation vector associated with the relative orientation of
two coordinate systems. There exists an open region around
0, the open ball with radius π Bπ (0) ⊂ R3 , such that the
exponential is bijective and its image corresponds to all non-
180◦ -orientations, SO(3)∗ . Thus an inverse exists which is
called the logarithm, log : SO(3)∗ → Bπ (0).
With this we can construct boxplus and boxminus oper-
ations which adopt the function of addition and subtraction
[4]:

Together with the concatenation operation, orientations form
:SO(3) × R3 → SO(3), (14)
a Lie group known as the special orthogonal group SO(3). Φ, ϕ 7→ exp(ϕ) ◦ Φ,
The concatenation ◦ : SO(3)×SO(3) → SO(3) comes with
:SO(3) × SO(3) → R3 , (15)
the following (defining) identity:
Φ1 , Φ2 7→ log(Φ1 ◦ Φ−1
2 ).
(ΦCB ◦ ΦBA )(A r) , ΦCB (ΦBA (A r)). (5)
Similarly to regular addition and subtraction, both operators
There also exists an identity element ΦI and an inverse Φ−1 fulfill the following identities (axioms proposed by [4]):
such that
Φ
0 = Φ, (16)
ΦI ◦ ΦBA = ΦBA ◦ ΦI = ΦBA , (6) (Φ
ϕ) Φ = ϕ, (17)
Φ−1 −1
BA ◦ ΦBA = ΦBA ◦ ΦBA = ΦI . (7) Φ1
(Φ2 Φ1 ) = Φ2 . (18)
The Lie group SO(3) is not a vector space, has no addition This approach distinguishes between actual orientations,
operation, and consequently no subtraction either. This poses which are on SO(3) (Lie group), and differences of ori-
an issue if using orientations in filtering or optimization entations which lie on R3 (Lie algebra, see Figure 1). The
frameworks, which strongly rely on small differences and above operators take care of appropriately transforming the
gradients (e.g. for linearization). Fortunately, since SO(3) elements into their respective spaces and allow a smooth em-
is a Lie group, there exists an exponential map Exp : bedding of rotational quantities in filtering and optimization
TΦI SO(3) → SO(3) relating SO(3) to its Lie algebra frameworks.
TΦI SO(3). The later coincides with the tangent space at the The definition of differentials involving orientations can be
identity element, which is isomorphic to R3 . The exponential adapted by replacing the regular plus and minus operators by
map is smooth and fulfills the following (uniquely) defining the above boxplus and boxminus operators. For instance the
identities ∀t, s ∈ R, ∀~
ϕ ∈ TΦI SO(3): differential of a mapping f1 : R → SO(3) can be defined
as:
Exp((t + s)~ ϕ) ◦ Exp(s~
ϕ) = Exp(t~ ϕ), (8)
∂ f1 (x + ) f1 (x)
d/dt(Exp(t~ ~.
ϕ))|t=0 = ϕ (9) f1 (x) := lim . (19)
∂x →0 
Elements on TΦI SO(3) are abstract vectors and are not The same can be done for the other case where we have a
very suitable for actual computations. By choosing a basis mapping f2 : SO(3) → R:
B = [~ϕ1 , ϕ ~ 3 ] the map can be extended to R3 . We define
~ 2, ϕ  T
the exponential exp : R3 → SO(3) of a coordinate tuple f2 (Φ
(e1 ))−f2 (Φ)
∂ 
ϕ = (ϕ1 , ϕ2 , ϕ3 ) ∈ R3 by f2 (Φ) := lim  f2 (Φ
(e2))−f2 (Φ)  .
 
(20)
∂Φ →0 f (Φ
(e ))−f (Φ)
2 3 2
exp(ϕ) := Exp(~ ~ 2 ϕ2 + ϕ
ϕ1 ϕ1 + ϕ ~ 3 ϕ3 ). (10) 

There is a certain degree of freedom in the selection of the IV. I MPLEMENTATION -I NDEPENDENT I DENTITIES
basis [~ ~ 2, ϕ
ϕ1 , ϕ ~ 3 ]. We choose the basis vectors ϕ
~ i such that Some identities directly follow from the above consider-
∀i ∈ {1, 2, 3}, ∀v ∈ R3 : ations and are independent of the choice of the underlying
orientation representation. By concatenating the exponential
d/dt(Exp(t~
ϕ i )(v))|t=0 = ei × v (11) and the coordinate mapping we retrieve the well known
where ei ∈ R3 are the standard basis vectors in R3 . This Rodriguez’ formula (see Appendix II-A):
makes exp(·) a unique smooth mapping that fulfills ∀t, s ∈ C(ϕ) := C(exp(ϕ)) (21)
R, ∀ϕ, v ∈ R3 : × ×2
sin(kϕk)ϕ (1 − cos(kϕk))ϕ
exp((t + s)ϕ) = exp(tϕ) ◦ exp(sϕ) (12) =I+ + ,
kϕk kϕk2
d/dt(exp(tϕ)(v))|
t=0 =ϕ×v (13) C(ϕ) ≈ I + ϕ× , (kϕk ≈ 0). (22)
This shows that the argument of the exponential, ϕ, can be B. Concatenation
interpreted as the coordinate tuple of the (passive) rotation The concatenation of two unit quaternions Φ1 = (q0 , q̌)
vector associated with the relative orientation of two coordi- and Φ2 = (p0 , p̌) is given by:
nate systems. Thus, if the corresponding coordinate systems
are known we can write: Φ1 ◦ Φ2 = (q0 p0 − q̌ T p̌, q0 p̌ + p0 q̌ + q̌ × p̌). (37)
ΦBA = exp (B ϕBA ) = exp (A ϕBA ) . (23) C. Exponential and Logarithm
We can also derive the following (adjoint related) identity Given a ϕ ∈ R3 , the exponential map to a unit quaternion
(see Appendix II-B): is given by:
exp(Φ(ϕ)) = Φ ◦ exp(ϕ) ◦ Φ−1 . (24)
 
ϕ
exp(ϕ) = (q0 , q̌) = cos(kϕk/2), sin(kϕk/2)
Useful identities can be derived for derivatives involving kϕk
orientations (see Appendix I): (38)
exp(ϕ) ≈ (1, ϕ/2), (kϕk ≈ 0). (39)
∂/∂t (ΦBA (t)) = −B ω AB (t), (25)
∂/∂r (Φ(r)) = C(Φ), (26) The above small angle approximation is required to avoid
× numerical instabilities (typically for angles below 10−4 rad).
∂/∂Φ (Φ(r)) = − (Φ(r)) , (27) The corresponding logarithm is given by:
∂/∂Φ Φ−1 = −C(Φ)T ,


(28) q̌
log(Φ) = 2 atan2(kq̌k, q0 ) , (40)
∂/∂Φ1 (Φ1 ◦ Φ2 ) = I, (29) kq̌k
∂/∂Φ2 (Φ1 ◦ Φ2 ) = C(Φ1 ), (30) log(Φ) ≈ sign(q0 ) q̌, (kq̌k ≈ 0). (41)
∂/∂ϕ (exp(ϕ)) = Γ(ϕ), (31)
−1 D. Consistency Tests
∂/∂Φ (log(Φ)) = Γ (log(Φ)). (32)
The consistency of the implementation can be tested
The derivative of the exponential map is given by the
through the following unit tests:
Jacobian Γ(ϕ) ∈ R3×3 which has the following analytical
expression: Φ() C ◦ exp log
2 C(Φ)r = Φ(r)
(1 − cos(kϕk))ϕ× (kϕk − sin(kϕk))ϕ×
Γ(ϕ) = I + + , (Φ1 ◦ Φ2 )(r) = Φ1 (Φ2 (r))
kϕk2 kϕk3 C(exp(ϕ)) = C(ϕ)
(33)
Φ = exp(log(Φ))
Γ(ϕ) ≈ I + 1/2ϕ× , (kϕk ≈ 0). (34)
On the right-hand side the involved operators are listed. The
V. Q UATERNION I MPLEMENTATION third test compares against Rodriguez’ formula (eq. (21)).
Theoretically, these tests should be carried out for all possible
The above discussion is completely decoupled from any
values of Φ, Φ1 , Φ2 ∈SO(3), r, ϕ∈R3 . In practice, testing
actual orientation parameterization. It is valid whether Euler-
various samples, including very small angles, should be
angles, rotation matrices, quaternions, or other representa-
sufficient.
tions are employed. In the following we provide one possible
implementation of 3D orientations along with the means VI. S IMPLE M ODELING E XAMPLE
to check its correctness. Here we propose the use of unit
quaternions following the Hamilton convention [5] and we This section presents how to apply the above notation and
discuss the implementation of the different operations that convention to an actual system modeling task. We want to
are required. For more details on the differences between estimate the position, velocity (expressed in B to simplify
existing quaternion conventions we refer the reader to [6]. the Jacobians), and orientation of a robot using an IMU and
A unit quaternion is composed of a real part, q0 ∈ R, and a generic position and orientation sensor (pose sensor). To
an imaginary part, q̌ ∈ R3 , which meet q02 + kq̌k2 = 1. We avoid complicated modeling or specific knowledge about the
denote this as Φ = (q0 , q̌). motion model the IMU can be used to do a prediction of the
state. This is very common in visual-inertial state estimation
A. Coordinates Mapping and Rotation Matrix e.g. [7]. In the following, we first show how to use the IMU
For arbitrary coordinate systems, A and B, with relative for predicting the state and then show the necessary steps to
orientation ΦBA = (q0 , q̌) the coordinates of a vector ~r can perform an update with the pose sensor.
be mapped as:
A. Continuous Time Description
ΦBA (A r) = (2q02 − 1)A r + 2q0 q̌ ×A r + 2q̌(q̌ TA r). (35) Let us assume we have an IMU driven dynamic system
From this, we can directly derive the expression for the with inertial coordinate system I and IMU-fixed coordinate
associated rotation matrix: system B for which we wish to estimate the motion. Consid-
C(ΦBA ) = (2q02 − 1)I + 2q0 q̌ × + 2q̌q̌ T . (36) ering additive biases, B bf and B bω , and using continuous-
time white noise processes, B nf , B nω , B nbf , B nbω , we can
model the IMU measurements, B f̃ B and B ω̃ B , as: The noise is discretized such that, if Ri is the noise density
of the white noise process ni , then the discrete Gaussian
B f̃ B = Φ−1
IB (I aB − I g) + B bf + B nf , (42)
noise n̂i is distributed with N (0, Ri /∆t).
B ω̃ B = B ω IB + B bω + B nω , (43)
B ḃf = B nbf , (44) C. Differentiation
B ḃω = B nbω , (45) Using the identities (25)-(32) and applying the chain rule,
where I g is the gravity vector expressed in the inertial frame. the following Jacobians of the discrete process model can be
We add the IMU biases to the state x. This gives the full derived (F is w.r.t. the state, G is w.r.t. the process noise):
state by
−∆t ΦIB (B v B )×
 
I ∆t C(ΦIB ) 0 0
 × T ( g)× × 
0 I − ∆tω ∆t C(Φ IB ) I −∆t I −∆t v
B B
x = (I r IB , B v B , ΦIB , B bf , B bω ) . (46)

F = 0 ,
 
 0 I 0 −∆tC(ΦIB )Γ(∆tω)

0 0 0 I 0 
0 0 0 0 I
The resulting continuous-time equations of motion can be 
∆t C(ΦIB ) 0 0 0 0


written as: 

G = 
0 −∆tI −∆t(B v B ) × 0 0 
 0 0 −∆tC(ΦIB )Γ(∆tω) 0 0 ,
 0 0 0 ∆tI 0 
I ṙ IB = ΦIB (B v B + B nv ), (47) 0 0 0 0 ∆tI

−1


B v̇ B = /dt ΦIB (I v B )
d
D. Measurement

×
= Φ−1 −1
IB (I v̇ B ) − ΦIB (I v B ) C(ΦIB )T I ω BI For simplicity we assume a GPS position measurement
= Φ−1 ×
IB (I aB ) − B v B B ω BI I r̃ IB
and an orientation measurement Φ̃IB . The measure-
= ΦIB (I g) + B f IB − B ω ×
−1 ment equations are given by
IB B v B , (48)
Φ̇IB = −I ω BI = ΦIB (B ω IB ), (49) I r̃ IB = I r IB + I n̂p , (61)
B ḃf = B nbf , (50) Φ̃IB = ΦIB
I n̂Φ (62)
B ḃω = B nbω , (51) = exp(I n̂Φ ) ◦ ΦIB , (63)
with the bias and noise corrected proper acceleration and
with the discrete Gaussian measurement noise vectors I n̂p
angular velocity measurements
and I n̂Φ .
B f IB = B f̃ B − B bf − B nf , (52) Using the identities (29),(30),(31) and because the ex-
B ω IB = B ω̃ B − B bω − B nω . (53) pectation of the orientation measurement noise is zero, the
following Jacobians can be derived (H is w.r.t. the state, J
To derive (48) we used the product rule, followed by the is w.r.t. the update noise):
chain rule and the identities (25),(27),(28).  
I 0 0 0 0
B. Euler-Forward Discretization H= ,
0 0 I 0 0
One of the simplest and most commonly used discretiza- 
I 0

tion methods is Euler-Forward discretization. Other dis- J= ,
0 I
cretization schemes can of course also be employed. For
a time increment ∆t, Euler-Foward discretization of the E. Hints for the EKF Implementation
above formulation yields (the next state is denoted by a bar,
discretized noise by a hat): Now that we have derived all the required parts, the well
known EKF equations can be used to estimate the state. The
I r̄ IB = I r IB + ∆t ΦIB (B v B + B n̂v ), (54) only difference to the standard EKF is that we need to use
 
−1 × the operator for the innovation residual and the
operator
B v̄ B = B v B + ∆t ΦIB (I g) + f − ω B v B (55)
for updating the state estimate instead of normal addition and
Φ̄IB = ΦIB
(∆tΦIB (ω)) subtraction.
= exp(ΦIB (∆tω)) ◦ ΦIB
= ΦIB ◦ exp(∆tω) ◦ Φ−1 VII. C ONCLUSION
IB ◦ ΦIB
= ΦIB ◦ exp(∆tω), (56) This document derived and summarized the main identities
B b̄f = B bf + ∆tB n̂bf , (57) related to 3D orientations in robotics and other engineer-
ing fields. In particular it discussed a more abstract but
B b̄ω = B bω + ∆tB n̂bω , (58)
convention-less notion of 3D orientations, the boxplus and
with the discretized IMU measurements (bias and noise boxminus operators, as well as the concept of differentials.
corrected) given by Various differentials involving 3D orientations are derived,
which can be used to compute the Jacobians of more complex
f = B f̃ B − B bf − B n̂f , (59)
models by applying the chain rule. A simple modeling
ω = B ω̃ B − B bω − B n̂ω . (60) example shows how to apply the introduced concepts.
 A PPENDIX I 3) Derivative of Coordinate Map: The map of an ori-
D ERIVATIVES I NVOLVING O RIENTATIONS entation applied to a coordinate tuple can be differentiated
1) Time Derivative of Orientation: Here we need the kine- w.r.t. the orientation itself. This yields (used identities:
matic concept of angular velocities. We assume the existence (20),(14),(5),(22)):
of an inertial observer I which observes the motion, over a
 
∂ (Φ
ei )(r) − Φ(r)
duration , of a moving coordinate system B(t). We use the Φ(r) = lim
∂Φ i
→0 
following definition of angular velocities (the negative sign
C(ei )C(Φ)r − C(Φ)r
is required so that the angular velocity corresponds to the = lim
→0 
active rotation which is measured by typical IMU devices):
(I + e×i )C(Φ)r − C(Φ)r
B(t) ϕB(t+)B(t) = lim
B(t) ω IB(t) := − lim (64) →0 
→0  e× C(Φ)r
Additionally we require the limit (based on the limits = lim i
→0 
(39),(41)): = − (C(Φ)r)× ei .
log(exp(ϕ1 ) ◦ exp(ϕ2 )) ∂
lim = ϕ1 + ϕ2 . (65) Φ(r) = − (C(Φ)r)× . (68)
→0  ∂Φ
With this we can derive the derivative of an 4) Concatenation - Left: The concatenation of two orien-
orientation ΦB(t)A(t) w.r.t. time t (used identities: tations can be differentiated w.r.t. the involved orientations.
(19),(15),(23),(24),(64),(65)): We first derive the derivative w.r.t. the left orientation (used
∂ ΦB(t+)A(t+) ΦB(t)A(t) identities: (19),(20),(15),(14)):
ΦB(t)A(t) = lim  
∂t →0  ∂ ((Φ1
ei ) ◦ Φ2 ) (Φ1 ◦ Φ2 )
1  Φ 1 ◦ Φ 2 = lim
= lim (ΦB(t+)B(t) ◦ ΦB(t)A(t) ◦ ΦA(t)A(t+) ) ∂Φ 1 i
→0 
→0 
 log(exp(ei ) ◦ Φ1 ◦ Φ2 ◦ Φ−1 −1
2 ◦ Φ1 )
ΦB(t)A(t) = lim
→0 
1 = ei .
= lim log(ΦB(t+)B(t) ◦ ΦB(t)A(t)
→0  ∂
 Φ1 ◦ Φ2 = I. (69)
◦ ΦA(t)A(t+) ◦ Φ−1B(t)A(t) ) ∂Φ1
1 5) Concatenation - Right: The derivative of the con-
= lim log(exp(B(t) ϕB(t+)B(t) ) ◦ ΦB(t)A(t)
→0  catenation w.r.t. the right orientation yields (used identities:

−1
◦ exp(A(t) ϕA(t)A(t+) ) ◦ ΦB(t)A(t) ) (19),(20),(15),(14),(24)):
 
1  ∂ (Φ1 ◦ (Φ2
ei )) (Φ1 ◦ Φ2 )
= lim log(exp(B(t) ϕB(t+)B(t) ) Φ1 ◦ Φ2 = lim
→0  ∂Φ 2 i
→0 

◦ exp(B(t) ϕA(t)A(t+) )) log(Φ1 ◦ exp(ei ) ◦ Φ2 ◦ Φ−1 −1
2 ◦ Φ1 )
= lim
1 →0 
= lim log(exp(− B(t) ω IB(t) ) log(exp(Φ1 (ei )))
→0  = lim
 →0 
◦ exp( B(t) ω IA(t) )) = Φ1 (ei ) = C(Φ1 )ei .
= − B(t) ω IB(t) + B(t) ω IA(t) ∂
Φ1 ◦ Φ2 = C(Φ1 ). (70)
= − B(t) ω A(t)B(t) (66) ∂Φ 2

2) Derivative of Inverse: Here we derive the deriva- 6) Exponential Derivative: Define:

tive of the inverse of an orientation (used identities: Γ(ϕ) := ∂/∂ϕ (exp(ϕ)) . (71)
(19),(20),(15),(14),(24)):

∂ −1

(Φ
ei )−1 Φ−1 Differentiate the adjoint related identity using the chain rule
Φ = lim and product rule (identities (71),(27) for left side, identities
∂Φ i
→0 
−1
(29),(30),(28) for right side):
log(Φ ◦ exp(−ei ) ◦ Φ)
= lim h i
→0  ∂/∂Φ exp(Φ(ϕ)) = Φ ◦ exp(ϕ) ◦ Φ−1 , (72)
log(exp(−Φ−1 (ei ))
= lim −Γ(Φ(ϕ))Φ(ϕ)× = I − C(Φ)C(ϕ)C(Φ)T . (73)
→0 
= − Φ−1 (ei ) = −C(Φ)T ei . Set Φ to identity:
∂ −1
Φ = − C(Φ)T . (67) Γ(ϕ)ϕ× = C(ϕ) − I. (74)
∂Φ
Now consider the map f (x) = exp(xϕ) for some arbitrary B. Concatenation and Exponential – Adjoint Related
ϕ ∈ R3 . The chain rule yields f 0 (x) = Γ(xϕ)ϕ. Alter- We want to prove the following identity:
natively, it can be differentiated using the limit (19) (used
identities: (8),(15)): exp(Φ(ϕ)) = Φ ◦ exp(ϕ) ◦ Φ−1 . (88)

exp((x + )ϕ) exp(xϕ) Since we know that exp is unique it is sufficient to show that
f 0 (x) = lim the right hand side is indeed the exponential of Φ(ϕ) and
→0 
log(exp(ϕ) ◦ exp(xϕ) ◦ exp(xϕ)−1 ) thus check the defining properties. First we verify eq. (12):
= lim
→0  exp((t + s)Φ(ϕ)) (89)
= ϕ. (75) −1
= Φ ◦ exp((t + s)ϕ) ◦ Φ (90)
Compare both derivatives at x = 1: = Φ ◦ exp(tϕ) ◦ exp(sϕ) ◦ Φ−1 (91)
−1 −1
= Φ ◦ exp(tϕ) ◦ Φ ◦ Φ ◦ exp(sϕ) ◦ Φ (92)
Γ(ϕ)ϕ = ϕ. (76)
= exp(tΦ(ϕ)) ◦ exp(sΦ(ϕ)). (93)
This can be combined with eq. (74) in order to obtain the
Equation (13) poses a requirement on the derivative which
following matrix equation:
can also be verified:
Γ(ϕ) ϕ× ϕ = C(ϕ) − I ϕ .
   
(77) d/dt (exp(tΦ(ϕ))(v)) | (94)
t=0
T



Right multiply with ϕ×
 T
ϕ and simplify: = d/dt C(Φ) exp(tϕ)(C(Φ) v) |t=0 (95)
× T
2
= C(Φ)ϕ C(Φ) v (96)
Γ(ϕ)(−ϕ× + ϕϕT ) = (I − C(ϕ))ϕ× + ϕϕT , (78) ×
= (C(Φ)ϕ) v = Φ(ϕ) × v. (97)
Γ(ϕ)kϕk2 = (I − C(ϕ))ϕ× + ϕϕT , (79) −1
Since Φ◦exp(ϕ)◦Φ fulfills both uniquely defining proper-
(I − C(ϕ))ϕ× + ϕϕT ties of the exponential it is indeed equivalent to exp(Φ(ϕ)).
Γ(ϕ) = . (80)
kϕk2
R EFERENCES
If substituting C(ϕ) we obtain eq. (33).
[1] T. Drummond, “Lie groups, Lie algebras, projective geometry and
optimization for 3D Geometry, Engineering and Computer Vision,”
A PPENDIX II http://twd20g.blogspot.ch/p/notes-on-lie-groups.html, 2014, [Online].
OTHER P ROOFS [2] E. Eade, “Lie groups for Computer Vision,” http://http://ethaneade.com/,
2014, [Online].
A. Rodriguez’ Formula [3] T. D. Barfoot, State Estimation for Robotics: A Matrix Lie Group Ap-
proach. Draft in preparation for publication by Cambridge University
From eqs. (5), (12) and (13) we obtain the following Press, 2016.
[4] C. Hertzberg, R. Wagner, U. Frese, and L. Schröder, “Integrating
properties for C(ϕ) = C(exp(ϕ)), ∀t ∈ R, ϕ, v ∈ R3 : generic sensor fusion algorithms with sound state representations
through encapsulation of manifolds,” Information Fusion, vol. 14,
C((t + s)ϕ) = C(tϕ)C(sϕ) (81) no. 1, pp. 57–77, 2011. [Online]. Available: http://www.sciencedirect.
d/dt (C(tϕ)(v)) |
com/science/article/pii/S1566253511000571
t=0 =ϕ×v (82) [5] W. R. Hamilton, “On quaternions; or on a new system of imaginaries in
algebra,” The London, Edinburgh, and Dublin Philosophical Magazine
For a given ϕ we define the curve C ϕ (t) := C(tϕ). Using and Journal of Science, vol. 25, no. 163, pp. 10–13, 1844.
a change of coordinate t = s + r, we can extend the range [6] J. Sola, “Quaternion kinematics for the error-state kf,” Laboratoire
dAnalyse et dArchitecture des Systemes-Centre national de la recherche
of the differential identity ∀t ∈ R, v ∈ R3 : scientifique (LAAS-CNRS), Toulouse, France, Tech. Rep, 2012.
[7] M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust visual
d/dt (C(tϕ)v) = d/ds (C(sϕ)C(rϕ)v) |s=0,r=t (83) inertial odometry using a direct ekf-based approach,” in Intelligent
Robots and Systems (IROS), 2015 IEEE/RSJ International Conference
= ϕ× C(tϕ)v (84) on. IEEE, 2015, pp. 298–304.

Thus, we obtain the following matrix differential equation:

d/dt (C
ϕ (t)) = ϕ× C ϕ (t), (85)

which has the matrix exponential solution

×
C ϕ (t) = etϕ . (86)

Since this is valid for arbitrary ϕ, we obtain:

×
C(ϕ) = eϕ , (87)

which can be shown to be the same as eq. (21) using series

expansions.