Abstract

The complimentry trust estimator presented in this whitepaper is useful for:

• Single axis attitude estimation
• Multi-axis attitude estimation where gimbal lock can be tolerated
• Systems with limited computational resources

The estimator requires gyroscopic and accelerometer measurements; these measurements are fused with the following algorithm:

\[\begin{split} \bar{\phi}^{gyro}_k &= \phi^{est}_{k-1} + \frac{\dot{\phi}^{gyro}_k + \dot{\phi}^{gyro}_{k-1}}{2}\Delta t_k \\ \bar{\phi}^{accel}_{k} &= \tan^{-1}\left(\frac{\ddot{y}^{accel}_K}{\ddot{z}^{accel}_K}\right) \\ \phi^{est}_{k} &= \bar{\phi}^{gyro}_k + f^{trust}(\hat{a}^{accel}_k) \times \left(\bar{\phi}^{accel}_k - \bar{\phi}^{gyro}_k\right) \end{split}\]

Where we are estimating the roll $\phi$ which is the angle about the x axis.

A computationally cheap trust function $f^{trust}(\hat{a}_k)$ is defined:

\[\begin{split} \left \| \hat{a}^{accel}_k \right \|^2 &= (\ddot{x}^{accel}_k)^2 + (\ddot{y}^{accel}_k)^2 + (\ddot{z}^{accel}_k)^2 \\ f^{trust}(\hat{a}^{accel}_k) &= F^{max} \max\left(0, 1 - \frac{1}{2 K^{tol}}\left( \frac{\left \| \hat{a}^{accel}_k \right \|^2}{g} - 1 \right)\right) \end{split}\]

Where:

• $g$ is the gravity constant $9.81m.s^{-1}$
• $F^{max}$ affects the corner frequency of the low-pass/high-pass complimentry filter
  • Make $F^{max}$ small to integrate gyro measurements into the estimator
• $K^{tol}$ represents the deviation from 1g we accept when including accelerometer estimates. Suggested values are:
  • 0.05 for well calibrated, low noise accelerometers
  • 0.20 for poorly calibrated, noisy accelerometers

Introduction

This paper outlines an attitude estimator using gyroscope and accelerometer measurements. The outputs of the estimator are roll (x-axis) and pitch (y-axis) in a right-handed system where gravity is aligned with the z-axis when all angles are zero.

An estimator in the hand is worth two in the bush

A digital gyroscope-only estimator using trapezoidal integration on step $K$ is given by:

\[\bar{\phi}_{K}^{gyro} = \phi_{0} + {\sum_{k=1}^K} \frac{\dot{\phi}_k + \dot{\phi}_{k-1}}{2}\Delta t_k\]

Where:

• $\phi_{0}$ is the initial angle
• $\bar{\phi}_{K}^{gyro}$ is the $K^{th}$ estimated angle
• $\dot{\phi}_k$ is the gyroscope measurement on the $k^{th}$ sample
• $\Delta t_k$ is the change in time since the previous sample

Discrete integration causes the gyroscope estimator to drift due to:

• Offets and misalignment in $\dot{\phi}$ measurements
• Dynamics not measured between samples

A digital accelerometer-only estimator using trigonometry on step $K$ is given by:

\[\bar{\phi}_{K}^{accel} = \tan^{-1}\left(\frac{\ddot{y}_K}{\ddot{z}_K}\right)\]

Where:

• $\bar{\phi}_{K}^{accel}$ is the $K^{th}$ estimated angle
• $\ddot{z}_K$ and $\ddot{y}_K$ are the $K^{th}$ acceleration measurements

The accelerometer estimator assumes the accelerometer is only measuring gravity; typically, this only holds when the measured object is stationary.

Both estimators have a tendency to run off under various conditions. However, they both provide complimentry properties - the accelerometer estimator provides a “lost-in-space” estimate to compensate for drift; meanwhile, the gyroscope estimator provides reasonable dynamic estimates.

