Detailed explanation of how mathematics is written in the NUbots codebase.
Ysobel Sims GitHub avatarAlex Biddulph GitHub avatarCameron Murtagh GitHub avatar
Updated 1 Oct 2023

The NUbots codebase uses specific mathematical concepts and notation that are relevant to the field of robotics. In the codebase there are many uses of vectors, matrices, spaces, and transforms between spaces. A specific notation is used for these transforms for better readability. This page details how these concepts and notations are implemented in the codebase.

Linear Algebra

We currently use the Eigen C++ template library for linear algebra in the NUbots codebase. Be aware that Eigen has some common pitfalls that are listed on the Eigen pitfalls page.


A basis is a maximal set of linearly independent vectors.

In this context, bases usually span R3\mathbb{R}^3. That is, they cover three-dimensional space. Given a basis with three vectors b1,b2,b3\mathbf{b}_1, \mathbf{b}_2, \mathbf{b}_3, any three dimensional vector u\mathbf{u} can be written as

u=a1b1+a2b2+a3b3\mathbf{u} = a_1 \cdot \mathbf{b}_1 + a_2 \cdot \mathbf{b}_2 + a_3 \cdot \mathbf{b}_3

where a1,a2,a3Ra_1, a_2, a_3 \in \mathbb{R}. In other words, a1,a2,a3a_1, a_2, a_3 are scalars. These scalars are unique.

If the vectors in the basis are mutually perpendicular, then they are orthogonal.

An orthonormal basis (ONB) is a basis which is orthogonal and each vector has length 1.

An example of an ONB is the standard basis

e1=(1,0,0)e2=(0,1,0)e3=(0,0,1)e_1 = (1, 0, 0) \quad e_2 = (0, 1, 0) \quad e_3 = (0, 0, 1)


As mentioned in the previous section, any vector r\mathbf{r} can be expressed as a linear combination of the elements of a basis

ra=r1aa1+r2aa2+r3aa3\mathbf{r}^a = r_1^a \mathbf{a}_1 + r_2^a \mathbf{a}_2 + r_3^a \mathbf{a}_3

The scalars r1a,r2a,r3ar_1^a, r_2^a, r_3^a are the coordinates for the vector r\mathbf{r} in the basis aa.

In the NUbots codebase, vectors are written as rABb which denotes a vector from point BB to point AA in the basis (or space) bb. These are column matrices of the coordinates of the vectors

ra=[r1ar2ar3a]\mathbf{r}^a = \begin{bmatrix} r_1^a \\ r_2^a \\ r_3^a \end{bmatrix}

To make computations such as inner product (also called dot product) or cross product with coordinates, the vectors must be relative to the same basis.

When drawing a vector rA/Bb\mathbf{r}_{A/B}^b, that is a vector from BB to AA in the space bb, the tail is at BB and the head is at AA. Vector addition requires that the head of one vector is the same as the tail of the other. This is demonstrated here. We have that

rA/Cb=rA/Bb+rB/Cb=rB/Cb+rA/Bb\mathbf{r}^b_{A/C} = \mathbf{r}^b_{A/B} + \mathbf{r}^b_{B/C} = \mathbf{r}^b_{B/C} + \mathbf{r}^b_{A/B}

since vector addition is commutative. This is written in code as

Eigen::Vector3d rACb = rABb + rBCb;


To describe the motion of the robot, we need a reference frame and a coordinate system.

The reference frame is the physical rigid body and the coordinate system is a mathematical concept, which is represented as a basis.

NUbots use a right-handed coordinate system to measure positions and rotations.

A right hand with the index finger pointed forwards and labelled 'x', the middle finger pointing to the left and labelled 'y', and the thumb pointing upwards and labelled 'z'
Right-Hand Rule. Image (modified) from Wikimedia Commons

Rotations are defined by a 3x3 matrix. If we have a vector ra\mathbf{r}^a, that is the vector r\mathbf{r} in the coordinate system aa, the rotation

[cos(ψ)sin(ψ)0sin(ψ)cos(ψ)0001]\begin{bmatrix} \cos(\psi) & -\sin(\psi) & 0 \\ \sin(\psi) & \cos(\psi) & 0 \\ 0 & 0 & 1 \end{bmatrix}

