Position Overview

A fully funded PhD position is open at the Australian Centre for Robotics (ACFR) at the University of Sydney! The topic is on geometric techniques in the estimation and control of aerial robotic systems and will be supervised by Dr Pieter van Goor.

Aerial robotic system hardware has become increasingly capable of high precision tasks involving interaction with their environment, including delivery, maintenance, and (physical) inspection. However, outside of laboratory environments, many of these tasks require human control or intervention due to the sensitivity of automatic control algorithms to the accuracy of on-board measurements.

The aim of this project is to develop new estimation and control algorithms that are faster and more robust than the state of the art, and that can unlock the hardware’s full capability in real-world outdoor environments. Emerging theoretical techniques that exploit the mathematical symmetry of aerial robot dynamics provide a solution, but they are yet to be implemented in real-world systems. Depending on the candidate’s skills and interests, there are opportunities to explore:

• Hardware implementations and testing of new control and estimation algorithms.
• Theoretical development of symmetry-based approaches.
• High-speed estimation using novel sensors (e.g. cameras, LIDAR, UWB range, etc.).

The Australian Centre for Robotics (ACFR) is a part of the School of Aerospace, Mechanical, and Mechatronics Engineering (AMME). The ACFR offers specialised labs and facilities, robotic platforms, and robotic field labs on-campus and in nearby off-campus sites. You will have access to mechanical and electronics workshops and a pool of technical staff to help realise your research ambitions. The University of Sydney offers a rich academic setting in a world class city, and the ACFR has strong ties to a network of nearby and international academic and industrial collaborators.

Desirable Qualities in a Candidate

• Bachelor’s degree with honours or master’s degree in a relevant discipline, or evidence that a relevant degree will be obtained within the next six months.
• Interest in robotics and control.
• Strong background in mathematics and programming.
• Excellent communication and interpersonal skills.
• Hands-on experience with (aerial) robotic platforms, ROS, Python, C++.
• Creativity, curiosity, rigour and passion.

How to Apply

Your application must include

• A cover letter (at most 1 page in length) stating your prior experience and research interest in relation to the topic of geometric methods for aerial robotics.
• A curriculum vitae with three references and a GPA.
• A transcript of bachelor’s and master’s degree grades (if applicable).

Please send an email to pieter.vangoor@sydney.edu.au with the title [Aerial Robotics PhD Application] to submit your application or if you would like further information. The deadline for applications is Sunday 29 March 2026.

Introduction

The special Euclidean group \(\mathbf{SE}(3)\) described the symmetry of rigid bodies and frames of reference. Aside from describing their symmetry, it can also be used to represent the position and orientation of a frame in one handy matrix. While the elements of this group can be viewed as consisting of a rotation matrix and a position vector, most people prefer to use the \(4\times 4\) matrix form, sometimes called a homogeneous matrix. Like my other articles on Lie groups, my goal here is to provide derivations of some of the most useful and important formulas and identities that come up for \(\mathbf{SE}(3)\). That said, the previous post on the special orthogonal group should be read before reading this one, since I will draw on a number of the results therein. For an excellent explanation of the relationships between frames of reference, their velocities, and the special Euclidean group, I recommend this blog post by Shiraz Khan.

The special Euclidean group is defined by

On first impressions, this looks as simple as just combining \(\mathbf{SO}(3)\) with \(\mathbb{R}^3\), but things get interesting when we look at the multiplication of two \(\mathbf{SE}(3)\) elements. We will typically use the notation \(P \in \mathbf{SE}(3)\) to refer to the whole matrix, and the notations \(R \in \mathbf{SO}(3), x \in \mathbb{R}^3\) to refer to the rotation and translation components, respectively. Given \(P_1, P_2 \in \mathbf{SE}(3)\), their product is given by

Note that the rotation components \(R_1,R_2\) are simply multiplied together, and their product is not affected by the translation component. On the other hand, the translation components \(x_1,x_2\) are not simply summed together, rather, the second translation vector \(x_2\) is rotated by \(R_1\) before being added to \(x_1\). This property, that one component of the product is affected by the other component, but not vice versa, is why \(\mathbf{SE}(3)\) is called a semi-direct product of \(\mathbf{SO}(3)\) and \(\mathbb{R}^3\), which is sometimes written as \(\mathbf{SE(3)} = \mathbf{SO(3)} \ltimes \mathbb{R}^3\).

The group properties are easily verified. The identity matrix \(I_4\) lies in \(\mathbf{SE}(3)\), with its rotation given by \(I_3\) and its translation vector given by \(0_{3\times 1}\). For matrix inversion, examining the product leads to the conclusion that

This formula yields another element of \(\mathbf{SE}(3)\), and we should note that it is entirely expressed in terms of sums and products of the original matrix components. This is useful, since it is usually much easier and computationally cheaper to take sums and products of vectors and matrices than it is to invert them. All of this shows that \(\mathbf{SE}(3)\) is indeed a group.

Lie algebra

The Lie algebra \(\mathfrak{se}(3)\) is obtained by differentiating the defining conditions of the Lie group \(R \in \mathbf{SO}(3)\) and \(x \in \mathbb{R}^3\) near the identity,

Recall that \(\mathfrak{so}(3)\) is the Lie algebra of \(\mathbf{SO}(3)\), defined by the property that \(\omega^\times \in \mathfrak{so}(3)\) if and only if \((\omega^\times)^\top = - \omega^\times\). Recall also that the skew map \(\cdot^\times : \mathbb{R}^3 \to \mathbb{R}^{3\times 3}\) such that \(\omega^\times\) is the unique matrix for which \(\omega^\times a = \omega \times a\) for all vectors \(a\in \mathbb{R}^3\), that is,

From here, we define the basis of $\mathfrak{se}(3)$ to be

Having defined this basis, we can also write down the wedge map \(\cdot^\wedge : \mathbb{R}^6 \to \mathfrak{se}(3)\). For convenience of notation, we will typically write our elements of \(\mathbb{R}^6\) as consisting of two concatenated \(\mathbb{R}^3\) vectors. Specifically, given \(\omega, v \in \mathbb{R}^3\), the wedge map is given by

The final compact form is particularly useful, but if it ever looks confusing, just remember that it is only a convenient way to write things down. The object we are describing here is ultimately a \(4 \times 4\) matrix, even if it has an elegant \(2 \times 2\) block matrix structure.

Adjoint and Lie bracket

To study the adjoint maps, we will denote arbitrary elements by \(P \in \mathbf{SE}(3)\) and \(U \in \mathfrak{se}(3)\). The rotation and translation components of \(P\) are written \(R\) and \(x\), respectively, while the rotation and translation components of \(U\) are written \(\omega^\times\) and \(v\), respectively. Then, the adjoint operator is defined by

Here, we have used a few cross-product identities, that were derived in the post on \(\mathbf{SO}(3)\). From simply reading the entries of the result, the matrix form of the Adjoint operator is found to be

The little adjoint matrix and the Lie bracket are easily obtained by differentiating \(\mathrm{Ad}_P^\vee\) with respect to \(P\) at the identify \(I_4\),

As always, the Lie bracket can also be obtained by computing the matrix commutator \([U_1, U_2] = U_1 U_2 - U_2 U_1\). It is useful to be aware of these equivalences so that you can pick the technique you find easiest in your situation.

Exponential and Logarithm

The exponential and logarithm of \(\mathbf{SE}(3)\) admit elegant terms involving trigonometric expressions, similarly to \(\mathbf{SE}(2)\). Recall from the previous post on \(\mathbf{SO}(3)\) that, for any \(\omega \in \mathbb{R}^3\),

