LSM6DSOX Kalman Filter Sensor Fusion

Kalman Filter implementation on IMU

This repo entails an implementation of the Discrete Kalman Filter on the 6-Axis IMU LSM6DSOX. The goal is to output an angle that can avoid the instabilities or inaccuracies of the gyroscope and accelerometer by itself.

Introduction

This project aims to develop a system capable of generating precise pitch and roll angles. While these angles can be obtained from the accelerometer or gyroscope in the LSMD6SOX IMU used for this project, both sensors produce noisy data. Specifically:

• The gyroscope is susceptible to gyro drift, leading to a gradual increase in angle due to the integration of errors.
• Accelerometers, although accurate at rest, become noisy when exposed to vibrations or motion.

Considering the mentioned flaws, it becomes evident that the gyroscope provides accurate short-term data, whereas the accelerometer offers accurate long-term data. With this understanding, a Discrete Kalman Filter can be employed to fuse the two datasets, yielding a more accurate output.

The Kalman filter is an algorithm used for estimating the state of a system in the presence of noisy measurements. It works by iteratively predicting the next state based on a dynamic model and comparing it with actual measurements. The filter optimally combines predictions and measurements, dynamically adjusting their contributions based on their uncertainties. This allows the Kalman filter to provide a more accurate and stable estimate of the system's true state, making it widely used in applications like sensor fusion for robotics, navigation, and control systems.

Components

• RTIC
  • rust framework for interrupt-driven concurrency
• Timer 2
  • generates 15ms interrupts to update Kalman Filter with new IMU data
• LSM6DSOX driver
  • Currently only i2c capabilities provided
  • https://github.com/KavinTheG/LSM6DSOX-Embedded-Rust-Driver/

Queues

When using RTIC mutexes, the Kalman Filter performance suffered. I believe this is due to the fact that the filter is not able to get an accurate value of the delta time, since it has to wait for the mutex. To mitigate this, a queue was used instead.

NOTE

rustc 1.78.0-nightly (2024-02-08) is used. For some reason, rustc 1.78-nightly (2024-02-23) does not work well with embedded-hal 0.2, as the program halts when trying to read/write from the i2c bus.

Results

The following graphs displays the values of the roll with the given parameters. The first image corresponds to a slow increase in the IMU's roll motion, while the second image corresponds to slight vibrations of the imu.

q_ang = 0.001,
q_gyro = 0.1,
r = 0.1,

q_ang = 0.001,
q_gyro = 0.01,
r = 0.03,

References

• https://github.com/TKJElectronics/KalmanFilter/blob/master/Kalman.cpp

• An Introduction to the Kalman Filter,
  Greg Welch and Gary Bishop 1994,
  https://api.semanticscholar.org/CorpusID:9209711