2.5 Attitude and Position Integration

Inertial navigation systems use the angular rate and acceleration measurements from gyroscopes and accelerometers to determine the position, velocity, and attitude of a system by integrating this data over time. The non-linearities inherent to attitude require these integrations to occur at very high rates. To minimize the computational requirements of the user system, most inertial navigation systems output what are known as coning and sculling integrals which are integrated internally and can then be used at lower rates for full state integration.

Coning and Sculling Integrals

The coning and sculling integrals are integration processes that properly account for the coning and sculling motion and are valid despite the non-linearities inherent to real-world motion. Typically, the coning and sculling integrals are performed at higher rates, which allows the integration of the velocity and angular rate outputs to be performed at much lower speeds, thus reducing the amount of bandwidth needed to process the data. The coning integral provides a principal rotation vector known as Delta-Theta, $\Delta\boldsymbol{\theta}$, while the sculling integral generates a Delta-Velocity, $\Delta\boldsymbol{v}$, over specified amount of time, $\Delta t$.

These techniques have the advantage of providing the change in orientation and change in velocity over an arbitrary amount of time with higher accuracy as compared to averaging the accelerations or angular rates over longer time steps. In addition, the coning and sculling integrals provide the benefit of lower computational complexity as compared to other algorithms, such as the quaternion attitude update.

Attitude Integration

The Delta-Theta output from the coning integral is easily combined with quaternions to produce a continuously updated attitude estimate. An updated quaternion value ($\boldsymbol{q}_{k+1}$) is computed from the previous quaternion value ($\boldsymbol{q}_{k}$) using Equation \ref{eq:aiqu}. This equation assumes the scalar term of the quaternion is $q_4$ and that $\Delta\boldsymbol{\theta}$ is provided in radians.

\begin{equation}\label{eq:aiqu} \boldsymbol{q}_{k+1} = \begin{bmatrix} \cos\gamma[I_{3\times3}] - \left[\boldsymbol{\Psi}^\times\right] & \boldsymbol{\Psi} \\ -\boldsymbol{\Psi}^\intercal & \cos\gamma \end{bmatrix}\boldsymbol{q}_k\end{equation}

where

\begin{equation*} \gamma = \frac{\|\Delta\boldsymbol{\theta}\|}{2}\qquad \boldsymbol{\Psi} = \begin{cases}\frac{\sin\gamma}{2\gamma}\Delta\boldsymbol{\theta}& \gamma\ge1\mathrm{e}{-5}\\ \frac{1}{2}\Delta\boldsymbol{\theta} & \gamma<1\mathrm{e}{-5} \end{cases} \qquad \left[\boldsymbol{\Psi}^\times\right] = \begin{bmatrix} 0 & -\Psi_3 & \Psi_2 \\ \Psi_3 & 0 &{-\Psi}_1 \\ {-\Psi_2} & \Psi_1 & 0 \end{bmatrix}\end{equation*}

Because no computation can be achieved with perfect numerical precision, it is recommended that the updated quaternion is normalized per Equation \ref{eq:aiqun} to ensure that this updated quaternion value remains unit length.

\begin{equation}\label{eq:aiqun} \hat{\boldsymbol{q}}_{k+1} = \frac{\boldsymbol{q}_{k+1}}{\|\boldsymbol{q}_{k+1}\|}\end{equation}

Once the quaternion has been calculated from the Delta-Theta, this orientation can then be converted into the desired attitude representation. For more information about the quaternion and different attitude representations, refer to Sections 2.3 and 2.4.

Position and Velocity Integration

Information about an object's position can be obtained by integrating the velocity solution over a discrete period of time. Given a Delta-Velocity output in the body frame (${^B}\Delta\boldsymbol{v}$), the attitude at the start of the integration step is used to transform it into the inertial frame (typically NED):

\begin{equation} {^I}\Delta\boldsymbol{v}=[C]^{\intercal}_k{^B}\Delta\boldsymbol{v}\end{equation}

The inertial frame Delta-Velocity must then be corrected for gravity ($\boldsymbol{g}$) and the Coriolis term arising from Earth's angular rate ($\boldsymbol{\omega}_\oplus$) and the current velocity estimate ($\boldsymbol{v}_k$), each in the inertial frame.

\begin{equation}\begin{split} \Delta\boldsymbol{v}_{g/cor}&=\Delta t\left(\boldsymbol{g}-2\boldsymbol{\omega}_{\oplus} \times \boldsymbol{v}_k\right) \\ \Delta\boldsymbol{v}_{c}&={^I}\Delta\boldsymbol{v}+\Delta\boldsymbol{v}_{g/cor} \end{split}\end{equation}

VectorNav sensors can be configured to output the term $\Delta\boldsymbol{v}_c$ directly, utilizing the onboard Kalman filter attitude estimates, eliminating these steps. Once the corrected Delta-Velocity is available, the position and velocity can be easily updated via Equation \ref{eq:intposvel}.

\begin{equation}\begin{split} \boldsymbol{v}_{k+1}&=\boldsymbol{v}_{k}+\Delta\boldsymbol{v}_{c}\\ \boldsymbol{p}_{k+1}&=\boldsymbol{p}_{k}+\Delta t\boldsymbol{v}_{k} + \frac{\Delta t}{2} \Delta\boldsymbol{v}_{c}\end{split}\label{eq:intposvel}\end{equation}