Combining sensors

In the following sections, a Kalman filter is designed to fuse the accelerometer and gyroscope estimates. Some drawbacks of Kalman filters are:

• Filter gains are too abstract to base on physical properties
• Requires a matrix inversion, which can be computationally too intense for some bare-metal embedded systems

An alternative apporach is taken using a complementary filter with a dynamic gain computed using a trust function. This approach is equivalent to the core behaiour of a Kalman filter, but is more intuitive to tune and runs quickly on resource limited embedded systems.

System of equations

Here we present an imaginary - but useful - system for simulating an attitude estimator which operates in a 2-D plane.

\[\underset{\dot{X}}{ \begin{bmatrix} \dot{y} \\ \ddot{y} \\ \dot{z} \\ \ddot{z} \\ \dot{\phi} \\ \ddot{\phi} \\ \end{bmatrix} } = \underset{A}{ \begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & -d_y & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & -d_z & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & -d_{\phi} \end{bmatrix}} \underset{X}{ \begin{bmatrix} y \\ \dot{y} \\ z \\ \dot{z} \\ \phi \\ \dot{\phi} \\ \end{bmatrix}} + \underset{B}{ \begin{bmatrix} 0 & 0 & 0 \\ \frac{1}{M} & 0 & 0 \\ 0 & 0 & 0 \\ 0 & \frac{1}{M} & 0 \\ 0 & 0 & 0 \\ 0 & 0 & \frac{1}{I} \end{bmatrix}} \underset{U}{ \begin{bmatrix} F_y \\ F_z \\ \tau_{\phi} \end{bmatrix}}\]

Where:

• $X$ is the system state
• $y$ and $z$ are the translational dimensions making up the y-z plane
• $\phi$ is the angle about the $x$ axis perpendicular to the y-z plane
• $d_y$ and $d_z$ are translational damping in $y$ and $z$ respectively
• $d_\phi$ is angular damping
• $M$ and $I$ are the point mass and inertia about $\phi$ of our object
• $U$ are our system inputs:
  • $F_y$ and $F_z$ are forces in $y$ and $z$ respectively
  • $\tau_{\phi}$ is the torque applied to the $x$ axis

IMU equations

We specify two frames:

• $F^i$ the inertial frame, where:

\[\begin{bmatrix} y^i \\ z^i \\ \end{bmatrix} = \begin{bmatrix} y \\ z \\ \end{bmatrix}\]

• $F^s$ the sensor frame, which is equal to $F^i$ when $\phi = 0$

The transformation between the two frames is:

\[\begin{bmatrix} y^s \\ z^s \\ \end{bmatrix} = \begin{bmatrix} \cos(\phi) & \sin(\phi) \\ -\sin(\phi) & \cos(\phi) \\ \end{bmatrix} \begin{bmatrix} y^i \\ z^i \\ \end{bmatrix}\]

Accelerometer

The accelerometer measurement equation for any given sample $n$ is given as:

\[\begin{bmatrix} \ddot{y}^{accel}_k \\ \ddot{z}^{accel}_k \\ \end{bmatrix} = \begin{bmatrix} \cos(\phi) & \sin(\phi) \\ -\sin(\phi) & \cos(\phi) \\ \end{bmatrix} \begin{bmatrix} \ddot{y}^i_k \\ \ddot{z}^i_k + 9.81 \\ \end{bmatrix} + \begin{bmatrix} \ddot{y}^{noise}_k \\ \ddot{z}^{noise}_k \\ \end{bmatrix} + \begin{bmatrix} \ddot{y}^{offset} \\ \ddot{z}^{offset} \\ \end{bmatrix}\]

Where:

• $\ddot{y}^{noise}_k$ and $\ddot{z}^{noise}_k$ vary every update
• $\ddot{y}^{offset}$ and $\ddot{z}^{offset}$ are treated as constant measurement errors

