Accelerometer

A 3DoF accelerometer is a device that measures acceleration along three orthogonal axes $(x,y,z)$. They are used in robotics, fitness trackers, spacecraft, and many other applications.

Attitude

Attitude refers to the orientation of an entity in a 3D space. It’s usually represented by three angles: roll, pitch, and yaw. Roll is the rotation around the x-axis, pitch is the rotation around the y-axis, and yaw is the rotation around the z-axis.

Typically, a gyroscope is used to measure angular rates which is then integrated to determine the attitude.

But, we can also calculate the roll and pitch angles using accelerometer readings alone. But with one caveat:

The accelerometer must not be accelerating i.e. it must be at rest or move with a constant velocity.

Working Principle

The principle behind this being possible is that gravity is the only accelerating force acting on the sensor when it’s at rest or moving at a constant velocity.

Deriving the Formula

Let the gravity vector in the inertial frame (reference frame with 0 angles in all axes) be:

$G = \begin{bmatrix}0 \\ 0 \\ -g\end{bmatrix}$ where $g = 9.81 m/s^2$

The accelerometer readings in the body frame are given by the vector:

$A = \begin{bmatrix}a_x \\ a_y \\ a_z\end{bmatrix}$

To find the acceleration on each axis due to the gravity vector, we need to convert it from the inertial frame into the body frame.

The rotation matrices for each axes are given by:

Roll:

$M_1(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & c \phi & s \phi \\ 0 & -s\phi & c \phi \end{bmatrix}$

Pitch:

$M_2(\theta) = \begin{bmatrix} c \theta & 0 & -s \theta \\ 0 & 1 & 0 \\ s \theta & 0 & c \theta \end{bmatrix}$

Yaw:

$M_3(\psi) = \begin{bmatrix} c \psi & s \psi & 0 \\ -s \psi & c \psi & 0 \\ 0 & 0 & 1 \end{bmatrix}$

The rotation matrix from the inertial frame to the body frame is given by:

$R = M_1(\phi) \times M_2(\theta) \times M_3(\psi)$

$R = \begin{bmatrix} 1 & 0 & 0 \\ 0 & c \phi & s \phi \\ 0 & -s(\phi) & c \phi \end{bmatrix} \begin{bmatrix} c \theta & 0 & -s \theta \\ 0 & 1 & 0 \\ s \theta & 0 & c \theta \end{bmatrix} \begin{bmatrix} c \psi & s \psi & 0 \\ -s \psi & c \psi & 0 \\ 0 & 0 & 1 \end{bmatrix}$

$R = \begin{bmatrix} c \theta c \psi & c \theta s \psi & -s \theta \\ s \phi s \theta c \psi - c \phi s \psi & s \phi s \theta s \psi + c \phi c \psi & s \phi c \theta \\ c \phi s \theta c \psi + s \phi s \psi & c \phi s \theta s \psi - s \theta c \psi & c \phi c \theta \end{bmatrix}$

The gravity vector in the body frame is given by:

$A = R \times G$

Substituting the values of $G$ and $A$:

$\begin{bmatrix} a_x \\ a_y \\ a_z \end{bmatrix} = R \times \begin{bmatrix} 0 \\ 0 \\ -g \end{bmatrix}$

After multiplying the matrices, we get the following:

$a_x = gsin \theta$

$a_y = -g sin \phi cos \theta$

$a_z = -g cos \phi cos \theta$

Notice that none of the equations include $\psi$ or yaw. This is because the gravity vector acts along the z-axis and is not affected by the yaw angle.

From the above equations, we can calculate the roll and pitch angles as follows:

$\theta = arcsin(\frac{a_x}{g})$

$\frac{-g sin \phi cos \theta}{-g cos \phi \ cos \theta} = \frac{sin \phi}{cos \phi} = tan \phi$

$\phi = arctan(\frac{a_y}{a_z})$