Now consider an arbitrary element \(U \in \mathfrak{se}(3)\) with rotational and translational components \(\omega\) and \(v\). Looking at the powers of \(U\), we find

From here we can prove the pattern by induction. Assume that

for a given \(n \geq 2\). Then

Since the formula holds for \(n = 2\), we have therefore shown that it holds for all \(n \geq 2\) by induction. From here we can compute the matrix exponential explicitly. By definition,

Let \(P = \exp(U)\) denote the result, with rotation and translation terms \(R \in \mathbf{SO}(3)\) and \(x \in \mathbb{R}^3\), respectively. Then \(R = \exp(\omega^\times)\), which is simply the \(\mathbf{SO}(3)\) exponential computed previously. To determine \(x = \left( \sum_{n=1}^\infty \frac{1}{n!} (\omega^\times)^{n-1} \right) v\), we first define \(M(\omega) = \sum_{n=1}^\infty \frac{1}{n!} (\omega^\times)^{n-1}\) and then attempt to simplify it using trigonometric power series expansions. Assume for now that \(\omega \neq 0\). Then

In summary, the full exponential is given by

In the case where \(\omega = 0\), the result is simply

The logarithm is simply the inverse of the exponential, so suppose that \(\exp(U) = P\). Then \(\exp(\omega) = R\), which we already know how to solve using

for \(\vert \omega \vert \in (0, \pi)\). In this case, we have that \(x = M(\omega) v\), which is solved by \(v = M(\omega)^{-1} x\), as long as \(M(\omega)\) is invertible. When is \(M(\omega)\) invertible? Rather, if \(M(\omega)\) is not invertible, then there must be a vector \(v\) for which \(M(\omega) v = 0\). This would mean that

If \(v\) is not parallel to \(\omega\), then the second term \(\frac{ 1 - \cos(\vert \omega \vert) }{ \vert \omega \vert^2 } \omega^\times v\) is non-zero and is also orthogonal to the other terms, so the full expression cannot be zero. Concretely, we can premultiply both sides by \((\omega^\times v)^\top\) to obtain

This is zero if and only if \(\frac{ 1 - \cos(\vert \omega \vert) }{ \vert \omega \vert^2 } \vert \omega^\times v \vert^2 = 0\), which can only occur if \(\omega^\times v = 0\). Considering the case that \(v = a \omega\) for some scalar \(a\), then we would require

which is only solved by \(a \omega = 0\). What this all means is that \(M(\omega)\) is always invertible, no matter the value of \(\omega\). Thus the full logarithm is given by

Conclusion

The 3D special Euclidean group is an important Lie group to understand for robotics, where it is used to describe transformations between frames of reference of different rigid bodies. The goal of this post is not to explain this application, but rather, to go into detail of how the important formulas for \(\mathbf{SE}(3)\) can be derived. If you want to look at an implementation of some of the functions I have described, I suggest you to look at pylie. Remember that a lot of the formulas here are derived from the formulas for \(\mathbf{SO}(3)\), which you can find in my previous post. Finally, I will leave a summary of the formulas below for quick reference.

Quick Reference

The 3D special Euclidean group and its Lie algebra

The Lie algebra wedge map

Adjoint matrices

Exponential formula (for \(\omega \neq 0\))

Logarithm formula (for \(R \neq R^\top\))

Introduction