will rotate ra\mathbf{r}^a by ψ\psi radians around the coordinate axis a3\mathbf{a}_3.

More on rotation matrices can be found here.

The orientation of the basis bb relative to aa can be described by three consecutive rotations about the main axis. There are 12 different orders to do these rotations in, and each triplet of rotated angles is called a set of Euler angles. In robotics, the triplet is yaw, pitch and roll (Z-Y-X).

In the NUbots codebase, we denote a rotation matrix from space bb to space aa as Rab.

The rotation matrices used for changing between spaces belong to the special orthogonal group of order 3. These all have the following properties

R1=RT\mathbf{R}^{-1} = \mathbf{R}^T
det(R)=1\text{det}(\mathbf{R}) = 1

The first property reduces the complexity of the computation to find the inverse matrix. The second property tells us this matrix can be inverted and it preserves scale between the two spaces.

If we have a vector rC/Da\mathbf{r}_{C/D}^a, that is it goes from point D to point C in a space aa, and we want to rotate it from space aa to space bb, we write

rC/Db=RabrC/Da\mathbf{r}^b_{C/D} = \mathbf{R}^b_a \cdot \mathbf{r}^a_{C/D}

This is written in the NUbots codebase as rCDb = Rba * rCDa. Note that the notation is Rfromto\mathbf{R}_{from}^{to}.

Homogeneous Transformations

A homogeneous transformation matrix in three dimensions is a 4x4 matrix containing a rotation component and a translation component.

If we have some rotation Rab\mathbf{R}_a^b and a translation component rA/Bb\mathbf{r}^b_{A/B}, then we have a transformation matrix that transforms from space aa to space bb:

Hab=[RabrA/Bb01]\mathbf{H}_a^b = \begin{bmatrix} \mathbf{R}_a^b & \mathbf{r}_{A/B}^b \\ \mathbf{0} & 1 \end{bmatrix}

This is written in the NUbots codebase as Hba.

The vector rA/Bb\mathbf{r}_{A/B}^b is the vector from B (the origin of space b) to A (the origin of space a) in b space, which would be written in the codebase as rABb.

The reason for this is that if we apply the transform HabH^b_a to some point ρa\rho^a, then ρa\rho^a will first be rotated by RabR_a^b, giving ρb\rho^b. ρb\rho^b will then be translated by some vector according to the transformation matrix. Since ρb\rho^b is now rotated in the bb coordinate system, the translation vector must also be in this bb coordinate system. The vector is from B to A because a point (0, 0, 0) in space aa should be moved to the origin of aa in bb space after transformation. Thus, the translation vector is rA/Bb\mathbf{r}_{A/B}^b.

This is illustrated in the following diagram.

Two three dimensional coordinate spaces, one labelled as space 'a' and the other as space 'b'. There is a vector from space 'a' to some point 'c', and is labelled 'rCAb'. There is another vector from 'b' to 'a' and is labelled 'rABb'. There is a final vector from 'b' to 'c' which is labelled 'rCBb = rABb + rCAb'

This assumes the original vector, rCAa has already been rotated into space bb, i.e. we have rCAb.

Finding the inverse of HabH^b_a is made simpler by the properties of the matrix.

(Hab)1=Hba=[(Rab)T(Rab)TrA/Bb01](H^b_a)^{-1} = H^a_b = \begin{bmatrix} (\mathbf{R}_a^b)^T & (\mathbf{R}_a^b)^T \cdot -\mathbf{r}_{A/B}^b \\ \mathbf{0} & 1 \end{bmatrix}

Transformation matrices left multiply with vectors to get a vector in the new space. This vector is rotated and then translated.

rCBb=HabrC/Aa\mathbf{r}^b_{CB} = \mathbf{H}^b_a \cdot \mathbf{r}^a_{C/A}

This is written in the codebase as rCBb = Hba * rCAa.

This is equivalent to

rCBb=rA/Bb+(RabrC/Aa)r_{CB}^b = r_{A/B}^b + (R^b_a * r_{C/A}^a)