Gyroscope

Rate measurements are independent of orientation so we only add noise and offset:

\[\dot{\phi}^{gyro}_k = \dot{\phi}_k + \dot{\phi}^{noise}_k + \dot{\phi}^{offset}\]

Estimator design

Here we will design an estimator, starting with a Kalman filter - and then simplify.

Kalman Filter

The Kalman Filter Algorithm

We assume we have a system of equations:

\[\hat{x}_{k} = A \hat{x}_{k-1} + B \hat{u}_{k}\]

Where:

• $\underset{n\times 1}{\hat{x}}$ is the system’s state
• $\underset{n\times n}{A}$ represents the change in system state between iterations
• $\underset{m\times 1}{\hat{u}}$ are system inputs
• $\underset{n\times m}{B}$ represents the change is system state due to inputs

And a set of observations:

\[\hat{z}_{k} = C \hat{x}_{k} + D \hat{u}_{k}\]

Where:

• $\underset{p\times 1}{\hat{z}}$ are the measured observations
• $\underset{p\times n}{C}$ describes how the state $\hat{x}$ maps to the observation
• $\underset{p\times m}{D}$ represents the change in observation due to inputs

note: A, B, u, and state x are not the same as in the system of equations. In this case we are dealing with a digital estimator, whereas those equations are for a continuous system.

Estimates of state $\underset{n\times 1}{\hat{x}}$ are represented as a gaussian distribution with:

• a mean, $\underset{n\times 1}{\hat{\mu}}$
• a variance, $\underset{n\times n}{\Sigma}$.

Given inputs from the previous iteration:

• $\hat{\mu}_{k-1}$ - the estimated mean
• $\Sigma_{k-1}$ - the estimated covariance matrix
• $\hat{u}_{k}$ - system inputs
• $\hat{z}_{k}$ - measured observations

Predict the distribution using the system model:

\[\begin{split} \bar{\hat{\mu}}_{k} &= A \hat{\mu}_{k-1} + B \hat{u}_{k} \\ \bar{\Sigma}_{k} &= A \Sigma_{k-1} A^T + R \end{split}\]

Calculate the Kalman Gain $K_k$:

\[K_{k} = \bar{\Sigma}_{k}C^T(C\bar{\Sigma}_{k}C^T + Q)^{-1}\]

Estimate the distribution by combining the observations with the predicted state:

\[\begin{split} \hat{\mu}_{k} &= \bar{\hat{\mu}}_{k} + K_{k}(z_k - C\bar{\hat{\mu}}_{k}) \\ \Sigma_{k} &= (I - K_{k} C)\bar{\Sigma}_{k} \end{split}\]

Estimator’s system of equations

We would like a generic estimator which only uses IMU measurements - this means we have no information about forces or torques being applied to the system.

The state we care about is:

\[\hat{x} = \begin{bmatrix} \phi \\ \dot{\phi} \\ \end{bmatrix}\]

Firstly, for state prediction, we end up using the gyroscope measurements for the inputs $u$:

\[\begin{bmatrix} \bar{\phi}_k \\ \bar{\dot{\phi}}_k \\ \end{bmatrix} = \begin{bmatrix} 1 & \frac{\Delta t_k}{2} \\ 0 & 0 \\ \end{bmatrix} \begin{bmatrix} \phi_{k-1} \\ \dot{\phi}_{k-1} \\ \end{bmatrix} + \begin{bmatrix} \frac{\Delta t_k}{2} \\ 1 \\ \end{bmatrix} \dot{\phi}^{gyro}_{k}\]

This is equivalent to the gyroscope-only estimator presented in the introduction.

Next, the observation is handled using accelerometer measurements:

