Crate sensor_fusion 

sensor-fusion Rust Crate

This crate contains sensor fusion algorithms to combine output from a gyroscope, accelerometer, and optionally a magnetometer to give output that has less uncertainty than the output of the individual sensors.

Six sensor fusion implementations are available:

1. Complementary Filter
2. Mahony Filter
3. Madgwick Filter
4. Altitude Kalman Filter
5. 2D Position Kalman Filter.
6. 3D Position Kalman Filter.

Simple example

Here’s a simple example that uses a Madgwick filter to calculate the orientation by fusing accelerometer and gyro values:

use sensor_fusion::{MadgwickFilterf32, SensorFusion};
use vqm::Vector3f32;

fn main() {
    let mut madgwick = MadgwickFilterf32::new();
    let dt = 0.001; // 1 millisecond

    // Mock sensor values, gyro converted to rps (values normally read from IMU).
    let gyro_rps = Vector3f32::new(90.0, 8.0, 10.0).to_radians();
    let acc = Vector3f32::new(0.1, 0.2, 0.9);

    // Fuse acc and gyro values
    let orientation = madgwick.fuse_acc_gyro(acc, gyro_rps, dt);

    let (roll, pitch, yaw) = orientation.calculate_euler_angles_degrees();

    // Print out the Euler angles.
    println!("pitch={}, roll={}, yaw={}", pitch, roll, yaw);
}

Madgwick Filter

The Madgwick filter has been refactored to be more computationally efficient (and so faster) than the standard version used in many implementations, see Optimization below.

The Madgwick filter also supports three-way fusing of accelerometer, gyroscope, and magnetometer readings:

let orientation = madgwick.fuse_acc_gyro_mag(acc, gyro_rps, mag, dt);

Mahony filter

The Mahony filter has the same interface as the Madgwick filter:

use crate::sensor_fusion::{MahonyFilterf32,SensorFusion};

let mut mahony = MahonyFilterf32::default();

let orientation = mahony.fuse_acc_gyro(acc, gyro_rps, dt);

The Mahony filter does not support 3-way fusion using a magnetometer.

Complementary filter

This has been implemented for reference. It is not recommended for general use: both the Mahony filter and the Madgwick filter are faster and don’t suffer from gimbal lock.

Method call interface

The FuseAccGyro and FuseAccGyroMag traits allow method-call syntax to be used:

use {crate::sensor_fusion::{FuseAccGyro, FuseAccGyroMag, MadgwickFilterf32}, vqm::Vector3f32};

let dt = 0.001;
let mut madgwick = MadgwickFilterf32::default();
let acc = Vector3f32::default();
let gyro = Vector3f32::default();
let mag = Vector3f32::default();

let orientation = (acc, gyro).fuse_acc_gyro_using(&mut madgwick, dt);
// or
let orientation = (acc, gyro, mag).fuse_acc_gyro_mag_using(&mut madgwick, dt);

Kalman filter

Three Kalman filters are provided:

1. KalmanFilterZ - a filter to estimate altitude and vertical speed.
2. KalmanFilterXY - a filter to estimate 2D position and velocity.
3. KalmanFilterXYZ - a filter to estimate 3D position and velocity.

3D Kalman filter

When using a Kalman filter for 3D there are two possible implementation choices:

1. Use a KalmanFilterXYZ.
2. Use a spilt architecture: use a KalmanFilterXY for horizontal position, and a KalmanFilterZ for vertical position.

Feature	Split Architecture (1D + 2D)	Unified 3D Architecture
Computational Cost	Lower (2×2 and 4×4 matrices )	Higher (6×6 and 9×9 matrices)
Sensor Coupling	Independent (Barometer noise cannot corrupt horizontal accuracy)	Coupled (Vertical errors can bleed into horizontal)
Tuning Complexity	Easier (can tune vertical and horizontal separately)	Harder
Fault Tolerance	Higher	Lower

For vehicles that stay upright, or tilt minimally (eg a walker, a car, or a cinematic quadcopter) the split approach is a good one.

For vehicles such as acrobatic quadcopters or rockets, where pitching and rolling movements are common, the combined approach is better.