or in code,

Eigen::Vector3d rCBb = rABb + (Rba * rCAa);

Vectors should only be multiplied with a transformation matrix if the last two letters of the vector match. That is, the position they are going from should match the origin of the space they are measured in. This means that Hab * rCBb is valid, but Hab * rCDb is invalid. Note that the first vector is rCBb and the second is rCDb.

If you would like the vector only rotated, extract the rotational component of the transform and apply it to the vector. The spaces that the vector and rotation matrix are in should match.

Eigen::Vector3d rCDb = Hba.rotation() * rCDa;

Homogeneous transformation matrices can be multiplied together to get a new homogeneous transformation matrix.

Hac=HbcHab\mathbf{H}_a^c = \mathbf{H}_b^c \cdot \mathbf{H}^b_a

This is written in the codebase as

Eigen::Isometry3d Hca = Hcb * Hba;

Unit Vectors

A unit vector is a vector with a length of 1. They are used to represent directions in space.

They are written by describing their origin, the direction they are pointing and the space they are in.

uA/Bb\mathbf{u}^b_{A/B} is a unit vector at B pointing towards A measured in space b.

The direction of a vector can be found by normalising it. This is done by dividing the vector by its length.

Eigen::Vector3d uABb = rABb.normalized();

Unit vectors can be rotated by rotation matrices but not translated. They should not be multiplied by a homogeneous transformation matrix.

Velocity and Acceleration

The velocity and acceleration of some object is represented relative to some space.

If we have the velocity of object A moving relative to space b vAb\mathbf{v}^b_{A}, then we write this in the codebase as vAb.

If we have the acceleration of object A moving relative to space b aAb\mathbf{a}^b_{A}, then we write this in the codebase as aAb.

Velocities and accelerations can only be rotated, not translated. They should not be multiplied by a homogeneous transformation matrix, but they can be multiplied by a rotation matrix.

If a velocity is multiplied with time, it becomes a displacement vector.

Eigen::Vector3d rA_tAb = vAb * t;

If an acceleration is multiplied with time, it becomes a velocity vector.

Eigen::Vector3d vA_tAb = aAb * t;

If you only have a partial velocity or acceleration, such as velocity in the direction of an object you can represent this by labelling the target the velocity is towards.

For example vA/Bb\mathbf{v}^b_{A/B} would describe the velocity of B towards the point A.


Using Eigen, we define our vectors as Eigen::Vector3d, rotation matrices as Eigen::Matrix3d, and homogeneous transformation matrices as Eigen::Isometry3d. The d denotes double and instead can be f for float. Eigen::Isometry3d allows us to do computations easier than with Eigen::Matrix4d.

It allows multiplication with a Eigen::Vector3d even though the dimensions do not match. Using Eigen::Isometry3d makes it quicker to find the inverse, as it employs the techniques mentioned above. Eigen::Isometry3d has functions to individually return the rotational component of the matrix and the translation component of the matrix, by using Htw.rotation() and Htw.translation() respectively, where Htw is of type Eigen::Isometry3d. It also makes it easy to individually assign the rotational and the translational components of the matrix by using Htw.linear() = Rtw and Htw.translation() = rWTt respectively.

When we get a message that is a homogeneous matrix it will be of type mat4. This can then be cast to Eigen::Isometry3d. Let the matrix be called Htw (world to torso transform) and the message be called sensors (see: Sensors.proto). Then this is written as

Eigen::Isometry3d Htw(sensors.Htw);

If you have a transformation matrix Htw (world to torso transform) and you want the matrix Hwt (torso to world transform), this is can be achieved by calling Htw.inverse(). The inverse function swaps the letter order. The returned inverse matrix is not mutable.

If you have a vector rTWf (world to torso in foot space) and you want the opposite letter order (torso to world in foot space), this is done by negating the vector, rWTf = -rTWf.

Negating does not change the space the vector is measured in.

If you have two transformation matrices and you would like to multiply them to get a new transformation matrix, the inner two letters should be identical, i.e. the same space.

Eigen::Isometry3d Htf = Htw * Hwf;

This gives the foot to torso transform by multiplying the world to torso transform with the foot to world transform.

