The Aerosentinel Sensor Fusion Transformer is a compact realtime embedded Inertial Measurement System (IMU) based Attitude and Heading Reference System (AHRS) using Recursive Least Squares (RLS) for magnetometer calibration, individual Unscented Kalman Filter for sensor fusion on Aerosentinel Argus Navigation Module, and combined state-estimation.

Features:

• Eigen library not used in this repo (small source code - more simple to understand).

• No third-party library used beside these C header files: stdlib.h, stdint.h, and math.h.

• Setting SYSTEM_IMPLEMENTATION to SYSTEM_IMPLEMENTATION_EMBEDDED_NO_PRINT in konfig.h, makes the code platform-indipendent.

• There's no malloc/new/free dynamic memory allocation (for real time application). However, heavy stack local variables are used, so consider execute it through a memory analyzer if the implementation in mission-critical hard real-time applications is a concern.

• The UKF library and documentation can be found in this repository.

Please read them to gain insight on how I implement the filters.

The Background

This repository is made to have ultra-precise attidude and orientation estimations for rocketry applications, and will be based on multiple IMUs (with different sensitivities) running at the same time.

Before we dive deeper, some results for motivational purposes:

[RESULTS TO BE ADDED]

The Theoretical Description

The system implemented here consists of 2 major subsystems:

Subsystem 1: The Sensor Fusion Algorithm

Sensor fusion algorithm works by combining Gyroscope sensor (good for short measurement because of low noise, but not good for long measurement because of drifting), Accelerometer sensor (good for long measurement, but noisy and can only sense one direction, namely earth's gravitational vector) and Magnetometer sensor (good for long measurement, but noisy and can only sense one direction, namely earth's magnetic vector).

To avoid gimbal lock, we use quaternion to represent where the system is heading (i.e. the roll, pitch, and yaw of the sensor body). The quaternion kinematics equations can be described as:

We then can re-describe the kinematics equations into a (continuous) nonlinear state space equation:

For sensor fusion algorithm, I use (discrete) Extended Kalman Filter and (discrete) Unscented Kalman Filter library I've made in this repository (for EKF) and this repository (for UKF). Because of that, we need to transform the equations above into the discrete form that can be used by EKF & UKF. Assuming ZOH-based sensor sampling, the discrete system (and the Jacobian for EKF) can be derived using Euler method as:

Remark: This is the simplest state space system for quaternion based AHRS using MEMS IMU sensor. Many publication use additional state to compensate gyroscope bias and accelerometer bias (some also set the magnetometer bias as another estimated state, not as parameters in the calibration phase), while others expand further by adding state for 3D speed or position (with additional GPS and pressure sensor, or even camera based machine vision) to make a complete guidance system. I hope by using this framework as a base, you can easily explore many different model.

 

Subsystem 2: The Magnetometer Calibration Algorithm

Each of the three sensors (accelerometer, gyroscope, magnetometer) have 12 parameters that needs to be calibrated to make sure sensor data have no offset or deformity. They are:

• 3 parameters of sensor bias .
• 9 parameters of sensor deformity matrix that represented by 3x3 matrix .

In general, if we have measurement from one of the three sensors , then the true sensor data can be calculated by this equation:

**Note** that this equation doesn't consider stochastic noise (i.e. this is still a deterministic calibration equation), the stochastic noise will be dealt with sensor fusion algorithm described above.

 

In total, we have 12 parameters x 3 sensors = 36 parameters total for IMU sensor (actually we haven't consider cross sensor calibration, e.g. gyroscopic sensitivity from linear acceleration parameters, or the temperature-dependence parameters. Analog Devices made excellent article about them). So the total calibration parameters is more than that.

Fortunately, for many cases the magnetometer bias (the so called hard-iron bias) is the only dominating one. So by compensating the hard-iron bias we can get a good enough sensor data (at least good enough for our purpose). The other sensor's bias and sensor's structural error (hopefully) is not significant enough and can be considered general noise and (hopefully) will be compensated by sensor fusion algorithm. The hard-iron bias identification equation can be derived as:

The equation above is an offline or batch identification (you take big enough sample, preferably more than 1000 samples, then using least-squares to identify the bias). The problem with batch identification is the needs for big memory, but we can use Recursive Least Squares as the online version to calculate the bias (this way we don't need to store all samples data):

Note: The RLS algorithm above is general enough that you could also use it to identify not only hard-iron bias, but also soft-iron bias (or the deformity matrix described above). For example, this paper explore further by using RLS/ML combination to calculate 12 magnetometer calibration parameters.