How to compute quaternions from raw IMU and magnetometer data?

Question

I have raw accelerometer data (w_ib^b) and magnetometer data from a UAV test flight. I would like to reconstruct the quaternion dynamics of the UAV without accumulating error from integration.

The method I was using involves solving the following integration: d(q_b^w)/dt = 0.5*Omega(w_ib^b)*q, where Omega(w_ib^b) is of the form: [-skew(w_ib^b) w_ib^b; -w_ib^b' 0].

I was wondering whether there was an alternative method of computing the aircraft attitude given raw angular velocity and magnetometer information from a UAV?

Answer 1

If all equations are digitally computed multiple times per second, all variables are updated at every frame and the integration error is only one time-step old. Full Flight Simulators use four sets of quaternions to compute the state of the aircraft in earth reference frame. To illustrate how it is done in FFS flight dynamics, here is a partial set of the steps that take place:

$$ \dot{E_1} = 0.5 \cdot (-E_4 \cdot p - E_3 \cdot q - E_2 \cdot r) + E_1 \cdot C_Q * C_{Q\Delta t}\tag{1}$$ $$ E_1 = \dot{E_1} * \Delta t \tag{2}$$ $$ \Sigma E^2 = {E_1}^2 + {E_2}^2 + {E_3}^2 + {E_4}^2 \tag{3}$$ $$ C_Q = 1 - \Sigma E^2 \tag{4}$$ $$ E_{1P} = E_1 * \frac {1}{\sqrt{\Sigma E^2} } \tag{5}$$

• Equation (1) is one of four equations for $\dot{E_1}, \dot{E_2}, \dot{E_3}, \dot{E_4}$, while $p,q,r$ are aircraft roll, pitch, yaw rates.
• $C_Q$ is a quaternion correction factor which uses the sum of all quaternions squared.
• $C_{Q\Delta t}$ is a timing correction factor and should be less than the iteration rate in Hz. A constant value is empirically chosen that gives good stability.
• Equation (2) is a simple Euler integrator using the actual time step of the computation frame. So if we're computing at 1000 Hz, $\Delta t$ = 0.001.
• $ E_{1P}$ is the prime quaternion substituted in the transformation matrix from body to earth axes.