These two inner letters are cancelled, with the outer two remaining. This also works for rotation matrices.

If you have two vectors in the same space and their inner (or outer, since vector addition is commutative) letters are both the same then they can be cancelled in a similar way to the transformation matrices.

Eigen::Vector3d rXZa = rXYa + rYZa;

In general, t is reserved for the torso, w for the world, r for the robot, and b for ball. f could be field or foot, and should be determined from context.

When measured relative to the robot the x-axis is forward out of the torso, the y-axis is to the left and the z-axis is up the neck. This is robot space, also called torso space. The origin of this space is the base of the torso between the legs.

World space is defined with the z-axis pointing up. The other two axes are determined from the directions of the x-axis and the y-axis of the robot when it turns on. The origin of this space is the same as torso space when the robot is turned on, but on the ground.

Robot space is defined with the z-axis pointing up inline with world, the x-axis pointing forward out of the torso, and y-axis to the left. The origin of this space is below the torso, on the ground. It is created from the X-Y position and yaw angle from odometry.

For measurements on the field we use field space. For field coordinates the x-axis is toward the opponent's goal, the y-axis is left when facing the opponents goal and the z-axis is up. The origin of this space is the centre of the field.


HabDenotes a 3D affine transformation matrix going from space bb to space aa.
RabDenotes a 3D rotation matrix from space bb to space aa.
rABbDenotes a vector from point BB to point AA in space bb.
uABbDenotes a unit vector from point BB towards point AA in space bb.
vAbThe velocity of AA in space bb.
vABbThe velocity of BB towards AA in space bb.
aAbThe acceleration of AA in space bb.
aABbThe acceleration of BB towards AA in space bb.
rXZa = rXYa + rYZaCreate a new vector from two other vectors.
rACc = Hcb * rABbChanging a vector from one space to another using a homogeneous transform.
rABd = Rdc * rABcChanging a vector from one space to another using a rotation transform
Hba = Hab.inverse()Inverting a matrix swaps the letter order.
Hac = Hab * HbcCreating a new transform from two other transforms.
Euler AnglesAre of the form Z-Y-X.

Useful Links

Spherical Coordinates

Spherical coordinates are a system of representing points in 3D space. Rather than representing points as (x,y,z)\left(x, y, z\right) cartesian coordinates, points are represented as (r,φ,θ)\left(r, \varphi, \theta\right), where r is radial distance, φ\varphi is the polar angle, and θ\theta is the azimuthal angle.

'Spherical coordinates $(r, \varphi, \theta)$. Radial distance r to the origin, polar angle $\varphi$, and azimuthal angle $\theta$'
Spherical Coordinates. Image from Wikimedia Commons

Spherical coordinates can be calculated from cartesian coordinates using the follow equations:

r=x2+y2+z2φ=acos(zr)=atan(x2+y2z)θ=atan(yx)r = \sqrt{x^2 + y^2 + z^2} \\ \varphi = \textrm{acos}\left(\frac{z}{r}\right) = \textrm{atan}\left(\frac{\sqrt{x^2 + y^2}}{z}\right) \\ \theta = \textrm{atan}\left(\frac{y}{x}\right)

Cartesian coordinates can be calculated from spherical coordinates using the follow equations:

x=rcos(θ)sin(φ)y=rsin(θ)sin(φ)z=rcos(φ)x = r\textrm{cos}\left(\theta\right)\textrm{sin}\left(\varphi\right) \\ y = r\textrm{sin}\left(\theta\right)\textrm{sin}\left(\varphi\right) \\ z = r\textrm{cos}\left(\varphi\right) \\

A vector representing a point in the spherical coordinates would be written as sA/Bbs_{A/B}^b, which would be sABb in the codebase.

In order to ensure a unique representation of coordinates, the following restrictions are enforced on spherical coordinates:

r00φπ0θ2πr \geqslant 0 \\ 0 \leqslant \varphi \leqslant \pi \\ 0 \leqslant \theta \leqslant 2\pi \\

Reciprocal Distance

