Derivations of formulas for estimating pitch, roll, and heading of an animal from a tag with an accelerometer and magnetometer.

biologging

Author

Max Czapanskiy

Published

August 30, 2024

Every 6-18 months, my friend Jessie and I try to remember the linear algebra required to estimate animal orientation (pitch, roll, heading) from an accelerometer/magnetometer tag. This usually involves a 2-3 hour Zoom call, frantic whiteboard activity, and moderate hair loss. In the hopes of breaking this stressful cycle, here are my notes about the subject.

Note

This post is my version of Mark Johnson’s notes from the 2011 fine-scale animal movement workshop in Hobart, Australia. The bulk of the material is his. My changes include different axis conventions, expanded derivations where I found myself lost, and removal of some material about hardware.

Reference frames

A major source of confusion is often the reference frame. We’re going to describe orientation as a rotation. The reference frame describes what it’s a rotation from. A pitch of 45° means something intuitively (facing halfway from the horizon to straight up), but mathematically it only has meaning if we specify what we’re pitching 45° relative to. Enter reference frames.

We use three reference frames, which we call \(\Psi\). One for the world at large (AKA navigation, \(\Psi^n\)), one for the animal’s body (\(\Psi^a\)), and a third for the tag (\(\Psi^t\)).

Where \(N\), \(E\), and \(D\) represent the vectors for north, east, and down, respectively. \(X^a\) is the longitudinal axis of the animal (along the spine), \(Y^a\) is the transverse axis (left to right), and \(Z^a\) is the dorso-ventral axis. Same for \(X^t\), \(Y^t\), \(Z^t\), but for the tag instead of the animal.

Because the vectors \(N\), \(X^a\), etc are 3x1, these reference frames are all 3x3 matrices. When we align with their point of view, we define them to be the identity matrix, \(I\).

Note

\(\Psi^n\) is controversial within the animal tagging community. A conventional right-hand rule reference frame would be \(\begin{bmatrix} N ~ W ~ U \end{bmatrix}\), not \(\begin{bmatrix} N ~ E ~ D \end{bmatrix}\). Hold your hand out and stick out your thumb and first two fingers all at right angles (pointer forward, thumb up, middle finger left). Let your pointer be \(N\), your middle finger be \(W\), and your thumb be \(U\). Notice how comfortable that is? Unfortunately, the math makes it such that positive pitch means down in this orientation. Most people find that counter-intuitive. We have two options to change that: adopt a left-hand rule system \(\begin{bmatrix} N ~ E ~ U \end{bmatrix}\) or an “upside down” right-hand rule \(\begin{bmatrix} N ~ E ~ D \end{bmatrix}\). Within the marine vertebrate research community, there’s a split. Some use the left-hand rule \(\begin{bmatrix} N ~ E ~ U \end{bmatrix}\)(Johnson and Tyack 2003) and others the upside-down right-hand rule \(\begin{bmatrix} N ~ E ~ D \end{bmatrix}\)(Cade et al. 2021). I find keeping the right-hand rule makes more sense to me personally, so that’s what I’ll use here.

Rotation matrices

Rotations are defined using Euler angles: the familiar pitch, roll, and heading. We define two important rotation matrices: \(\boldsymbol{Q}\) and \(\boldsymbol{W}\).

\(\boldsymbol{Q}\) is the rotation matrix describing the orientation of the animal frame relative to the navigation frame.

\[
\Psi^a = \Psi^n \boldsymbol{Q}
\]

That’s often tricky to understand, so I’ll repeat it. Our estimate of the animal’s body orientation is a rotation matrix. \(\boldsymbol{Q}\) itself is the product of three other rotation matrices, which define the pitch, roll, and heading of the animal. Therefore, \(\boldsymbol{Q}\) is a biologically meaningful variable that we’re interested in studying.

\(\boldsymbol{W}\) is the rotation matrix describing the animal’s orientation relative to the tag.

\[
\Psi^a = \Psi^t \boldsymbol{W}
\]

Like \(\boldsymbol{Q}\), \(\boldsymbol{W}\) is the product of three rotations, which yaw, pitch, and roll the tag relative to the animal. Unlike \(\boldsymbol{Q}\), \(\boldsymbol{W}\) is not biologically meaningful. However, we need \(\boldsymbol{W}\) to figure out \(\boldsymbol{Q}\) because we can’t measure \(\boldsymbol{Q}\) directly.

