Introduction

This repository contains a C++ implementation for Arduino of the DCM (Direction Cosine Matrix) algorithm for estimating the Euler Angles (pitch, roll and yaw) of a body in a three dimensional space. Note that you will need to feed this algorithm with data from three different sensors: accelerometer, gyroscope and magnetometer. Each sensor has to measure the given magnitude for each one of the three axis (x, y and z), so 9 values are needed to use this library. Note that this algorithm is quite sensitive to calibration, so you must calibrate your measurements before feeding the algorithm with them, specially the magnetometer ones.

Note that this library is totally independant from the sensors (both software and hardware) that is used. Select your favourite sensors, read data from them and call the update function provided in this repository.

In the following secton some information about DCM algorithm is presented, but without going to much in detail. If you want to know more about the existing maths under the hood, feel free to check the references in the last section, specially [1] which contains a C implementation of the algorithm and has been used as a reference for this implementation.

Example

# DCM Algorithm

The DCM algothim can estimate the pitch, roll and yaw using a DCM (Direction Cosine Matrix), taking into account the drift. A PI control system is used to correct the drift, where P corresponds to the proportional term and I to the integrative one. This drift correction makes this algothim quite suitable for many applications.

TODO Figure DCM

The algorithm can be divided in the following steps, assuming that the input data is calibrated:

Initialize DCM: This part is only for the first iteration. It will initialize the DCM matrix with the first available data.
Calculate heading: Calculate the heading using the magnetometer.
Update matrix: Update the DCM matrix.
Normalize: Keep the vectors orthogonal and normalize them.
Correct drift: Correct the drift of the gyroscope using a PI control.
Calculate Euler: Calculate Euler angles from DCM matrix.

Init DCM

The first step is to initialize the DCM matrix. This is done by using the following expression:

Remember that this rotation matrix assumes the ZYX convention explained before. Note that vec is the unitary vector in x direction, A is a vector that contains the accelerometer calibrated values and time is the cross product.

Using that auxiliary vector, the roll can be calculated as follows. Note that aux_y and aux_z are the second and third components of the vector.

Last step to feed the matrix, is calculating the yaw.

Calculate heading

In each iteration of the algorithm, the heading value (also referred as yaw) is calculated. Calibrated values of the magnetometer are used. Note that c(), s() is equivalent to \cos(), \sin().

## Update matrix

Next step is to update the DCM matrix with the values of Omega_I, Omega_P calculated in the last iteration. These values will be zero in the first iteration, because the are calculated in next steps.

To understand the following steps, is necessary to explain some facts of the rotation matrix. Having calibrated gyroscope data G that after drift correction using, the rotation matrix becomes.

So in each step, the rotation matrix will be updated using the read gyroscope values and corrected according to the PI controller that will be explained then.

Normalize

This step is very important for the performance of the algorithm. By definition a rotation matrix is orthogonal, which means that R^tR = RR^t = I. The problem is that numerical errors can keep accumulating and break the orthogonality condition, which will result in a bad performance because the matrix will no longer represent a rotation.

In order to avoid this, in each iteration two steps are done: first make the vectors orthogonal again, second normalize them. The Gram-Schmidt method for orthogonalization is a very well known process, but for this purpose a different approach is taken. The used method does not fix one of the vector and calculated the others. Both vectors are changed, applying to each one half of the error.

First step is to compute the X and Y orthogonal vectors of the rotation matrix. This can be done by using the following:

The error is calculated as the dot product of X dot Y. Remember that the dot product of a dot b is calculated as |a||b| \cos(\theta), where $\theta$ is the angle between both vectors. If they were orthogonal, cos(theta) would be zero and the error would be also zero.

Next step is to calculate the Z orthogonal vector to X and Y. This is done by calculating the cross product between X and Y X times Y. Remember the geometric representation of the cross product.

TODO Figure DCM

The last step is to re normalize the calculated vectors $X,Y,Z$. This step is trivial and can be done by dividing the vector by the Euclidean norm. However, a different approach presented in [5] is used. This way assumes that the norm is not much greater than one, so Taylor's expansion can be used. The norm of a vector $\vec{u}$ using this approach is shown in here:

$\vec{u}_{norm} = \dfrac{1}{2} (3 - \vec{u} \cdot \vec{u}) \vec{u}$

Correct drift

Next step is to correct the drift of the gyroscope. The accelerometer will be used to correct the pitch and roll values, and the magnetometer to correct the yaw or heading. For correcting the drift, a PI controller will be used where P stands for proportional and I for integrative. A PI controller is similar to a PID controller, which is a very well known control system.

$y(t) = K_p e(t) + K_i \int e(t) dt + K_d \dfrac{de}{dt}$

A PI control will only have the proportional and integrative term. In the case of study of this project there will be two different controllers: one for the pitch/roll and other for the yaw. The constants corresponding to the integral and proportional term are:

KP_pr: Proportional constant for pitch and roll
KI_pr: Integral constant for pitch and roll
KP_y: Proportional constant for yaw.
KI_y: Integral constant for yaw.

$y(t) = K_p e(t) + K_i \int e(t) dt$

First step is to calculate the error in pitch and roll. This can be calculated using the cross product between the acceleration calibrated vector $A_{cal}$ and the last row of the DCM matrix DCM_{31}, DCM_{32}, DCM_{33}.

$error_{pitch/roll} = A_{cal} \times \begin{pmatrix} DCM_{31} DCM_{32} DCM_{33}\\ \end{pmatrix}$

Next step is to calculate the correction terms Omega_P, Omega_I that will be used to correct the gyroscope measurements. Note that |\vec{A_{cal}}| is the Euclidean norm of the acceleration vector. It is recommended to constrain this value between 0 and 1. This will reduce the error when the sensor is under big accelerations, i.e. moving it with the hand.

$\Omega_P = error_{pitch/roll} K^P_{pitch/roll} |\vec{A_{cal}}|$

$\Omega_I = \Omega_I + error_{pitch/roll} K^I_{pitch/roll} |\vec{A_{cal}}|$

Now it is time to correct the yaw drift using the yaw obtained by the magnetometer.

$error_{yaw} = \begin{pmatrix} DCM_{31} DCM_{32} DCM_{33}\\ \end{pmatrix} \left( DCM_{11}\sin(yaw) - DCM_{21} \cos(yaw) \right)$

And now it is time to add the contribution of the yaw to Omega_I, Omega_P.

$\Omega_P = \Omega_P + error_{yaw} K^P_{yaw}$

$\Omega_I = \Omega_I + error_{yaw} K^I_{yaw}$

In the next iteration this values will be used to correct the drift of the gyroscope values.

## Calculate Euler Angles

Once this step has reached, there is nothing much left. Just calculate the Euler angles from the updated rotation matrix. This can be calculated using the following expression. After this section the algorithm will run from the beginning again.

$\begin{pmatrix} pitch \\ roll \\ yaw \\ \end{pmatrix}= \begin{pmatrix} -\arcsin \left( R_{31} \right) \\ \arctan \left( \frac{R_{31}}{R_{33}}\right) \\ \arctan \left( \frac{R_{21}}{R_{11}}\right) \\ \end{pmatrix}$

6stringninja/dcm-algorithm-cpp