A common variation on spherical coordinates in our codebase is to use reciprocal distance. If a vector has spherical coordinates (r,φ,θ)\left(r, \varphi, \theta\right), then a vector in spherical coordinates with reciprocal distance has coordinates (1r,φ,θ)\left(\frac{1}{r}, \varphi, \theta\right).

This is used in the vision detectors and localisation when representing the distance from the camera to an object. Using reciprocal distance also means our covariance (our uncertainty in our measurement) scales better with distance. The further away an object is the less certain we are about out measurement of it.

If sA/Bbs_{A/B}^b is the vector in spherical coordinates, then srA/Bbsr_{A/B}^b is the same vector in spherical coordinates, but with reciprocal radial distance.


Given a set of data points, often we want to find a function that contains all these points, which we call an interpolant. Interpolants are commonly polynomials, because they can approximate complex curves with a few points, with relatively low computational costs. Interpolant polynomials can be affected by Runge's phenomenon, where the interpolant is locally accurate for the given data points but has oscillations issues beyond these points. A further issue with interpolants is uniqueness - if there are too few constraints then there may be an infinite number of interpolants, and if there are too many constraints then there may not be a solution. Generally, for n+1n+1 distinct points, there is a unique polynomial of degree n\leq n that interpolates the points. Polynomials of degree higher than nn may have aliasing issues where the interpolant is unnecessarily 'wiggly' between points.


Rather than have one global polynomial, it can be useful to use piecewise polynomials, connected at their edges. These so-called splines are all defined locally around the interpolation points, and can avoid Runge's phenomenon. Splines are used in the Quintic Walk Engine.

2D graph plot of a piecewise polynomial function that passes through specific highlighted points.
Spline. Interpolation points are in blue.

Every pair of consecutive points on the graph has an associated polynomial that is plotted in the range between those two points.

The Quintic Walk specifically uses quintic splines. This means that each polynomial is of the form

Si=a5ix5+a4ix4+a3ix3+a2ix2+a1ix+a0iS_i = a_{5i} x^5 + a_{4i} x^4 + a_{3i} x^3 + a_{2i} x^2 + a_{1i} x + a_{0i}

For i=0,...,n1i = 0, ..., n-1, where nn is the number of interpolation points.

These particular quintic splines have interpolation points (x0,y0,y0,y0)(xn,yn,yn,yn)(x_0, y_0, y'_0, y''_0) \cdots (x_n, y_n, y'_n, y''_n). They must satify the conditions

  • Sj(x)S_j(x) is a polynomial in [xj,xj+1][x_j, x_{j+1}] for each j=0,,n1j = 0, \cdots , n-1
  • Sj(xj)=yjS_j(x_j) = y_j and Sj(xj+1)=yj+1S_j(x_{j+1}) = y_{j+1} for each j=0,,n1j = 0, \cdots , n-1
  • Sj+1(xj+1)=Sj(xj+1)=yj+1S'_{j+1}(x_{j+1}) = S'_j(x_{j+1}) = y'_{j+1} for each j=0,,n2j = 0, \cdots , n-2
  • Sj+1(xj+1)=Sj(xj+1)=yj+1S''_{j+1}(x_{j+1}) = S''_j(x_{j+1}) = y''_{j+1} for each j=0,,n2j = 0, \cdots , n-2
  • S0(x0)=b0S'_0(x_0) = b_{0} and Sn1(xn)=ynS'_{n-1}(x_n) = y'_{n}
  • S0(x0)=b0S''_0(x_0) = b_{0} and Sn1(xn)=ynS''_{n-1}(x_n) = y''_{n}

These conditions result in smooth joints between each polynomial, meaning the position, velocity and acceleration at those joints is the same for both polynomials. Each data point has an x and y value, as well as first and second derivative value. The conditions above ensure the splines satisfy the given values for each data point.


See the glossary.

NUbots acknowledges the traditional custodians of the lands within our footprint areas: Awabakal, Darkinjung, Biripai, Worimi, Wonnarua, and Eora Nations. We acknowledge that our laboratory is situated on unceded Pambalong land. We pay respect to the wisdom of our Elders past and present.
Copyright © 2023 NUbots - CC-BY-4.0
Deploys by Netlify