What does a rotation matrix actually look like? There are three we use often, which are the rotations around the reference frame axes themselves. Hold out your hand in \(\begin{bmatrix} N ~ E ~ D \end{bmatrix}\) orientation. Rotate forward to pitch your pointer down. Which axis did you rotate around? Your thumb is pointing in a new direction, too, but your middle finger (which represents \(E\), the y-axis) is still pointing to the right. So we see pitch is arotation around the y-axis. The same is also true for roll (around the x-axis) and yaw (around the z-axis). Mathematically, they look like this:

\[
\begin{align}
Y(\gamma) &= \begin{bmatrix}
cos ~ \gamma & -sin ~ \gamma & 0 \\
sin ~ \gamma & cos ~ \gamma & 0 \\
0 & 0 & 1
\end{bmatrix} \\
P(p) &= \begin{bmatrix}
cos ~ p & 0 & sin ~ p \\
0 & 1 & 0 \\
-sin ~ p & 0 & cos ~ p
\end{bmatrix} \\
R(r) &= \begin{bmatrix}
1 & 0 & 0 \\
0 & cos ~ r & -sin ~ r \\
0 & sin ~ r & cos ~ r
\end{bmatrix}
\end{align}
\]

Where \(Y\), \(P\), and \(R\) are the yaw, pitch, and roll matrices, respectively. \(\gamma\), \(p\), and \(r\) are the yaw, pitch, and roll angles themselves. They’re not symmetrical matrices, which means matrix multiplication won’t be commutative, so order matters. We always apply them in the order \(YPR\). This has the effect of rotating around the local axes, those of the thing itself being rotated. The reverse order, \(RPY\) applies the rotations relative to the global axes. If that doesn’t makes sense don’t worry about it, just remember for \(Q\) and \(W\) we always rotate in the order \(YPR\).

Accelerometers, gravity, pitch, and roll

What do accelerometers measure?

Accelerometers measure acceleration. Acceleration can mean a few different things, and the particular kind of acceleration measured by accelerometers is not the intuitive one for most of us.

Imagine a tag at rest on a table. Is it accelerating? Instinctively, we think of acceleration relative to a coordinate system. And since the tag’s position isn’t changing, we would say it isn’t accelerating. However, an accelerometer would measure an acceleration of 9.8 m s^{-2} straight down. Similarly, if we went to the top of a tower and dropped a tag, we would observe it accelerating at 9.8 m s^{-2} straight down, but the accelerometer would read 0.

These counter-intuitive readings are because accelerometers measure proper acceleration, which is relative to the inertial frame of reference. We don’t need to dive into the precise physics of inertial reference frames. In short, here’s the intuition that will help: the accelerometer readings are relative to an object in free fall.

Estimating pitch and roll

We can use the accelerometer readings to detect the pitch and roll of an object at rest. At rest, the only force is gravity, which is aligned downwards, along \(D\). So the acceleration measured by the tag is:

\[
A^t = g D^T \Psi^t
\]

Where \(A^t\) is the 1x3 vector of accelerometer readings \(\begin{bmatrix} a_x ~ a_y ~ a_z \end{bmatrix}\). \(g\) is the strength of gravitational acceleration (about 9.8 m s^{-2}). Notice the T in \(D^T\) is capitalized. We use lower case \(t\) to indicate something to do with the tag e.g., \(\Psi^t\). Capital T here means we’re transposing the column vector representing “down” into a row vector. That just makes the math easier.

We have a problem here, though. \(A^t\) is measured in the tag’s reference frame (hence the \(\Psi^t\)), but down \(D^T\) is defined in the navigation frame. Let’s pretend for a second the tag frame is aligned with the animal frame (i.e., \(\Psi^a = \Psi^t\)) . Then, recall our reference frame definition \(\Psi^a = \Psi^n Q\). Substituting that in, we get:

\[
\begin{align}
A^t &= g D^T \Psi^t \\
A^t &= g D^T \Psi^n Q \\
A^t &= g \begin{bmatrix} 0 ~ 0 ~ 1 \end{bmatrix}Q
\end{align}
\]That works because, by definition, \(D^T \Psi^n = \begin{bmatrix}0 ~ 0 ~ 1\end{bmatrix}\). That’s how we defined “down” in the navigational reference frame.