This post is inspired by a PhD student who emailed me for help in deriving the \(C\) and \(C^\star\) matrices in the Equivariant Filter equations. The Equivariant Filter (EqF) is an approach to state estimation of systems with Lie group symmetries (see the paper here: https://arxiv.org/abs/2010.14666). Such systems are very common in robotics and aerospace, particularly due to the need to consider 3D rotations. In this post, I will explain in detail the example presented also in the paper, which is the problem of estimating a single bearing. The main purpose is to expand some of the derivations from the original paper, and to provide an example of how to do practical computations with manifolds and Lie groups.

Problem Description and Set-up

Before we can apply the EqF methodology, we need to understand the problem we are studying. We consider a robot (e.g. a quadrotor) equipped with a gyroscope and a magnetometer. The gyroscope measures the angular velocity \(\Omega \in \mathbb{R}^3\) of the robot and the magnetometer measures the direction of the Earth’s magnetic field \(\eta \in S^2\). Both measurements are taken in the body-fixed frame of the robot. The kinematics of the magnetic field direction can then be expressed as

where \(\Omega^\times \in \mathbb{R}^{3\times 3}\) is the skew-symmetric matrix defined by

The measurement function is simply \(y = h(\eta) = c_m\eta\), where \(c_m > 0\) is the magnetic field strength.

With this set-up out of the way, we can start to apply the EqF methodology. We will follow the same steps as in the paper, but I will give a bit more detail at each step.

EqF Design Procedure

State Space Symmetry

To design an EqF, the first and most fundamental ingredient is a Lie group that acts transitively on the state space of the system we are studying. In this case, the state space is the sphere \(S^2\), and a natural choice for the Lie group is the set of 3D rotations, \(\mathbf{SO}(3)\). This is a matrix Lie group and is defined by

This group has a right-handed group action on \(S^2\) defined by

To verify that this is a group action, we just need to check that the identity and compatibility properties are satisfied:

In other words, the group action applied with the identity matrix does nothing, and the group action applied twice with different group elements is the same as the group action applied once with the product of those elements. A group action is called transitive if, for any \(\eta_1, \eta_2 \in S^2\), there exists a matrix \(R \in \mathbf{SO}(3)\) such that \(\phi(R, \eta_1) = \eta_2\). This is certainly the case here, although the proof is a little bit more detailed then I would like to include in this post.

Equivariant Lift

The lift of the system is a map from the inputs and state of the system to the Lie group we have chosen. It enable the trajectories of the system to be replicated on the Lie group. The lift is a map \(\Lambda : S^2 \times \mathbb{R}^3 \to \mathfrak{so}(3)\). That is, it is a map from the state space and the input space to the Lie algebra of our chosen symmetry group. To ensure that trajectories generated on the group by the lift will match trajectories on the original state space, we need to verify the lift condition:

To find a solution to this, we evaluate the left-hand side of the lift condition to

The last line is due to the fact that \(\Lambda(\eta, \Omega) \in \mathfrak{so}(3)\). Now the lift condition is simplified to

The general solution to this is given by

where \(\alpha(\eta, \Omega) \in \mathbb{R}\). For simplicity, we will choose \(\alpha \equiv 0\), so that the lift becomes

for all \(\eta \in S^2\) and \(\Omega \in \mathbb{R}^3\). It is worth noting that this is quite a special case. It is not always possible to find a lift independent of the state \(\eta\), but if it is then this is usually desirable since it greatly simplifies some of the later computations.

Output Equivariance

Sometimes the measurement function \(h\) associated with a system exhibits equivariance with respect to the action \(\phi\) on the state space. To find out whether this is the case, we can evaluate \(h(\phi(R, \eta))\) and see if it is possible to express the result in terms of \(R\) and \(h(\eta)\):

So, indeed, the measurement function \(h\) is compatible with the group action \(\phi\). Formally, this is expressed as another group action on the output space:

It is not required for this group action to be transitive, but it should also be right-handed. Output equivariance is desirable because of the ability to use it in deriving the \(C^\star\) matrix, which improves filter performance.

Origin and State Error

Implementing the EqF requires us to choose an origin on the state space \(S^2\). This choice is arbitrary, although it is generally useful to pick something easy to work with. For this example, we choose the origin to be the unit vector \(\mathbf{e}_1 = (1,0,0) \in S^2\).

Next, we need to choose a subspace of the Lie algebra \(\mathfrak{so}(3)\) to define normal coordinates on the manifold. The reason this is needed is because the Lie group \(\mathbf{SO}(3)\) is larger (in dimension) than the state space \(S^2\), and the normal coordinates are a way to choose a two dimensional subset of \(\mathbf{SO}(3)\) that represents the state space \(S^2\) (at least near the chosen origin \(\mathbf{e}_1\) ). The subspace we choose will be labelled \(\mathfrak{m} \subset \mathfrak{so}(3)\), and it must satisfy \(\mathrm{D}_R|_I \phi(R, \mathbf{e}_1)[\mathfrak{m}] = \mathrm{T}_{\mathbf{e}_1}S^2\). In other words, the differential map \(\mathrm{D}_R|_I \phi(R, \mathbf{e}_1) : \mathfrak{so}(3) \to \mathrm{T}_{\mathbf{e}_1}S^2\) must be full rank when constrained to \(\mathfrak{m}\). For any \(\Omega^\times \in \mathfrak{so}(3)\), we have

This is nonzero as long as \(\Omega\) is not a scalar multiple of \(\mathbf{e}_1\). Thus, to define \(\mathfrak{m}\) it suffices to choose a basis of two linearly independent elements \(\Omega_1^\times, \Omega_2^\times \in \mathfrak{so}(3)\) such that \(\Omega_1, \Omega_2\) are not scalar multiples of \(\mathbf{e}_1\). A very simple choice is then to select the unit vectors \(\Omega_1 = \mathbf{e}_2, \Omega_2 = \mathbf{e}_3\), so that \(\mathfrak{m}\) is defined by

Having made this choice, we define the normal coordinates \(\vartheta : \mathcal{U} \subset S^2 \to \mathcal{V} \subset \mathbb{R}^2\) for \(S^2\) about \(\mathbf{e}_1\) by

While \(\vartheta\) looks complicated, \(\vartheta^{-1}\) is very simple in terms of the Lie group exponential and the group action \(\phi\). Also, while it can be useful for analysis, the expressions for \(\vartheta\) and \(\vartheta^{-1}\) are not strictly necessary for implementation. Only the derivatives at \(e=\mathbf{e}_1\) and \(\varepsilon = 0_{2\times 1}\), respectively, are required. The derivative of \(\vartheta^{-1}\) is computed by letting \(\varepsilon\) be arbitrary and computing the directional derivative

In other words, we can express the differential \(\mathrm{D} \vartheta^{-1}(0)\) as the \(3\times 2\) matrix shown. The differential \(\mathrm{D} \vartheta(\mathbf{e}_1)\) can be obtained as the left-inverse of \(\mathrm{D} \vartheta^{-1}(0)\), i.e.

which satisfies \(\mathrm{D} \vartheta(\mathbf{e}_1) \cdot \mathrm{D} \vartheta^{-1}(0) = I_2\).

EqF Matrix Computation

The EqF matrices \(\mathring{A}_t, B_t, C_t, C^\star_t\) can now be computed by specialising their formulas.

A Matrix

Beginning with \(\mathring{A}_t\), we use the formula (51) in the paper [EqF]:

This formula can be intimidating, but in our case it simplifies greatly due to the fact that the lift \(\Lambda\) is independent of \(\eta\). This means that the fourth term in the formula, \(\mathrm{D}_\eta|_{\hat{\eta}} \Lambda(\eta, \Omega)\), is zero and therefore the whole matrix \(\mathring{A}_t\) is zero as well.

B Matrix

The \(B_t\) matrix is used in tuning the filter gains (i.e. the covariance of the process noise) and is given by formula (42) in [EqF]:

There are many ways to perform this computation. I will demonstrate here the method using an arbitrary input vector \(\mu\). Let \(\mu \in \mathbb{R}^3\) and compute:

To avoid this computation from becoming extremely long, we can work in stages starting from the right-most term. In other words, we start by working out \(\mathrm{D}_\Omega|_{\Omega_m} \Lambda(\hat{\eta}, \Omega)[\mu]\) and then carry on to the next term until we reach the end:

Now we compute the formula up to the second last term:

We carry on like this for the next term:

Note that I could have stopped at the second last line, but I chose to rewrite the expression with \(\mu\) at the front. This extra step is helpful in simplifying everything at the end. With these computations, we arrive at the final step:

Since this all applies for an arbitrary direction \(\mu\), we obtain the resulting \(B_t\) matrix, which is

C Matrix

The \(C_t\) matrix can be obtained in a similar fashion to the \(B_t\) matrix. We first recall formula (33) from [EqF], specialised to the notation we are using for this example:

Then, we can compute \(C_t \varepsilon\) for an arbitrary \(\varepsilon \in \mathbb{R}^2\), although this time we will do it without breaking it up into parts.

Alternative expressions can be found by further manipulations. We have that

Each of these expressions is equally valid, although the last one is most closely related to the \(C_t^\star\) matrix we will derive next.

C* Matrix

When a system exhibits output equivariance, we can replace the \(C_t\) matrix with the \(C_t^\star\) matrix to obtain improved performance. The formula is given by (35) in [EqF]. For any \(\varepsilon \in \mathbb{R}^2\),

Note that this formula relies on the output action \(\rho\), and also involves the measurement \(y\) taken from the real system. This ‘additional information’ is what leads to the performance advantage from using \(C_t^\star\) over \(C_t\).

To compute it \(C_t^\star\), we follow similar steps as when computing the \(B_t\) matrix. First,

Now consider an arbitrary \(\bar{y} \in \mathbb{R}^3\) and \(\Delta^\times \in \mathfrak{so}(3)\) and compute

Then it follows that

This gives us the formula for \(C^\star_t\) as

Summary

This post should be read as a supplement to the original Equivariant Filter (EqF) paper [EqF]. I have tried to provide some context so that things are relatively self-contained, but to understand the motivation behind everything I derived here, I strongly suggest reading through the paper. If there is anything unclear from my computations, however, or if you suspect a mistake, please reach out and let me know! I know that it can be overwhelming to start using the mathematics needed for dealing with Lie groups and manifolds, so I hope this post provides some help for understanding how to use it for computations.

I am very happy to announce that our new work on observer design for inertial navigation with GNSS and magnetometer measurements has been published in Automatica! An Inertial Navigation System (INS) is used to estimate the attitude, velocity, and position of a vehicle by combining measurements from an Inertial Measurement Unit (IMU) with other supporting sensors, such as GNSS, magnetometer, etc. The main idea behind this paper was to use the fact the dynamics of INS are “group-affine” to create a completely novel observer architecture, which could then be leveraged to design correction terms that guarantee almost-global asymptotic stability of the observer error. In other words, from practically any initial condition, the estimate provided by the observer we designed in this paper is guaranteed to converge to the true state of the system. This contrasts with existing methods, which either have a limited domain of convergence, or require high gains to increase the domain of convergence leading to high sensitivity to noise.

I would like to thank my coauthors Tarek Hamel and Robert Mahony for their contributions to this work. It really came about from a collaborative process of sharing ideas and identifying and overcoming difficulties together. Finally, if you are interested in this work or have questions, please don’t hesitate to reach out!

Paper: https://www.sciencedirect.com/science/article/pii/S0947358024001079

Preprint: https://arxiv.org/abs/2311.02234

Code: https://github.com/pvangoor/synchronous_INS

Abstract: Inertial Navigation Systems (INS) estimate a vehicle’s navigation states (attitude, velocity, and position) by combining measurements from an Inertial Measurement Unit (IMU) with other supporting sensors, typically including a Global Navigation Satellite System (GNSS) and a magnetometer. Recent nonlinear observer designs for INS provide powerful stability guarantees but do not account for some of the real-world challenges of INS. One of the key challenges is to account for the time-delay characteristic of GNSS measurements. This paper addresses this question by extending recent work on synchronous observer design for INS. The delayed GNSS measurements are related to the state at the current time using recursively-computable delay matrices, and this is used to design correction terms that leads to almost-globally asymptotic and locally exponential stability of the error. Simulation results verify the proposed observer and show that the compensation of time-delay is key to both transient and steady-state performance.

Introduction

The special orthogonal group \(\mathbf{SO}(3)\) is one of the most important Lie groups encountered in robotics. It is the most natural way to represent rotations and orientations of rigid bodies in 3D. Because of how important 3D rotations are, there are many ways to understand and approach them. I already wrote about one way in my previous post on unit quaternions, but in this post I will focus just on space of rotation matrices itself, and its Lie algebra. My main goal in writing this article is to provide derivations of some of the most important formulas when dealing with rotation matrices.

The special orthogonal group is defined by

The first condition \(R^\top R = I_3\) is equivalent to saying that each column of \(R\) must have length \(1\) and be orthogonal (perpendicular) to every other column. The second condition, \(\det(R)=1\) concerns the order of the columns of \(R\). It enforces a `right-handed’ orientation of each rotation matrix, and it is what makes them special orthogonal rather than simply orthogonal matrices.

We can verify the group properties as follows. The identity matrix clearly satisfies \(I^\top I=I\) and \(\det(I)=1\), so \(I \in \mathbf{SO}(3)\). As for matrix inversion, note that the first condition \(R^\top R = I_3\) means that \(R^{-1} = R^\top\). Therefore, for any \(R \in \mathbf{SO}(3)\), we have that \((R^\top)^\top (R^\top) = R R^{-1} = I_3\), and that \(\det(R^\top) = \det(R) = 1\). So the group is indeed closed under inversion. Finally, to see that the group is closed under matrix products, if \(R_1, R_2 \in \mathbf{SO}(3)\), then

Thus \(\mathbf{SO}(3)\) is indeed a group. What this means intuitively is that composing two or more rotations always leads to another rotation, and that any rotation can be undone by its inverse rotation.

Lie algebra

The simplest way of obtaining the Lie algebra \(\mathfrak{so}(3)\) is to differentiate the condition \(R^\top R = I_3\) around \(R \approx I + \Omega\). Doing so leads to

where all second-order terms have been removed. Differentiating the condition \(\det(R) = 1\) leads only to \(\mathrm{tr}(\Omega) = 0\), which is already guaranteed by the fact that \(\Omega + \Omega^\top = 0\). Hence, the Lie algebra of \(\mathbf{SO}(3)\) is

Next, we will define a basis for this Lie algebra. The condition \(\Omega^\top + \Omega = 0_{3\times 3}\) is exactly saying that \(\Omega\) is a ‘skew-symmetric’ matrix. Let us study this condition in terms of the coefficients of \(\Omega\). We have that

There are nine equations here, which can be reduced to

In other words, there are three degrees of freedom, or equivalently, the Lie algebra is a 3-dimensional vector space. We will choose the following basis for the Lie algebra:

Using these definitions, any element of the Lie algebra may be uniquely written as a vector \(\omega \in \mathbb{R}^3\) by defining the ‘skew map’ \(\cdot^\times : \mathbb{R}^3 \to \mathfrak{so}(3)\) as

We chose this particular basis and notation for a good reason, namely, that it relates the Lie algebra \(\mathfrak{so}(3)\) with the classis vector cross product. That is also why we use the notation \(a^\times\) rather than the notation \(a^\wedge\) used for other Lie algebras, although different authors have different conventions. The reason for these particular basis matrices is that if we have a vector \(a \in \mathbb{R}^3\), then the matrix \(a^\times \in \mathfrak{so}(3)\) is the unique matrix such that \(a \times b = a^\times b\) for all \(b \in \mathbb{R}^3\).

An Important Cross Product Identity

The following identity will be vital to understanding some of the later manipulations. Let \(a,b \in \mathbb{R}^3\) be arbitrary. Then we can compute that

This identification is extremely useful in manipulating expressions involving cross products and skew matrices. In the next section, we will use it in deriving the adjoint matrix.

Adjoint and Lie bracket

To study the adjoint maps, let \(R \in \mathbf{SO}(3)\) be arbitrary, and denote by \(R_1,R_2,R_3 \in \mathbb{R}^3\) the columns of \(R\). Then for the basis matrix \(E_1 = e_1^\times \in \mathfrak{so}(3)\),

The fact that \(R_2\times R_3 = R_1\) follows from the fact that \(R_1,R_2,R_3\) are orthogonal and that \(\det(R) = 1\). For the other basis vectors, the same process leads to \(\mathrm{Ad}_R(e_2^\times) = (Re_2)^\times\) and \(\mathrm{Ad}_R(e_3^\times) = (Re_3)^\times\). Therefore, for an arbitrary \(\omega^\times \in \mathfrak{so}(3)\),

So, while it may be some work to get there, we end up with a very clean result: the Adjoint applied to the skew matrix of a vector is the same as rotating the vector before applying the skew operator. Thus the matrix form of the Adjoint operator is simply

Differentiating this matrix in terms of the Lie group element \(R\) at the identity is an easy way to obtain the “little” adjoint matrix and, equivalently, the Lie bracket.

Lastly, this Lie bracket provides us with another useful skew operator identity, namely,

Exponential and Logarithm

Next are the exponential and logarithm, which also have nice forms. The exponential formula for \(\mathbf{SO}(3)\) is often referred to as the Rodrigues formula. To derive the exponential formula, we let \(\omega^\times \in \mathfrak{so}(3)\) be an arbitrary skew-symmetric matrix. Then we notice the following important identity:

A particular consequence of this is that

With this identity available to us, the exponential becomes relatively straightforward to compute. Assume first that \(\omega \neq 0\). Then, from the definition of the matrix exponential, we have

To see how the infinite sums collapsed, have a look at the power series expansions of sine and cosine. The fractions above are well-defined since we assumed \(\omega \neq 0\). In the case that \(\omega = 0\), then the exponential is simply \(\exp(0^\times) = I_3\).

Computing the logarithm is a matter of inverting the exponential formula, although we need to be careful since there is not always a unique logarithm defined! Suppose that \(R = \exp(\omega^\times)\) for some \(\omega \in \mathbb{R}^3\). Then by taking the trace of both sides,

We will assume that \(\vert \omega \vert < \pi\). Therefore, since \(\cos\) is invertible over the domain \([0,\pi]\), we obtain

Then, the vector direction of \(\omega\) can be extracted from the skew-symmetric part of \(R\). Observe that

Since we already know \(\vert \omega \vert\) from the trace formula, we now obtain

This formula applies when \(\vert\omega\vert \in (0, \pi)\), but what about the other cases? If \(\vert\omega\vert = 0\) then it simply means that \(R = I_3\) and thus \(\omega = 0\) as well. If, on the other hand, \(\vert\omega\vert = \pi\), then the logarithm is not uniquely defined. In this case, the skew-symmetric part of \(R\) disappears, and we must look to the symmetric part of \(R\) to find a solution. Under the condition that \(\vert \omega \vert = \pi\), we have

Multiplying both sides by \(\omega\) leads to

In other words, \(\omega\) is an eigenvector of \(R + R^\top\) with eigenvalue 2, and it is not difficult to see that it is unique up to change of sign. Therefore, if \(\vert \omega \vert = \pi\), then \(\omega\) is given by solving

and is unique up the choice of sign \(\pm \omega\).

Further Useful Formulas: Projections

We often use \(\mathbf{SO}(3)\) and its Lie algebra in their matrix form, and it is useful to be able to understand their relationship with the ambient space of matrices in which they are typically represented.

Lie Algebra Projection

For the Lie algbera \(\mathfrak{so}(3)\), the projection from \(\mathbb{R}^{3\times 3}\) to \(\mathfrak{so}(3)\) is often denoted as \(\mathbb{P}_{\mathfrak{so}}(3)\), and it is defined by

Here, the norm is taken as the matrix (Frobenius) norm, so \(\vert A \vert^2 := \mathrm{tr}(A^\top A)\). The solution to the minimisation is unique and can be obtained by differentiation. For any \(M \in \mathbb{R}^{3\times 3}\) and \(\Omega \in \mathfrak{so}(3)\), we write the cost function

Then, differentiating this with respect to \(\Omega\) in an arbitrary direction \(\Delta \in \mathfrak{so}(3)\) yields

This is zero for all \(\Delta \in \mathfrak{so}(3)\) exactly when \(\Omega = \frac{1}{2}(M - M^\top)\), which is also a skew symmetric matrix (as required). Therefore, to summarise,

The projection onto the Lie group \(\mathbf{SO}(3)\) is similarly defined, but less straightforward to obtain. For any \(M \in \mathbb{R}^3\),

To compute this, define \(l(R) = \frac{1}{2}\vert R - M \vert^2\) and differentiate to obtain

This is zero for all \(\Omega \in \mathfrak{so}(3)\) if and only if \(M^\top R = R^\top M\). Next, compute the singular value decomposition, \(M = U S V^\top\), where \(S\) is a diagonal matrix with the singular values of \(M\) in descending order. Then the previous condition is equivalent to

where \(Q = U^\top R V\). Note that \(Q\) is also an orthogonal (not necessarily special orthogonal matrix). We will now assume that the singular values of \(M\) are distinct, so the values along the diagonal of \(S\) are strictly descending \(s_1 > s_2 > s_3 \geq 0\). Then, for the unit vector \(e_1\),

Since \(Q\) is orthogonal and the \(s_i\) are descending, the sum on the left can be at most equal to \(s_1^2\) and this happens exactly when \(Q_{11} = \pm 1\) (meaning also that \(Q_{i1} = 0\) for \(i \neq 1\)). This moreover implies that \(Q_{1i} = 0\) for all \(i \neq 1\), and then the argument can be repeated for the second eigenvalue to obtain the result that

If \(Q\) were only orthogonal, then any of the eight combinations of signs would be possible. However, since \(Q = U^\top R V\), we have \(\det(Q) = \det(U^\top)\det(R) \det(V) = \det(U^\top)\det(V)\), since \(\det(R) = 1\) necessarily. Thus \(Q_{33}\) is determined by the other two values and the fixed determinant.

Returning to the original problem, expanding \(l\) yields

Since the \(s_i\) are descending, it is clear that the minimiser (among the possible \(Q\)) is given by \(Q = \mathrm{diag}(1,1,\det(U^\top) \det(V))\). Then, finally, \(R\) is recovered as \(R = U Q V^\top\), or succinctly,

This argument relied on the assumption that the singular values \(s_i\) were distinct, but a similar argument is possible when they are not. In any case, the procedure provided here will return a matrix \(R \in \mathbf{SO}(3)\) that minimises the Euclidean (Frobenius) distance to \(M\), although this matrix may not actually be a unique solution to the original problem.

Conclusion

The special orthogonal group in 3D is a beautiful and classical example of a Lie group, and it is of interest for both theoretical and practical reasons. My view is that, if you have a problem involving 3D rotations, your first choice should be to use the group \(\mathbf{SO}(3)\). It is sometimes worth using Euler angles or unit quaternions, but without some motivation for this, I would stick to the matrix Lie group. If you want to look at an implementation of some of the functions I have described, I suggest you to look at pylie. Finally, I will leave a summary of the formulas below for quick reference.

Quick Reference

The 3D special orthogonal group and Lie algebra

The Lie algebra wedge or ‘skew’ map

Cross product and skew matrix identities

Adjoint matrices

Exponential formula (for \(\omega \neq 0\))

Logarithm formula (for \(R \neq R^\top\))

Projection maps

Introduction

The indefinite orthogonal group \(\mathbf{O}(1,2)\) is the set of \(3\times 3\) matrices preserving the \((1,2)\) indefinite form. Specifically, define the indefinite \((1,2)\) form to be

Then the indefinite orthogonal group is defined to be

In applications, the indefinite form is important in studying the geometry of physics, where spacetime vectors have a structure like \((t,x,y,z)\). The relevant group in that case is \(\mathbf{O}(1,3)\), but the lower-dimensional \(\mathbf{O}(1,2)\) serves as a nice toy model. The elements of this group can also be thought of as `hyperbolic rotations’. This is because a hyperbola in \(\mathbb{R}^3\) can be written as a set \(H = \{ \xi \in \mathbb{R}^3 \; | \; \xi^\top g \xi = c \}\), where \(c\) is a constant. It follows that the indefinite orthogonal group preserves \(H\), much like how the standard orthogonal group preserves spheres.

The group properties are easily verified. The identity \(I_3\) is clearly in \(\mathbf{O}(1,2)\), and the matrix product and inverse preserve the group. Specifically, if \(L_1,L_2 \in \mathbf{O}(1,2)\), then

It is useful to dissect the definition of the Lie group a bit. Let

where \(A \in \mathbb{R}^{2\times 2}\) is the lower right block, \(d \in \mathbb{R}\), and \(b,c \in \mathbb{R}^2\). Then, by the definition of the group,

This leads to three equations,

For compactness of notation, it is not always useful to write out matrix \(L\) into these components, but the relationships are important for simplification of later formulas.

We will use these relations to compute the inverse of the matrix. The formula for the inverse of a \(2\times 2\) block matrix gives

Since the inverse of \(L\) also belongs to \(\mathbf{O}(1,2)\), we only need to compute the upper- and lower-left terms. The remaining terms are determined by the relationships outlined above. For the matrix \(S\), we have

Thus \(S^{-1} = A^\top\). This is a nice simplification and helps to compute the remaining terms. For the top-left term,

For the bottom-left term,

\(\begin{aligned} - d^{-1} S^{-1} c &= - d^{-1} A^\top c = -b. \end{aligned}\) Then by the fact that \((L^{-1})^{-1} = L\), the top-right term must be \(-c^\top\). In summary, the inverse of \(L\) is can be greatly simplified from a general \(3\times 3\) matrix inverse to

Lie algebra

The Lie algebra can be obtained by differentiating the condition \(L^\top g L = g\) at the identity. Computing this, we obtain

We will choose a basis for this Lie algebra in order to map between it and to \(\mathbb{R}^3\). To do this, let us decompose a Lie algebra element into its parts and check the condition. We have

where \(U_{11} \in \mathbb{R}\), \(U_{12} \in \mathbb{R}^{2\times 1}\simeq \mathbb{R}^2\), \(U_{21} \in \mathbb{R}^{1\times 2}\), and \(U_{22} \in \mathbb{R}^{2\times 2}\). It follows that

In other words, there are three degrees of freedom, and we define the basis of \(\mathfrak{o}(1,2)\) to be

For convenience, we will also adopt the notation

for any \(\omega \in \mathbb{R}\). Then we may write the wedge map \(\cdot^\wedge : \mathbb{R}^3 \to \mathfrak{o}(1,2)\) as

where \(u = (u_1,u_2) \in \mathbb{R}^2\).

The inverse of the wedge map is the ‘vee’ map \(\cdot^\vee : \mathfrak{o}(1,2) \to \mathbb{R}^3\).

Adjoint and Lie bracket

With the wedge and vee operators defined, we can get to work computing matrix representations of the adjoint operators and the Lie bracket. The adjoint operator is computed by

We obtain a matrix expression for \(\mathrm{Ad}_X\) by using the wedge and vee isomorphisms. From the computations above, we obtain the Adjoint matrix by extracting the coefficients of \(\omega,u\):

Differentiating this matrix in terms of the Lie group element \(L\) at the identity provides the “little” adjoint matrix (and the Lie bracket)

Exponential and Logarithm

As in any matrix Lie group, the exponential is given by the matrix exponential. However, this is an expensive computation, so we will try to find a simplified form that can be computed quickly. Let \(U \in \mathfrak{o}(1,2)\). Looking at the characteristic polynomial of \(U\), we have

From here, we can apply the Caley-Hamilton theorem, which says that every matrix satisfies its own characteristic equation. In other words,

This is a very useful relationship in simplifying the exponential map. Let \(q^2 = u_1^2 + u_2^2 -\omega^2,\) noting that \(q\) may be either real or pure imaginary. Then \(U^3 = q^2 U,\) and therefore

for all \(k =1,2,3,\ldots\). We are now ready to derive the exponential formula. We have

This is already a very nice formula, but we need to be careful with the possibility that \(q=0\) or \(q\) is imaginary. If \(q^2 > 0\) then the formula can be applied directly as shown. If \(q^2 = 0\) then the formula simplifies to \(\exp(U) = I_3 + U + \frac{1}{2} U^2.\) If \(q^2 <0\) then we may use the identities \(\sinh(x) = - i \sin(ix)\) and \(\cosh(x) = \cos(ix)\) to obtain

Finding an expression for the logarithm is a matter of inverting the exponential. We therefore start by supposing that \(L = \exp(U)\) and then solve for the components of \(U\) (implicitly assuming that there is such a solution!). Let \(t_1 = \frac{\sinh(q)}{q}\) and \(t_2 = \frac{\cosh(q)-1}{q^2}\) as shorthands. Then,

Taking the symmetric projection of both sides, i.e. mapping \(M \mapsto \frac{1}{2} (M + M^\top)\) yields

Likewise, taking the antisymmetric projection \(M \mapsto \frac{1}{2} (M - M^\top)\) yields

Extracting the bottom-left component of the symmetric projection equation, we can derive

Extracting the bottom-right component of the antisymmetric projection equation, we also have

Recall now that \(q^2 = \vert u \vert^2 - \omega^2\), so combing these equations we have

This now leads us to a solution for \(q\). If we allow \(q\) to be imaginary then

If we do not want to use imaginary numbers, then we need to consider the case that the left-hand side is less than \(0\). If this is the case, then we use the identity \(\sinh(x) = -i\sin(ix)\) to obtain

The important thing is that we are able to recover \(q\) or at least \(iq\), from which we can then immediately compute \(t_1\). Once we have \(t_1\), then we simply use the formulas obtained previously to compute

This completes the computation of the logarithm.

Conclusion

The indefinite orthogonal group is not often seen in robotics or control theory, but is highly relevant to physics. It is also a good chance to explore Lie group theory with a less familiar group, and particularly a group that is not overly similar to \(\mathbf{SO}(3)\) or \(\mathbf{SE}(3)\). The formulas presented in this post have all been added to my python Lie group library pylie as well, where they have been tested for correctness. If you spot any mistakes in this post or have any feedback, I would love to hear it!

Introduction

The special linear group \(\mathbf{SL}(n)\) is the set of \(n\times n\) matrices with determinant \(1\):

Geometrically, these matrices represent linear transformations of \(n\)-dimensional space that preserve (oriented) volume. In this post, we will focus solely on the case \(n=2\). This group, \(\mathbf{SL}(2)\) thus represents the linear transformations of 2D space that preserve oriented area. Using the definition of the determinant, the 2D Special Linear group is

The group properties are easily verified. The identity is \(I_2 \in \mathbf{SL}(2)\) and the product and inverse are just the matrix product and inverse, which satisfy

for all \(H,H_1,H_2 \in \mathbf{SL}(2)\). One useful consequence is that the inverse can be expressed simply as

The conjugation can thus be written as

Realistically, it is probably simpler just to use matrix operations if you even need to compute this.

Lie algebra

The Lie algebra can be obtained by differentiating the condition \(\det(H) = 1\) at the identity. It turns out that the Lie algebra is exactly the matrices \(U \in \mathbb{R}^{2\times 2}\) with trace zero. We write

This Lie algebra is a vector space just like any other Lie algebra, and we can relate it to \(\mathbb{R}^3\) by choosing a basis, or equivalently by defining a “wedge” map \(\cdot^\wedge : \mathbb{R}^3 \to \mathfrak{sl}(2)\). This map is required to be an isomorphism in the vector space sense, so it must be linear and invertible. We have already suggested the wedge map in our definition of the Lie algebra, but formally,

and the ‘vee’ map \(\cdot^\vee : \mathfrak{sl}(2) \to \mathbb{R}^3\) is simply the inverse. This choice defines a basis of \(\mathfrak{sl}(2)\) by \(\begin{aligned} E_1 &:= \begin{pmatrix} 1 & 0 \\ 0 & -1 \end{pmatrix}, & E_2 &:= \begin{pmatrix} 0 & 1 \\ 0 & 0 \end{pmatrix}, & E_3 &:= \begin{pmatrix} 0 & 0 \\ 1 & 0 \end{pmatrix}. \end{aligned}\)

Adjoint and Lie bracket

Using the wedge and vee operators, we can obtain expressions for the Adjoint operator and Lie bracket. While the expression for the conjugation operation was a bit complicated, the derivative (which gives the Adjoint operator) is a bit simpler.

where the last line follow using the fact that \(ad-bc = 1\), so \(ad = bc+1\). We obtain a matrix expression for \(\mathrm{Ad}_X\) by using the wedge and vee isomorphisms. From the computations above, we obtain the Adjoint matrix by extracting the coefficients of \(u_1,u_2,u_3\):

Differentiating this matrix in terms of the variable \(H\) at the identity provides the “little” adjoint matrix (and the Lie bracket)

Exponential and Logarithm

The exponential is given by the matrix exponential. This, by definition, involves an infinite power series, but we will try to ‘hide’ this inside some standard functions (in this case, \(\sinh\) and \(\cosh\)). Let \(U \in \mathfrak{sl}(2)\). Following the steps in my previous post on the why the Lie exponential is not surjective, we obtain

There are two important things to note in this formula. First, the fraction \(\frac{\sinh(\sqrt{\theta})}{\sqrt{\theta}}\) must be understood as a power series! Formally, we should define

and then write \(\exp(U) = \cosh(\sqrt{\theta}) I_2 + \mathrm{sinhc}(\sqrt{\theta}) U\). The second tricky part of this formula, is that the square root of \(\theta\) may not be real! That is to say, the value of \(\theta = u_1^2 + u_2 u_3\) inside the square root may, in fact, be negative, meaning that \(\sqrt{\theta}\) is an imaginary number. One way to deal with this is to simply choose one of the imaginary square roots, and apply \(\cosh\) and \(\sinh\) to the imaginary number. This is okay, but may be a little unsatisfying or cumbersome to implement. Fortunately, there is an alternative using the fact that \(\begin{aligned} \sinh(x) &= -i\sin(ix), & \cosh(x) &= \cos(ix) \end{aligned}\) for all \(x \in \mathbb{R}\). Suppose that \(\theta < 0\). Then,

This leads to the following final expression for the exponential. The exponential of \(U\) is given by

Note that we got rid of the \(\mathrm{sinhc}\) by specifying the third case.

Reversing this formula tells us how to find the logarithm as well. Consider a matrix \(H = \exp(U) \in \mathbf{SL}(2)\). From the formula for the exponential, and the fact that \(\mathrm{trace}(U)=0\), we find that

This makes it easy to distinguish which case we are dealing with, since \(\cosh(x) > 1\) for all \(x \neq 0\) and \(\cos(x) < 1\) for \(x \neq 2 k \pi\). Let $$\alpha = \frac{1}{2} \mathrm{trace}(H). Then

Now that we have recovered \(\theta\), we can recover \(U\) easily. In the case where \(\theta = 0\), then \(U = H - I_2\). In the case where \(\theta > 0\), then

and likewise, in the case that \(\theta < 0\),

Substituting \(\theta\) in terms of \(\alpha\) into these equations yields the final formula for the logarithm:

Conclusion

The special linear group comes up far less in robotics than the quaternions or the special euclidean group. However, it does have some beautiful formulas, and the exercise of working them out is never a wasted effort. The formulas here have all been implemented in pylie, so I hope people find them helpful. As always, please let me know if you have any suggestions for improvement!

Introduction

The Special Euclidean SE(n) group describes the orientation-preserving isometries of the Euclidean space \(\mathbb{R}^n\), where \(n \in \mathbb{N}\) is some integer. This group can be represented using matrices, as

In this post, we will focus solely on the 2D case. This is particularly relevant for ground-based robotics, where the group reflects the invariance of the robot’s dynamics. The 2D Special Euclidean group is given by

The group properties are easily verified. The identity is \(I_3 \in \mathbf{SE}(2)\) and the product and inverse are

Putting these together, we obtain the conjugation

Lie algebra

The Lie algebra can be obtained by differentiating the matrix \(X\) at \(\theta = 0\) and \(p = 0_{2\times 1}\). We get that

The notation \(\omega^\times\) is a very useful one. The operator \(\cdot^\times : \mathbb{R} \to \mathbb{R}^{2\times 2}\) is linear, so \((\omega_1 + \omega_2)^\times = \omega_1^\times + \omega_2^\times\) and, in particular, \(\omega^\times = \omega 1^\times\). This is a particularly nice feature since \(1^\times = R(\pi/2)\).

The Lie algebra \(\mathfrak{se}(2)\) is a vector space by definition, and we can relate it to \(\mathbb{R}^3\) by choosing a basis, or simply by defining a “wedge” map \(\cdot^\wedge : \mathbb{R}^3 \to \mathfrak{se}(2)\). This map is required to be an isomorphism in the vector space sense, so it must be linear and invertible. We choose the wedge map and its inverse, the vee map, to be

where \(\omega \in \mathbb{R}\) and \(v \in \mathbb{R}^2\). In other words, we define a basis of \(\mathfrak{se}(2)\) by \(\begin{aligned} E_1 &:= \begin{pmatrix} 1^\times & 0_{2 \times 1} \\ 0_{1\times 2} & 0 \end{pmatrix}, & E_2 &:= \begin{pmatrix} 0_{2\times 2} & \mathbf{e}_1 \\ 0_{1\times 2} & 0 \end{pmatrix}, & E_3 &:= \begin{pmatrix} 0_{2\times 2} & \mathbf{e}_2 \\ 0_{1\times 2} & 0 \end{pmatrix}, \end{aligned}\) where \(\mathbf{e}_1, \mathbf{e}_2 \in \mathbb{R}^2\) are the standard basis vectors.

Adjoint and Lie bracket

Using the wedge and vee operators, we can obtain expressions for the Adjoint operator and Lie bracket. The simplest way is to differentiate the conjugation operation. We have

We obtain a matrix expression for \(\mathrm{Ad}_X\) by using the wedge and vee isomorphisms. Specifically, for any linear operator \(L : \mathfrak{se}(2) \to \mathfrak{se}(2)\), we may define \(L^\vee \in \mathbb{R}^{3\times 3}\) to be the matrix such that \(L^\vee u = L(u^\wedge)^\vee\) for all \(u \in \mathbb{R}^3\). From this definition, we obtain the Adjoint matrix

Differentiating this matrix in terms of the variable \(X\) at the identity provides the “little” adjoint matrix and the Lie bracket

Exponential and Logarithm

The exponential is “simply” given by the matrix exponential. However, it is nice to have formulas that do not rely on solving infinite power series, or, at least, hide these solutions in well-known elementary functions like \(\sin\) and \(\cos\). Let \(U \in \mathfrak{se}(2)\). Then, we have that

This is the property that lets us simplify the exponential formula. It follows that \(U^{2k+1} = -\omega^2 U^{2k-1} = (-1)^k\omega^{2k} U\) for all \(k \geq 0\). We now solve the matrix exponential. We have that

Written in terms of the expanded matrix, we get

When \(\omega = 0\), the formula simplifies to

The expanded formula tells us how to take the logarithm as well. Given a matrix \(X \in \mathbf{SE}(2)\), we match the terms in \(\exp(U)= X\) to obtain

The first term is solved by \(\omega = \theta + 2k \pi\) for any \(k \in \mathbb{N}\), so we choose \(\omega \in [-\pi, \pi)\) as the standard solution. The second term is then given by solving

Observe that \((I_2 - R(\omega) ) (I_2 - R(-\omega) ) = 2(1-\cos(\omega))I_2\). Thus,

In summary,

Conclusion

The formulas presented in this summary are intended to be useful and practical for implementation, which is what I have done in the pylie library. I hope you find it helpful too, and please let me know if you find any issues or mistakes, or have suggestions for improvement!

Introduction

Quaternions are well-known to people working in robotics and aerospace. They (the unit quaternions, specifically) provide a smooth representation of attitude in using only four numbers, in contrast to rotation matrices that require 9 and Euler angles that are not smooth. In this post, I will explore the quaternions from a slightly different perspective: the quaternions (excluding zero) form a Lie group under multiplication. We will not restrict ourselves to the unit quaternions, instead exploring the full four-dimensional Lie group.

Basic group properties

Throughout this article, we will write a quaternion \(q \in \mathbb{H}\) as \(q = (r, u),\) where \(r \in \mathbb{R}_{\neq 0}\) and \(u \in \mathbb{R}^3\) represent the real and imaginary parts of \(q\), respectively. The product is defined by

The inverse of a quaternion is defined by

And the group identity is given by \(e := (1, 0_3)\).

The quaternions act on themselves by conjugation. Specifically,

Let us denote \(\vert q_1 \vert = \sqrt{r_1^2 + \vert u_1 \vert^2}\) and define \(u_1^\times \in \mathbb{R}^{3\times 3}\) to be the `skew’ matrix such that \(u_1^\times u_2 = u_1 \times u_2\). Then we end up with a nice and simple formula:

The Quaternion Lie Algebra

There are many ways to think of the Lie algebra of a given Lie group. Since our main interest is computation, we will choose the way that is easiest to work with for computation. The Lie algebra \(\mathfrak{h}\) of \(\mathbb{H}\) can identified with the tangent space at the identity \(e\). This definition is abstract, so we assign some coordinates. A Lie algebra element is described as \(w^\vee := (s, v) \in \mathbb{R}^4\), where the \(\vee\) operator is the map from the abstract Lie algebra to the coordinates in \(\mathbb{R}^4\). Near the identity, quaternion group elements can be written as

for small values of \(t \in \mathbb{R}\).

Exponential and Logarithm

The exponential relates the Lie algebra to the Lie group. We will use the `1-parameter subgroup’ definition here. Given a Lie algebra element \(w^\vee = (s,v)\), the exponential \(\exp(w)\) is defined as the solution to the initial value problem

at \(t = 1\). Let us evaluate the differential equation to find

This ODE is not straightforward to solve, unless we realise that this system is, in fact, linear! Writing \(q\) as a vector in \(\mathbb{R}^4\), we have

Since \(s\) acts as a scaling factor, we can pull it out of the equation for now, and solve the problem without it. Specifically,

so if we solve the problem while ignoring \(s\), we can add it back in at the end. To solve the ODE now, we only have to compute the matrix exponential

Examining the first nontrivial power of \(A\) reveals that

Substituting this into the exponential formula yields

Therefore, we have our final solution,

Note that, if \(\vert v \vert = 0\), then the whole computation simplifies and the solution is simply \(\exp(w) = ( e^s, 0_3)\).

The logarithm is found by inverting this formula, although there may be multiple solutions for a given \(q \in \mathbb{H}\). Suppose that \(q = \exp(w)\). Then we wish to determine the components of \(w = (s, v)\) in terms of \(q = (r, u)\). We have

Immediately, we see that \(e^s = r / \cos(\vert v \vert)\). Substituting this into the \(u\)-component,

Rather than substitute this back into the formula for \(e^s\), we observe that the norm of both sides of the original equation satisfies

In summary, we have thus found the logarithm to be

Similarly to the exponential formula, we should note that, if \(\vert u \vert = 0\), the formula simplifies to \(\log(q) = (\ln(r), 0_3)\).

Adjoint Operators and Lie Bracket

The big and little Adjoint operators are another important aspect of the Quaternion Lie algebra. The `big’ Adjoint operator \(\mathrm{Ad} : \mathbb{H} \times \mathfrak{h} \to \mathfrak{h}\) is defined by

In matrix form,

The `little’ adjoint operator \(\mathrm{ad} : \mathfrak{h} \times \mathfrak{h} \to \mathfrak{h}\) is defined as the derivative of the big Adjoint operator,

Once more, in matrix form,

The Lie bracket is equivalent to the adjoint operator, in the sense that

Matrix Representation

The final topic of interest for computations is the matrix representation of \(\mathbb{H}\). Matrix representations are rarely unique, but sometimes can be nice. The matrix representation we consider is \(\rho : \mathbb{H} \to \mathbf{GL}(4)\), given by

The matrix representation of the Lie algebra \(\mathfrak{h}\) is basically the same. Verifying that these are indeed representations is a messy and time-consuming computation. However, working out a matrix representation is very rewarding in that it provides a way to check all the other computations we have done. Specifically, we can check things like the inverse \(\rho(q)^{-1} = \rho(q^{-1})\), the exponential \(\mathrm{expm}(\mathrm{d}\rho(w)) = \rho(\exp(w))\), and the adjoint operators \(\mathrm{Ad}_q w = \rho(q) \mathrm{d}\rho(w) \rho(q)^{-1}\).

Summary

I decided to write this post when I needed these formulas for the \(n\)th time, and I realised that deriving them every time I needed them was taking too long. I hope they are helpful to anyone else who reads them, and please let me know if you spot any mistakes!

Introduction

The unscented transform is a way to approximate the probability distribution of a variable that is normally distributed after it is put through a nonlinear function. Concretely, let’s suppose that \(x \sim N(\mu, \Sigma)\), and let \(f : \mathbb{R}^n \to \mathbb{R}^m\) be some nonlinear function. Then we might ask: how can we approximate the distribution of \(f(x)\) by another normal distribution?

The ‘traditional’ approach is through linearisation. We know that if \(f\) is a linear or affine mapping, \(f(x) = A x + b\), then the distribution of \(f(x)\) is given by

So one way to approximate the distribution of \(f(x)\) for a nonlinear \(f\) is to simply say

where \(Df(\mu)\) is the differential (Jacobian) of \(f\) at \(\mu\). The problem is that this may not be a very good approximation, so what else can we do?

Sigma Points

What if, instead of trying to approximate \(f\) and then applying it to the distribution, we approximate the distribution and then apply \(f\)? This is the idea of the unscented transform. But how can we approximate the distribution? The answer is ‘sigma points’. A set of sigma points \(S\) for the distribution \(N(\mu, \Sigma)\) consists of points \(x^{(i)}\) and weights \(w^{(i)}\) so that

1. \(\sum_i w^{(i)} = 1\),
2. \(\sum_i w^{(i)} x^{(i)} = \mu\),
3. \(\sum_i w^{(i)} (x^{(i)} - \mu)(x^{(i)} - \mu)^\top = \Sigma\).

This approximates the original distribution if you interpret it as a probability function. Let \(\Omega = \{ x^{(i)} \mid i=0,1,...,p \}\), define a probability function \(p: \Omega \to \mathbb{R}\) by \(p(x^{(i)}) = w^{(i)}\), and let \(x\) be a random variable distributed according to \(p\). Then,

1. \(\sum_{x \in \Omega} p(x) = \sum_i w^{(i)} = 1\),
2. \(\mathbb{E}[x] = \sum_i w^{(i)} x^{(i)} = \mu\),
3. \(\mathrm{cov}(x,x) = \mathbb{E}\left[ (x - \mathbb{E}[x])(x - \mathbb{E}[x])^\top \right] = \sum_i w^{(i)} (x^{(i)} - \mu)(x^{(i)} - \mu)^\top = \Sigma\).

Now, this is only possible if there are at least \(n+1\) points, but there is no unique choice of sigma points. However, the original choice by Uhlmann gives us a ‘canonical’ choice; for \(i = 1,..., 2n\) define

Here \((\sqrt{n\Sigma})_i\) denotes the \(i\)th column of the matrix square-root of \(\Sigma\) multiplied by \(n\). Specifically, if \(A = \sqrt{n \Sigma}\), then \(A A^\top = n \Sigma\).

Unscented Transform

Now that we have a set of sigma points that approximate the original distribution, we need to apply the nonlinear function \(f\). This time, instead of approximating \(f\), we simply apply it to our sigma points and gather statistics at the end. Let \(x\) be a random variable distributed according to the sigma points. Then we compute the mean and covariance of \(f(x)\) as follows.

This is how we now approximate \(f(x)\): we say that, approximately, \(f(x) \sim N(\hat{\eta}, \hat{\Sigma})\). This is quite different from the linearisation approach, but is it any better? That depends on the function \(f\), the choice of sigma points, and also on what you mean by “better”.

Analysis

How does the unscented transform compare to the true distribution of \(f(x)\)? Let \(x \sim N(\mu, \Sigma)\), and let \(s = x - \mu \sim N(0, \Sigma)\). Then we can calculate the expected value of \(f(x)\) by using a Taylor expansion,

In other words, the expected value of \(f(x)\) is determined by the mean and covariance of \(x\) up to third order deviations from the mean. In particular, this means that the unscented transform gives the correct transformed distribution for all functions \(f\) where \(D^k f = 0\) for \(k \geq 3\); that is, functions \(f\) that are degree 2 polynomials. In fact, using the canonical choice of sigma points, this is true for polynomials of degree 3 as well.

What about the covariance obtained from the unscented transform? It turns out that this is equal to the covariance obtained from the unscented transform only when \(f\) is a first-order (linear-affine) function. However, practical experience has shown that it can offer better performance in a range of applications.

Conclusion

The unscented transform provides a different way to propagate uncertainty through nonlinear functions than the “standard” approach of linearisation. Rather than approximate the function as a linear function, it approximates the probability distribution as a discrete probability function. This has the advantage that there is no need to compute the derivative of a function to linearise it, and propagates the mean of the function more accurately than the linearisation approach. In practice, a number of applications show that the unscented transform can outperform the linearisation approach. Overall, in my view it is a very interesting perspective on approximations to probability that is not widely enough understood.