Position Kalman filter internals

A Kalman filter works by predicting an object’s state using a physical model of the object. So, for example, to predict an object’s position velocity it uses the measured acceleration and the kinematic equations s = u*t + 0.5*a*t^2 and v = u + a*t.

When it obtains a reading for the object’s actual position (for example from a GPS) it uses that reading to correct the prediction (essentially by taking a weighted average of the predicted state and the reading).

The naive implementation of a 3D Position Kalman filter is extremely computationally intensive.

To predict 3D position using a gyroscope, accelerometer, you need a massive 15x15 covariance matrix, even in 2D you need a 10x10 matrix.

Furthermore, since combining gyroscope and accelerometer measurements is an non-linear operation, a Linear Kalman Filter (LKF), cannot be used: an Extended Kalman Filter (EKF) is required. An EKF is even more computationally intensive: as well as doing the multiplication of these large matrices it also needs to calculate Jacobians.

Fortunately there are a number of things that can be done to reduce this computational load.

Firstly we remove the responsibility for calculating the orientation from the Kalman filter: we use a another sensor fusion filter (ie a Mahony or Madgwick) filter to do this.

This reduces the covariance matrix to more manageable size of 9x9. What’s more, prediction using a accelerometer alone is a linear operation, so we can use a LFK rather than an EKF.

   ┌─────┐  Acc/Gyro  ┌─────────────────┐
   │ IMU ├──────────► │ MADGWICK FILTER ├──► Orientation Quaternion
   └─────┘            └─────────────────┘
      │
      │ Acc
      │            ┌────────────────────────┐
      └──────────► │ POSITION KALMAN FILTER ├──► Position Vector
                   └────────────────────────┘    Velocity Vector
                                │
   GPS & Barometer  ──►─────────┘

But we are still not out of the woods: multiplying two 9x9 matrices requires 729 individual arithmetic operations, matrix inversion even more. And the Kalman filter predict and correct steps each require several matrix operations.

TODO: further explanation of Kalman filter internals in readme.

SIMD support

SIMD support (for the f32 variants) can be enabled with the simd feature.

It is currently experimental, so if you used SIMD make sure you benchmark to show that you are indeed getting a performance improvement over the non-SIMD version.

This uses portable simd, which requires the nightly compiler, since it is still unstable in rust.

This can be invoked using rustup, eg:

rustup run nightly cargo build --features simd --target thumbv8m.main-none-eabi