\[\bar{\phi}_{K}^{accel} = \tan^{-1}\left(\frac{\ddot{y}^{accel}_k}{\ddot{z}^{accel}_k}\right)\] \[\begin{bmatrix} \phi^z_k \\ \dot{\phi}^z_k \\ \end{bmatrix} = \begin{bmatrix} 1 & 0 \\ 0 & 0 \\ \end{bmatrix} \begin{bmatrix} \phi_{k}^{accel} \\ 0 \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \\ \end{bmatrix} \dot{\phi}^{gyro}_{k}\]

We will then need to tune matrices $Q$ and $R$ to compute reasonable Kalman gains for various measurements.

$Q$ will need to change on every step, because it should reflect the trust we have in the acceleration measurements - which will be zero when the magnitude of acceleration is not close to 9.81.

Simplifying the estimator

There are two aspects of the Kalman filter in it’s default form which aren’t particularly useful for our estimator:

• The covariance matrix
• Including $\dot{\phi}$ in our observations

Simplification 1: Covariance Matrix

The covariance matrix $\Sigma$ doesn’t help us much. The problems are:

• If we have a bad accelerometer reading - we can’t use it, so tracking covariance isn’t ideal
• After a period of bad accelerometer readings, we don’t want our covariance to blow up to a point where the next “valid” accelerometer reading is trusted in it’s entirety. This is because it is possible to get invalid accelerometer readings which look valid, so they need to be filtered adequately.

Simplification 2: We only care about $\phi$

The observation step doesn’t pay attention to $\dot{\phi}$.

Simplification 3: Replacing the Kalman gain with a trust function

The two simplifications above have a direct implication on computing the Kalman gain.

If we allow $Q$ to equal, (note, we don’t estimate noise on $\dot{\phi}$ since it is not involved in observations):

\[\begin{bmatrix} q_k & 0 \\ 0 & 0\\ \end{bmatrix}\]

Then the Kalman gain computation

\[K_{k} = \bar{\Sigma}_{k}C^T(C\bar{\Sigma}_{k}C^T + Q)^{-1}\]

Simplifies to:

\[\begin{split} K_{k} &= \begin{bmatrix} r^{11}_k & r^{12}_k \\ r^{21}_k & r^{22}_k \\ \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0 & 0 \\ \end{bmatrix} \left( \begin{bmatrix} 1 & 0 \\ 0 & 0 \\ \end{bmatrix} \begin{bmatrix} r^{11}_k & r^{12}_k \\ r^{21}_k & r^{22}_k \\ \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0 & 0 \\ \end{bmatrix} + \begin{bmatrix} q_k & 0 \\ 0 & 0\\ \end{bmatrix} \right)^{-1} \\ &= \begin{bmatrix} \frac{r^{11}_k}{r^{11}_k + q_k} & 0 \\ 0 & 0\\ \end{bmatrix} \end{split}\]

Given that the covariance of $R$ is constant, but $q_k$ depends on how much we trust the measured acceleration, a more meaningful equation discards $R$’s contribution as meaningless scaling and instead focusses on a trust function, where $\hat{a}^{accel}_k$ is the accelerometer’s measurements on step $k$:

\[\hat{a}^{accel}_k = \begin{bmatrix} \ddot{x}^{accel}_k \\ \ddot{y}^{accel}_k \\ \ddot{z}^{accel}_k \\ \end{bmatrix}\] \[K_{k} = \begin{bmatrix} f^{trust}(\hat{a}^{accel}_k) & 0 \\ 0 & 0\\ \end{bmatrix}\]

Our final estimate of $\mu_k$ becomes a lot simpler too:

\[\begin{split} \hat{\mu}_{k} &= \bar{\hat{\mu}}_{k} + K_{k}(z_k - C\bar{\hat{\mu}}_{k}) \\ &= \begin{bmatrix} \bar{\phi}_k \\ \dot{\phi}^{gyro}_k \\ \end{bmatrix} + \begin{bmatrix} f^{trust}(\hat{a}^{accel}_k) & 0 \\ 0 & 0\\ \end{bmatrix} \left( \begin{bmatrix} \phi^z_k \\ \dot{\phi}^{gyro}_k \\ \end{bmatrix} - \begin{bmatrix} \bar{\phi_k} \\ \dot{\phi}^{gyro}_k \\ \end{bmatrix} \right) \\ &= \begin{bmatrix} \bar{\phi}_k + f^{trust}(\hat{a}^{accel}_k) \times \left(\phi^z_k - \bar{\phi}_k\right) \\ \dot{\phi}^{gyro}_k \\ \end{bmatrix} \\ &= \begin{bmatrix} \bar{\phi}^{gyro}_k + f^{trust}(\hat{a}^{accel}_k) \times \left(\bar{\phi}^{accel}_k - \bar{\phi}^{gyro}_k\right) \\ \dot{\phi}^{gyro}_k \\ \end{bmatrix} \end{split}\]

In this form, the trust function simply outputs a value in range $[0, 1]$ which determines how much we trust our observations. When it is $0$ we only trust the system equations (gyroscope estimator), when it is $1$ we only trust the observations (accelerometer).

This is also known as a complimentary filter with the exception that the gain is not constant. The gyroscopic measurements represent the high-pass elements (because they include feedback), and the accelerometer measurements represent the low-pass elements.

A paper Quadrotor attitude determination: A comparison study demonstrates similar performance between a Kalman filter and a complimentry filter. This isn’t too surprising when considering that without knowing intimate details about an IMU’s offsets and coupling profile, the best estimates from a Kalman filter effectively reduces to a complimentry filter in this application.

Trust function

A trust function must meet the following requirements:

\[f^{trust}(\hat{a}^{accel}_k) = \begin{cases} F^{max},& \text{if } \left \| \hat{a}^{accel}_k \right \| = g\\ 0, & \text{if } \left(\left \| \hat{a}^{accel}_k \right \| - g\right) \ge gK^{tol} \\ 0, & \text{if } \left(g - \left \| \hat{a}^{accel}_k \right \|\right) \ge gK^{tol} \end{cases}\]

Where:

• $g$ is the gravity constant $9.81 ms^{-1}$
• $K^{tol}$ is the deviation from $g$ where we reject all accelerometer data
• $F^{max}$ is the maximum output of the trust function

A trust function with a linear decay from $g$ is:

\[f^{trust}(\hat{a}^{accel}_k) = F^{max} \max\left(0, 1 - \frac{1}{K^{tol}}\left( \frac{\left \| \hat{a}^{accel}_k \right \|}{g} - 1\right)\right)\]

The trust function plot below demonstrates:

• $F^{max} = 1$
• $K^{tol} = 0.2$ (20% deviation)

Optimization for embedded systems

A minor optimization can be made to the trust function to make it run faster on embedded systems. The absolute acceleration is given as:

\[\left \| \hat{a}^{accel}_k \right \| = \sqrt{(\ddot{x}^{accel}_k)^2 + (\ddot{y}^{accel}_k)^2 + (\ddot{z}^{accel}_k)^2}\]

The square root operation can be removed by linerizing $\frac{\left | \hat{a}^{accel}_k \right |}{g}$ about $g$.

If we let:

\[\left \| \hat{a}^{accel}_k \right \|^2 = (\ddot{x}^{accel}_k)^2 + (\ddot{y}^{accel}_k)^2 + (\ddot{z}^{accel}_k)^2\]

Then:

\[\frac{\left \| \hat{a}^{accel}_k \right \|}{g} = \frac{\sqrt{\left \| \hat{a}^{accel}_k \right \|^2}}{g}\]

Linearizing this about $\left | \hat{a}^{accel}_k \right |^2 = g^2$ gives us:

\[\frac{\left \| \hat{a}^{accel}_k \right \|}{g} \approx 1 + \frac{\left \| \hat{a}^{accel}_k \right \|^2 - g}{2g^2}\]

