How it works: sensors usage

The 10 DOF sensors fusion algorithm can get corrections from the following sensors:

  • Accelerometer

  • Barometer

  • Magnetometer

The state vector is composed of the following variables:

\[\begin{split}X_k = \left(\begin{array}{c} \mathbf{\theta x}_{\mathbf{err}_{k}}\\ \mathbf{\theta y}_{\mathbf{err}_{k}}\\ \mathbf{\theta z}_{\mathbf{err}_{k}}\\ b_{\mathbf{x}_{k}}\\ b_{\mathbf{y}_{k}}\\ b_{\mathbf{z}_{k}}\\ V_{\mathbf{NED}_{k}}\\ D_{\mathbf{NED}_{k}} \end{array}\right)\end{split}\]

In the following we will note \(\mathbf{q}\) the quaternion discribing body orientation relative to local frame.

Accelerometer

The accelerometer measures gravity projected on the body frame which means it provides the acceleration vector. This hypothesis is only true if the accelerometer does not measure non gravitational accelerations (movement).

\[\begin{split}\left(\begin{array}{c} a_x\\ a_y\\ a_z \end{array}\right) = \overline{\mathbf{q_{real}}} \otimes \left(\begin{array}{c} 0\\ 0\\ 1 \end{array}\right) \otimes \mathbf{q_{real}}\end{split}\]

With doing the same operation with our estimated quaternion, we can obtain angle-axis rotation error vector in body frame, usable in our kalman filter:

\[\begin{split}g_{body} = \overline{\mathbf{q_{est}}} \otimes \left(\begin{array}{c} 0\\ 0\\ 1 \end{array}\right) \otimes \mathbf{q_{est}}\end{split}\]

The cross product between accelerometer normalized measurements and gravity in body frame gives us the angle-axis rotation error vector in body frame:

\[\begin{split}\left(\begin{array}{c} \mathbf{\theta x}_{\mathbf{err}}\\ \mathbf{\theta y}_{\mathbf{err}}\\ \mathbf{\theta z}_{\mathbf{err}} \end{array}\right) = \left(\begin{array}{c} g_{{z}_{body}}\,a_y-g_{{y}_{body}}\,a_z\\ g_{{x}_{body}}\,a_z-g_{{z}_{body}}\,a_x\\ g_{{y}_{body}}\,a_x-g_{{x}_{body}}\,a_y \end{array}\right)\end{split}\]

Because of the possibility of non gravitational acceleration on the accelerometer, a noise difference between accelerometer norm and 1 is used to modify dynamically sensors noise covariance matrix and model covariance matrix (the thrust we consider for the prediction). This way we can change the part of the sensor correction vs the prediction.

If the noise is too high, the accelerometer correction will be ignored.

These parameters are configurable with the tunable parameters (SRX_MODEL_INS_10_DOF::modelParameters)

Magnetometer

Magnetometer is not used in the same way as the accelerometer because of the high sensitivity to disturbances. It is used to correct the quaternion of describing local frame attitude relative to global frame. The local quaternion receive slight yaw corrections from measurements.

After tilt compensation, the absolute heading is given by:

\[\psi = \arctan2(-B_y,B_x) + dec\]

  • \(B_x\), \(B_y\) are the magnetometer measurements in horizontal frame (tilt compensated from body)

  • \(dec\) is the magnetic declination at at local position

The global heading is given by:

\[ \begin{align}\begin{aligned}\psi_{err} = \psi_{meas} - (\psi_{est} + \psi_{offset})\\\begin{split}\\\end{split}\\\begin{split}q_{{localToGlobal}_{k+1}} = q_{{localToGlobal}_{k}} \otimes \left(\begin{array}{c} \cos(-k \cdot \psi_{err}/2)\\ 0\\ 0\\ \sin(-k \cdot \psi_{err}/2) \end{array}\right)\end{split}\end{aligned}\end{align} \]

The \(\psi_{offset}\) is a dynamic offset that is updated when difference between magnetometer heading and estimated heading is too high. The \(q_{localToGlobal}\) will follow this discontinuity (and global heading by the way), but local heading will remain continuous.

Barometer

Barometer provides pressure measurements that can be used to estimate vertical altitude. The relation with state spaces variables is:

\[D_{\mathbf{NED}_{err}} = -(alti_{meas} - alti_{init} - alti_{bias}) - alti_{est}\]

The role of immobility

The INS can take advantage of the immobility of the system to improve the estimation of the state vector:

  • When immobile, we consider that the gyrometer measurements are only bias and input this “measure” to kalman filter.

  • Barometer bias is estimated considering all \(\Delta alti\) measurements are bias when immobile.

  • Accelerometer Z bias is estimated with the difference of gravity projection in body frame and \(a_z\) measurements.