Madgwick Filter Optimization {#opt}

Classically, the calculation of the Madgwick gradient descent corrective step involves multiplication of a vector by a matrix, this involves a total of 54 arithmetic operations for the acc/gyro case.

However, because both the matrix and vector contain zero elements, and because there is some symmetry in the matrix, the calculation can be refactored to use fewer arithmetic operations.

Indeed it can be reduced to a total of 31 arithmetic operations. By using SIMD this can be further reduced to 16 operations (11 scalar and 5 SIMD operations). See below.

I haven’t yet benchmarked this Rust implementation, but the original C++ version of MadgwickFilter::update_orientation (equivalent to madgwick.fuse_acc_gyro) took or under 20 microseconds on a 240 MHz ESP32 S3.

The aim is to be able to run sensor fusion a part of a Gyro/PID loop running at 8kHz. That means everything (including reading the IMU, filtering the output, performing sensor fusion and calculation the motor outputs using a PID controller) needs to run in 125 microseconds. This is currently looking achievable.

// Classic version
//
// total:
//      54 arithmetic operations (35 multiplications, 19 additions/subtractions)
//
fn madgwick_step(q: Quaternionf32, a: Vector3f32) -> Vector4f32 {
    let M = Matrix4x4f32::new([ // 10 multiplications
        -2.0*q.x, 2.0*q.w,      0.0, 0.0,
         2.0*q.y, 2.0*q.z, -4.0*q.w, 0.0,
        -2.0*q.z, 2.0*q.y, -4.0*q.x, 0.0,
         2.0*q.w, 2.0*q.x,      0.0, 0.0
    ]);

    let v = Vector4f32::new( // 9 multiplications, 7 additions/subtractions
        2.0*(      q.w*q.y - q.z*q.x) - a.x,
        2.0*(      q.z*q.w + q.x*q.y) - a.y,
        2.0*(0.5 - q.w*q.w - q.x*q.x) - a.z,
        0.0
    );

    M * v // 16 multiplications, 12 additions
}

// Refactored version
//
// total:
//      31 arithmetic operations (19 multiplications, 12 additions/subtractions)
// when converted to SIMD this becomes:
//      16 operations (7 multiplications, 4 additions, 3 vector multiplications, 2 vector additions)
//
fn madgwick_step(q: Quaternionf32, a: Vector3f32) -> Quaternionf32 {
    let wz_common = 2.0 * (q.x * q.x + q.y * q.y); // 3 multiplications, 1 addition
    let xy_common = 2.0 * (q.w * q.w + q.z * q.z - 1.0 + 2.0 * wz_common + a.z); // 4 multiplications, 3 additions/subtractions

    Quaternionf32 { // 12 multiplications, 8 additions/subtractions
        w: q.w * wz_common + q.y * a.x - q.x * a.y,
        x: q.x * xy_common - q.z * a.x - q.w * a.y,
        y: q.y * xy_common + q.w * a.x - q.z * a.y,
        z: q.z * wz_common - q.x * a.x - q.y * a.y,
    }
}

Original implementation

I originally implemented this crate as a C++ library: Library-SensorFusion.

License

Licensed under either of:

• Apache License, Version 2.0 (LICENSE-APACHE or http://www.apache.org/licenses/LICENSE-2.0)
• MIT license (LICENSE-MIT or http://opensource.org/licenses/MIT)

at your option.

Structs

Anchor2d

Anchor3d

ComplementaryFilter

Complementary filter.

KalmanFilterXY

Linear Kalman Filter in 2 dimensions.

KalmanFilterXYWithSensors

KalmanFilterXYZ

Linear Kalman Filter in 3 dimensions.

KalmanFilterXYZWithSensors

KalmanFilterZ

KalmanFilterZWithSensors

MadgwickFilter

Madgwick filter.

MahonyFilter

Mahony filter.

Traits

FuseAccGyro

Trait to allow sensor fusion filters to be used with method-call syntax, ie:
let orientation = (acc, gyro_rps).fuse_acc_gyro_using(&mut madgwick, dt);

FuseAccGyroMag

Trait to allow sensor fusion filters to be used with method-call syntax, ie:
let orientation = (acc, gyro_rps).fuse_acc_gyro_using(&mut madgwick, dt);

SensorFusion

Common interface for the sensor fusion filters (Madgwick, Mahony, complementary).

SensorFusionMath

Math functions for SensorFusion, using SIMD accelerations for f32.

Functions

trilaterate_2d

Trilaterates a 2d position using the least squares method.

trilaterate_3d_weighted

Trilaterates a 3D position using the least squares method.

Type Aliases

Anchor2df32

2-dimensional anchor of f32 values

Anchor2df64

2-dimensional anchor of f64 values

Anchor3df32

3-dimensional anchor of f32 values

Anchor3df64

3-dimensional anchor of f64 values

ComplementaryFilterf32

Complementary filter for f32

ComplementaryFilterf64

Complementary filter for f64

KalmanFilterXYWithSensorsf32

f32 variant of PositionKalmanFilterExtended.

KalmanFilterXYZWithSensorsf32

f32 variant of KalmanFilterXYZWithSensors.

KalmanFilterXYZf32

f32 variant of PositionKalmanFilter0.

KalmanFilterXYf32

f32 variant of PositionKalmanFilterXY.

KalmanFilterZWithSensorsf32

f32 variant of AltitudeKalmanFilter.

KalmanFilterZWithSensorsf64

f64 variant of AltitudeKalmanFilter.

KalmanFilterZf32

f32 variant of AltitudeKalmanFilter.

KalmanFilterZf64

f64 variant of AltitudeKalmanFilter.

MadgwickFilterf32

Madgwick filter for f32

MadgwickFilterf64

Madgwick filter for f64

MahonyFilterf32

Mahony filter for f32

MahonyFilterf64

Mahony filter for f64