How it works: general overview

Introduction

This quick overview of underlying algorithm is given to help user understand how the library works and how to use it and adapt it to his needs thanks to tunable parameters.

Separation between local and global attitude estimation

To ensure robustness of the algorithm, and to be usable in any situation, the algorithm is based on a separation between local and global attitude estimation. Thie difference between the two is for the yaw that is aligned with magnetometer for global estimate and initilized at 0 and integrated for local estimation. The interest of this separation is to avoid magnetometer disturbances impact on local estimation.

Even if the local attitude is slightly corrected by magnetometer, it should ensure continuous and reliable attitude estimation every times, and can be used for control purposes.

The same principle could be applied for all the variables ensuring reliable control (using local variables), and global guidance (using global variables).

General overview

The algorithm used is extended Kalman filter (EKF) fusing data from 4 sensors actually

  • Accelerometer

  • Gyrometer

  • Magnetometer

  • Barometer

The states estimated are:

  • Angle-axis rotation error vector in body frame: \(\mathbf{\theta}_{\mathbf{xyz}}\)

  • gyrometer bias vector in body frame: \(\mathbf{b}_{\mathbf{xyz}}\)

  • distance covered verticaly in NED frame: \(\mathbf{d}_{\mathbf{z}}\)

  • vertical speed in NED frame: \(\mathbf{v}_{\mathbf{z}}\)

The main steps of the algorithm are:

Main operation

  • Prediction step:

    • the algorithm integrates gyrometer rotation speeds at the highest frequency possible (500Hz) in order to propagate attitude estimation

    • the algorithm integrates accelerometer data at the highest frequency possible (500Hz) in order to propagate speed and altitude estimation

  • Correction step:

    • the algorithm corrects attitude estimation with accelerometer data at the highest frequency possible (500Hz)

    • the algorithm corrects altitude estimation with barometer data at the barometer data frequency (71.4Hz)

Separated operations

Magnetometer handling:

  • To avoid magnetometer disturbances potentially catastrophic impact on main estimation, a separated algorithm is used to estimate local to global transformation (\(\mathbf{Q}_{\mathbf{local to global}}\)). This quaternion absorbs part of the magnetometer disturbances and ensure that local quaternion is unaffected.

Barometer bias:

  • While the INS is considered immobile, a simple complementary filter allows for barometer bias estimation (all measurements are considered as bias).

Accelerometer z bias:

  • While the INS is considered immobile, a complementary filter allows for accelerometer z bias estimation. The error is the difference between the projected gravity vector in body frame and z measure (which is supposed to be equal to gravity).