Now recall the rotation matrices for yaw, pitch, and roll, and that \(Q\) is the composition of those three. When we substitute in \(Y\), \(D\) will just cancel it out. This should make sense intuitively: gravity’s effect on your acceleration is the same whether you face north, south, east, or west. Then we can solve for \(A^t\) based on \(P\) and \(R\).

\[
\begin{align}
A^t &= g \begin{bmatrix} 0 ~ 0 ~ 1 \end{bmatrix} Q \\
A^t &= g \begin{bmatrix} 0 ~ 0 ~ 1 \end{bmatrix} Y(\gamma) P(p) R(r) \\
A^t &= g \begin{bmatrix} 0 ~ 0 ~ 1 \end{bmatrix} \begin{bmatrix}
cos ~ \gamma & -sin ~ \gamma & 0 \\
sin ~ \gamma & cos ~ \gamma & 0 \\
0 & 0 & 1
\end{bmatrix} P(p) R(r) \\
A^t &= g \begin{bmatrix} 0 ~ 0 ~ 1 \end{bmatrix} P(p) R(r) \\
A^t &= g \begin{bmatrix} 0 ~ 0 ~ 1 \end{bmatrix} \begin{bmatrix}
cos ~ p & 0 & sin ~ p \\
0 & 1 & 0 \\
-sin ~ p & 0 & cos ~ p
\end{bmatrix} \begin{bmatrix}
1 & 0 & 0 \\
0 & cos ~ r & -sin ~ r \\
0 & sin ~ r & cos ~ r
\end{bmatrix} \\
A^t &= g \begin{bmatrix} -sin~p & 0 & cos~p \end{bmatrix} \begin{bmatrix}
1 & 0 & 0 \\
0 & cos ~ r & -sin ~ r \\
0 & sin ~ r & cos ~ r
\end{bmatrix} \\
A^t &= g \begin{bmatrix} -sin~p & -cos~p~sin~r & -cos~p~cos~r \end{bmatrix} \\
\end{align}
\]

Now we can solve for \(p\) and \(r\), our pitch and roll. We’ll call them \(\hat{p}\) and \(\hat{r}\) to indicate they’re estimates. First, \(p\):

Note: remember \(sin ~ {-x} = -sin ~ x\). The formula for \(\hat{p}\) is correct for an object at rest, but usually we’re tracking a moving animal. We can improve our estimate if we divide by the norm of the whole acceleration vector, rather than dividing by g. So in practice, \(\hat{p}\) will be:

\[
\hat{p} = -sin^{-1} \frac{a_x}{||A^t||}
\]

Roll is a bit trickier, but still doable. Start by dividing \(a_y\) by \(a_z\).

Magnetometers, Earth’s magnetic field, and heading

What do magnetometers measure?

From accelerometers we can estimate pitch and roll, but we need an alternative to estimate heading. The Earth’s magnetic field, roughly speaking, points up at the magnetic south pole, is parallel to the earth’s surface around the equator, and points down at the magnetic north pole. So unlike gravity, which always points down at the same strength, the magnetic field vector changes depending on where you are^{1}. We usually call the magnetic field \(B\) and it’s defined by three parameters: \(b\) (the strength of the field), \(i\) (inclination of the field, or the angle it’s pointing below the horizon), and \(d\) (declination of the field, or the angle of magnetic north east of true north). Using rotation matrices, \(B\) is:

\[
\begin{align}
B &= b N^T \Psi^n P(-i) Y(d)
\end{align}
\]

\(b N^T \Psi^n\) means a vector with magnitude \(b\) pointing north in the navigational frame. Then we incline the field by pitching it \(i\) below the horizon and yaw it \(d\) degrees east. \(-i\) because in our navigational reference frame negative pitch means down, remember? Also, this is the only time \(P\) will be applied before \(Y\). That’s because these are global rotations, not local. You’ll never do this with tag or animal rotations; those are local rotations^{2}. Solving for \(B\), we get:

\[
\begin{align}
B &= b N^T \Psi^n P(-i) Y(d) \\
B &= b N^T \Psi^n
\begin{bmatrix}
cos ~ {-i} & 0 & sin ~ {-i} \\
0 & 1 & 0 \\
-sin ~ {-i} & 0 & cos ~ {-i}
\end{bmatrix}
\begin{bmatrix}
cos ~ d & -sin ~ d & 0 \\
sin ~ d & cos ~ d & 0 \\
0 & 0 & 1
\end{bmatrix} \\
B &= b N^T \Psi^n
\begin{bmatrix}
cos ~ i & 0 & -sin ~ i \\
0 & 1 & 0 \\
sin ~ i & 0 & cos ~ i
\end{bmatrix}
\begin{bmatrix}
cos ~ d & -sin ~ d & 0 \\
sin ~ d & cos ~ d & 0 \\
0 & 0 & 1
\end{bmatrix} \\
B &= b \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}
\begin{bmatrix}
cos ~ i & 0 & -sin ~ i \\
0 & 1 & 0 \\
sin ~ i & 0 & cos ~ i
\end{bmatrix}
\begin{bmatrix}
cos ~ d & -sin ~ d & 0 \\
sin ~ d & cos ~ d & 0 \\
0 & 0 & 1
\end{bmatrix} \\
B &= b \begin{bmatrix} cos ~ i & 0 & -sin ~ i \end{bmatrix}
\begin{bmatrix}
cos ~ d & -sin ~ d & 0 \\
sin ~ d & cos ~ d & 0 \\
0 & 0 & 1
\end{bmatrix} \\
B &= b \begin{bmatrix} cos~i ~ cos~d & -cos~i ~ sin~d & -sin ~ i \end{bmatrix} \\
\end{align}
\]

A little more complicated than just \(\begin{bmatrix} 0 & 0 & g \end{bmatrix}\), isn’t it? But it’s essential for figuring out our heading.

Estimating heading

When you use a compass to find north, it’s important to lay it flat. You may have seen compasses where the needle floats in a bubble, which keeps it level despite minor pitches and rolls. That’s called gimbaling the compass. We need to do the same for our 3d magnetometer readings. Unlike a hand-held compass, we can’t do that mechanically. Instead we use \(\hat{p}\) and \(\hat{r}\), which we got from the accelerometer, to rotate the the magnetometer readings to the horizontal. If \(M^a\) is the magnetometer reading on the animal, we’ll call the gimbaled version \(M^h\), where \(h\) means horizontal.

In matrix multiplication, we get the inverse of a product \((AB)^{-1}\) by reversing and inverting the matrices \(B^{-1}A^{-1}\). That’s why roll and pitch get flipped around. And rotation matrices have the convenient property where their inverse is their transpose.

Now that we’ve removed the pitch and roll of the magnetometer to get \(M^h\), the magnetic field is only a function of the animal’s heading, which we estimate as \(\hat{h}\).

This is different than Mark Johnson’s result and I haven’t figure out why yet. I may have defined \(B\) differently, perhaps? But this way makes more sense to me because I can see how it uses declination to correct for the difference between magnetic and true north. I need to ask someone about this!

What about \(W\)?

Up to this point we have operated under the assumption the tag and animal reference frames were aligned. That’s often not the case, and figuring out \(W\) can be tricky! If the tag is attached to an animal in hand, as is often the case for seabirds and pinnipeds, then \(W\) may be very close to \(I\). However, if the tag is attached to an animal in motion, which is common in cetacean studies, \(W\) could be just about anything. The full suite of methods for estimating \(W\) from tag data itself is outside the scope of this blog post, but I will show how to derive \(W\) on two conditions. 1) There’s a period of time when the animal’s orientation is known (i.e., we know \(Q\)) and 2) the yaw of the tag with respect to the animal is 0. These conditions are typically met in the animal-in-hand scenario, where the tag is manually attached along the longitudinal axis of the animal, but the curvature of the animal’s body may pitch or roll the tag away from \(I\).

Let \(\bar{A}_t\) be the mean acceleration reading during the period of known animal orientation. Let’s also say that during this time, the animal’s pitch and roll relative to the navigational frame are 0 (i.e., it’s laying flat). Define the pitch and roll of the tag relative to the animal as \(p_0\) and \(r_0\), respectively. Notice \(p_0\) and \(r_0\) are the orientation of the tag relative to the animal, so they define \(W^{-1}\), not \(W\). Then:

\[
\begin{align}
\bar{A}_t &= g D^T Q W^{-1} \\
Q &= Y(\gamma)P(0)R(0) \\
W^{-1} &= Y(0)P(p_0)R(r_0) \\
\bar{A}_t &= g \begin{bmatrix}0 & 0 & 1\end{bmatrix} Y(\gamma)P(0)R(0) Y(0)P(p_0)R(r_0) \\
\bar{A}_t &= g \begin{bmatrix}0 & 0 & 1\end{bmatrix} P(p_0)R(r_0) \\
\bar{A}_t &= g \begin{bmatrix}0 & 0 & 1\end{bmatrix}
\begin{bmatrix}
cos ~ p_0 & 0 & sin ~ p_0 \\
0 & 1 & 0 \\
-sin ~ p_0 & 0 & cos ~ p_0
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \\
0 & cos ~ r_0 & -sin ~ r_0 \\
0 & sin ~ r_0 & cos ~ r_0
\end{bmatrix} \\
\bar{A}_t &= g \begin{bmatrix}-sin ~ p_0 & 0 & cos ~ p_0\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \\
0 & cos ~ r_0 & -sin ~ r_0 \\
0 & sin ~ r_0 & cos ~ r_0
\end{bmatrix} \\
\bar{A}_t &= g \begin{bmatrix}
-sin~p_0 & cos~p_0~sin~r_0 & cos~p_0~cos~r_0
\end{bmatrix}
\end{align}
\]

Now we can get \(p_0\) from \(\bar{A}_{tx}\) and \(r_0\) from the ratio of \(\bar{A}_{ty}\) and \(\bar{A}_{tz}\).

Now that we have the derivations in place, we can estimate the animal’s pitch, roll, and heading. Here are the typical steps.

Measure acceleration and magnetism in the tag frame, \(A^t\) and \(M^t\)

Estimate \(W\)

May use \(p_0 = -sin^{-1}\frac{\bar{A}_{tx}}{||\bar{A}_t||}\) and \(r_0 = tan^{-1}\frac{\bar{A}_{ty}}{\bar{A}_{tz}}\) if applicable (but remember \(W^{-1}=P(p_0)R(r_0)\), not \(W=P(p_0)R(r_0)\))

Convert \(A^t\) and \(M^t\) to animal frame \(A^a=A^tW\), \(M^a=M^tW\)

Estimate pitch and roll

\(\hat{p}=-sin^{-1} \frac{A^a_x}{||A^a||}\)

\(\hat{r} = tan^{-1}~\frac{A^a_y}{A^a_z}\)

Gimbal \(M^a\), to get \(M^h = M^a R(\hat{r})^T P(\hat{p})^T\)

Let’s say we have an animal laying flat on the ground. The tag is on the animal’s back, to the right of the spine, closer to the tail than the head. So let’s say the tag’s pitch is 15° and the roll is 30° relative to the animal. In this case, the tag’s measured acceleration is

Because \(\Psi^a=\Psi^tW\), \(A_w = A_tW\), so rotation the tag’s acceleration vector by \(W\) should give us the animal’s acceleration.

# Animal is only experiencing gravity in the Z axisAa <-c(0, 0, 9.8)# Rotating At by W should give us AaAtW <- At %*% Wrbind(Aa =round(as.vector(Aa), 2), AtW =round(as.vector(AtW), 2))

[,1] [,2] [,3]
Aa 0 0 9.8
AtW 0 0 9.8

Beautiful!

References

Cade, David E., William T. Gough, Max F. Czapanskiy, James A. Fahlbusch, Shirel R. Kahane-Rapport, Jacob M. J. Linsky, Ross C. Nichols, et al. 2021. “Tools for Integrating Inertial Sensor Data with Video Bio-Loggers, Including Estimation of Animal Orientation, Motion, and Position.”Animal Biotelemetry 9 (1). https://doi.org/10.1186/s40317-021-00256-w.

Johnson, M. P., and P. L. Tyack. 2003. “A Digital Acoustic Recording Tag for Measuring the Response of Wild Marine Mammals to Sound.”IEEE Journal of Oceanic Engineering 28 (1): 3–12. https://doi.org/10.1109/joe.2002.808212.

Footnotes

The Earth’s gravitational field isn’t exactly uniform, but it’s close enough for our purposes.↩︎

Linear algebra is frickin’ weird, I know, I’m so sorry↩︎