Substituting this into $f^{trust}(\hat{a}^{accel}_k)$ gives a computationally cheaper trust function:

\[f^{trust}(\hat{a}^{accel}_k) = F^{max} \max\left(0, 1 - \frac{1}{2 K^{tol}}\left( \frac{\left \| \hat{a}^{accel}_k \right \|^2}{g} - 1 \right)\right)\]

As shown in the figure below, the resulting approximation carries up to 10% error from our ideal trust function. In practice this doesn’t impact the behavior of the complimentary filter in any meaningful way.

Complimentry Trust Estimator

The final form of the estimator is:

\[\begin{split} \bar{\phi}^{gyro}_k &= \phi^{est}_{k-1} + \frac{\dot{\phi}^{gyro}_k + \dot{\phi}^{gyro}_{k-1}}{2}\Delta t_k \\ \bar{\phi}^{accel}_{k} &= \tan^{-1}\left(\frac{\ddot{y}^{accel}_K}{\ddot{z}^{accel}_K}\right) \\ \phi^{est}_{k} &= \bar{\phi}^{gyro}_k + f^{trust}(\hat{a}^{accel}_k) \times \left(\bar{\phi}^{accel}_k - \bar{\phi}^{gyro}_k\right) \end{split}\]

Where the only tunable component is the trust function $f^{trust}(\hat{a}_k)$. When tuning the trust function:

• $F^{max}$ affects the corner frequency of the low-pass/high-pass complimentry filter.
  • Make $F^{max}$ small to integrate gyro measurements into the estimator.
• $K^{tol}$ represents the deviation from 1g we accept when including accelerometer estimates.
  • Base $K^{tol}$ on the confidence we have in our accelerometer calibration. SSuggested values are:
    • 0.05 for well calibrated, low noise accelerometers
    • 0.20 for poorly calibrated, noisy accelerometers

Simulation

The code in this simulation is written in Matlab and can be downloaded from github.com/leetware/IMUAttitudeSimulation.

The simulation operates in a 2D plane where:

• the plane axes are $y$ and $z$
• the axis orthogonal to the simulated plane is $x$
• the angle about $x$ is $\phi$ and follows the right-handed rule

Dynamics model

We want to expose a simulated body to $y$ and $z$ accelerations, and torques about $\phi$. An imaginary body is simulated in a mystery environment where:

• $y$ and $z$ directions have constant damping
• $\phi$ has constant damping
• gravity is detected by the imu, but never acts on the body
  • this was to avoid unnecessary simulation of surfaces

The governing state-space equations were presented in the System of equations section.

A simulation using sinusoidal forces and torques for a portion of the simulation is shown below:

IMU model

The IMU model has three sensors to simulate:

• Accelerometer Y
• Accelerometer Z
• Gyroscope X

Note: in this simulation, the accelerometer measures in units of $g$ where $1g = 9.81 m.s^{-1}$. This is typical of many IMUs.

Noise and offset are selected using a random function. This improves tuning since we make no assumptions about the noise or offset profile of the sensor our estimator will use.

\[\begin{split} \ddot{y}^{offset} &= 0.01 \times rand(\mathcal{N}(0, 0.01)) \\ \ddot{y}^{stddev} &= 0.005 - 0.010 \times rand([1, 2]) \\ \ddot{z}^{offset} &= 0.01 \times rand(\mathcal{N}(0, 0.01)) \\ \ddot{z}^{stddev} &= 0.005 - 0.010 \times rand([1, 2]) \\ \ddot{\phi}^{offset} &= \frac{4\pi}{360} (1 - 2 \times rand([0,1])) \\ \ddot{\phi}^{stddev} &= \frac{\pi}{360} (1 - 2 \times rand([0,1])) \end{split}\]

The IMU equations in the System of equations section already provides the frame transformations from interial to sensor frame.

Finally, noise is computed on each step as follows:

\[\begin{split} \ddot{y}^{noise}_k &= rand(\mathcal{N}(0, \ddot{y}^{stddev})) \\ \ddot{z}^{noise}_k &= rand(\mathcal{N}(0, \ddot{z}^{stddev})) \\ \ddot{\phi}^{noise}_k &= rand(\mathcal{N}(0, \ddot{\phi}^{stddev})) \end{split}\]

The plot below shows IMU simulated accelerometer data across a number of Monte Carlo simulations.

Estimator performance is judged and tuned based on its statistical performance against hundreds of Monte Carlo simulations.

Estimator Performance

Three estimators are compared:

• Accelerometer only estimator
• Gyro only estimator
• Complimentry trust estimator

Handling angular wrap-around

A practical implementation of the complimentry trust estimator needs to handle angle wrap-around. The important parts are:

• When finding the difference between two angles, a shortest radian path algorithm is used
  • This algorithm ensures that the magnitude of angles when computing a difference is never greater than pi
• All estimates of $\phi$ are wrapped into the range $[-\pi, +\pi]$

Results - Single Simulation

A single run of the simulated system is shown below.

• The accelerometer-only estimates are poor when subjected to dynamics.
• The gyroscope estimates degrade the longer the simulation runs.
• The complimentry trust estimator:
  • rejects accelerometer estimates during dynamic periods.
  • corrects for gyroscope drift once the accelerometer estimates are trustworthy.

Results - Monte Carlo

To compare performance between estimators, the simulations were run 100 times with the following results:

The accelerometer-only estimator is non-recursive, so noise and offsets do not aggregate over time.

The gyroscope-only estimator is recursive, noise and offsets cause estimates to drift relative to the inaccuracies of the sensor.

The complimentry trust estimator combines properties of both estimators - it is prone to drift during dynamics, but will reconverge once acclerometer measurements become accurate again.

The mean-squared error across simulations is indicative of the general performance of the estimators. The result is a little misleading since gyro errors get larger the longer the simulation runs, and the other two estimator’s errors will decrease assuming there are no more inputs to the system.

Implementing on Bittle

To demonstrate the behavior of the complementry trust estimator on a real system; the estimator was implemented on Petoi’s Bittle.

Bittle is a quadrapedal robot which is controlled by Petoi’s NyBoard. The NyBoard’s microcontroller is the ATMega328P which is a 16Mhz single-core 8-bit microcontroller with only 2kB of RAM. The NyBoard uses InvenSense’s MPU6050 IMU.

The motion and peripheral algorithms leave only a handful of bytes avaliable for attitude determination. There is also a trade-off between update period and computational power. We want to update frequently for improved gyroscope estimates; however, the ATMega328P is limited in the number of operations it can complete between updates.

The embedded implementation on Bittle has the following characteristics:

• Updates at 100Hz
• Configures the MPU6050 with:
  • a low-pass filter with a 100Hz corner frequency
  • an accelerometer range of ±1g
  • a gyroscope range of ±1000°/s

The Bittle MPU6050’s sensor frame is not aligned with the Bittle’s body frame. The transform between the sensor frame and body frame is a 180° rotation about the z-axis.

Embedded algorithm

The core of the Embedded algorithm is available on github.com/leetnz/Bittleet.

Bittle samples three accelerometer and three gyroscope readings through I2C at 400kHz. Each value is represented by a 16-bit integer count.

A simple transform is applied IMU measurements to move the x and y axes from the sensor frame into the body frame:

\[\begin{split} \dot{\phi}^{gyro}_k &= -\dot{\phi}^{imu}_k \\ \dot{\theta}^{gyro}_k &= -\dot{\theta}^{imu}_k \\ \ddot{x}^{accel}_k &= -\ddot{x}^{imu}_k \\ \ddot{y}^{accel}_k &= -\ddot{y}^{imu}_k \end{split}\]

Where:

• $\dot{\phi}$ represents the rate on the x axis
• $\dot{\theta}$ represents the rate on the y axis

We want pitch $\theta$ estimates, which is the angle in the $y$ axis. The accelerometer estimator for $\theta$ is:

\[\begin{split} \bar{\theta}^{accel}_{k} &= \tan^{-1}\left(\frac{-\ddot{x}^{accel}_k}{\ddot{z}^{accel}_k}\right) \\ \end{split}\]

When computing the accelrometer estimates, we do not convert the raw values into accelerations or units of $g$. Instead we represent $1g$ in units of counts. Trigonometry works in any unit, so this is a slight optimization to avoid floating point arithmetic which the ATMega328P is not suited for.

The trust function is computed once per iteration using the linearized trust function which reduces computational load by avoiding a square root.

The overall breakdown of timings is shown below:

Function	Timing
Sample IMU	1920 us
Estimator update	1200 us

This allows a maximum sample rate of 300 Hz. Bittle firmware is also responsible for actuating servos and taking user commands; so a compromise of 100Hz sampling is used.

Experimental setup

The code used to capture data in this experiment is available on https://github.com/leetnz/imuAttitudeProcessing.

The goal of this experiment was to visualize the difference between Bittle’s perceived attitude, and the true attitude. To achieve this goal, we needed two sets of sensors:

• The IMU which is used for attitude estimation.
• A camera, which can record the “true” attitude of bittle for comparison.

The logCapture.py script runs on the RaspberryPi attached to Bittle. For the length of the experiment it:

• Stores serial csv data from the NyBoard which contains time and attitude estimates.
• Captures video output.

Both the serial data and video outputs are at 25Hz.

Results

The first two figures show bittle being moved in the roll direction with the roll estimate overlayed.

The final figure shows bittle walking on uneven carpet causing bittle to fall over; which is quite helpful for demonstrating a real-world application of requiring responsive dynamic estimation.

The video https://youtu.be/kj7YloDtiZY demonstrates this estimator operating alongside video at 25 FPS.

Issues with Gimbal lock

In the above demonstrations, when bittle is rolled more than 90 degrees, the accelerometer-based pitch estimate reaches a discontinuity, where it goes from 0 degrees to 180 degrees.

This is a weakness of the representation of state in this estimator. Using Euler angles will always result in gimbal-lock derived discontinuities - in this case, pitch aligning with yaw. A quaternion representation of attitude would resolve this issue, but is outside of the goals of this estimator design.

Gimbal lock is inconsequential in the following scenarios:

• We only require estimation in one axis.
• The body is not expected to exceed $[-90^\circ, 90^\circ]$ in any axis under normal operation.

Bittle meets the second requirement, where we assume if it has reached or exceeded 90 degrees, it has fallen over.

Conclusion

The complimentry trust algorithm presented in this paper estimates attitude in a single axis using a three axis accelerometer and a gyroscope measurement on the axis to be estimated.

The key features of the estimator are:

• computationally simple as demonstrated on a 16Mhz 8-Bit microcontroller:
  • runs at 100Hz consuming less than 33% of CPU
  • uses only a handful of bytes of RAM
• tuning parameters have physcial meaning

In cases where gimbal lock discontinuities can be tolerated, this algorithm may be applied to more than one axis, as demonstrated on Petoi’s Bittle for fall detection.

External Links

• Quadrotor attitude determination: A comparison study - A brief summary of some competing attitude estimators.
• petoi.com - creators of Bittle, the robot arduino dog used in this paper
• github.com/leetnz/Bittleet - Firmware written for Bittle which includes the complimentry trust estimator
  • Bittleet:Attitude.cpp - The complimentry trust estimator implementation
• github.com/leetnz/IMUAttitudeSimulation - Matlab simulation of estimator
• github.com/leetnz/imuAttitudeProcessing - Capture and plotting scripts for experimental data
• Estimator video - video demonstrating estimator in action