LSM6DSOX Gyroscope Accelerometer Kalman Filter

[ata_tr]

Hello everyone,

I am working on a project using the LSM6DSOX (Accelerometer Gyroscope)and an Arduino UNO for position monitoring. After doing some research, I learned that using "pitch" and "roll" values instead of raw data from the sensor can improve the accuracy for my application. I’ve also been looking into "Kalman" and "Complementary" filters to reduce noise in the data.

However, as a beginner in this field, I have been struggling to ensure the accuracy of my results, and I haven't been able to find a similar project to use as a reference. Most of the projects I came across use sensors like the MPU6050, but I am specifically working with the LSM6DSOX.

I developed a code and have received some data, but I'm still unsure about the accuracy and would greatly appreciate any feedback or suggestions. Below is my code, and I would be thankful if anyone can point out any improvements, particularly for the following:

1. Kalman Filter Tuning: Is my implementation of the Kalman filter correct? I’ve set the initial angles using the accelerometer, but I’m not sure if the filter is giving me accurate results.
2. Sensor Calibration: Am I properly handling the accelerometer and gyroscope data? Should I apply any additional calibration steps to improve the accuracy of the pitch and roll angles?
3. Filter Performance: How can I improve the performance of the Kalman and complementary filters for more stable and accurate data?

Here’s my code:

#include <Wire.h>
#include <Adafruit_LSM6DSOX.h>
#include "Kalman.h"  // Source: https://github.com/TKJElectronics/KalmanFilter

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead

// Create the Kalman instances
Kalman kalmanX;
Kalman kalmanY;

// IMU Data
float accX, accY, accZ;
float gyroX, gyroY, gyroZ;

double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY;   // Calculated angle using a Kalman filter

uint32_t timer;


Adafruit_LSM6DSOX sox; // Create the sox object
void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit LSM6DSOX test!");

  if (!sox.begin_I2C()) {
    // if (!sox.begin_SPI(LSM_CS)) {
    // if (!sox.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) {
    // Serial.println("Failed to find LSM6DSOX chip");
    while (1) {
      delay(10);
    }
  }

  // Configure sensor settings
  sox.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
  sox.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS );  // ±250 dps range
  sox.setAccelDataRate(LSM6DS_RATE_12_5_HZ);  // 1kHz accelerometer sampling rate
  sox.setGyroDataRate(LSM6DS_RATE_12_5_HZ);  // 1kHz gyroscope sampling rate

  // Initialize angles using accelerometer data
  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temp;
  sox.getEvent(&accel, &gyro, &temp);

  // Calculate initial roll and pitch from accelerometer
#ifdef RESTRICT_PITCH
  double roll = atan2(accel.acceleration.y, accel.acceleration.z) * RAD_TO_DEG;
  double pitch = atan(-accel.acceleration.x / sqrt(accel.acceleration.y * accel.acceleration.y + accel.acceleration.z * accel.acceleration.z)) * RAD_TO_DEG;
#else
  double roll = atan(accel.acceleration.y / sqrt(accel.acceleration.x * accel.acceleration.x + accel.acceleration.z * accel.acceleration.z)) * RAD_TO_DEG;
  double pitch = atan2(-accel.acceleration.x, accel.acceleration.z) * RAD_TO_DEG;
#endif

  // Set starting angles for Kalman filter
  kalmanX.setAngle(roll);
  kalmanY.setAngle(pitch);
  gyroXangle = roll;
  gyroYangle = pitch;
  compAngleX = roll;
  compAngleY = pitch;

  timer = micros();
}

void loop() {
  // Read the sensor data
  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temp;
  sox.getEvent(&accel, &gyro, &temp);

  // Calculate delta time
  double dt = (double)(micros() - timer) / 1000000;
  timer = micros();

  // Get accelerometer data
  accX = accel.acceleration.x;
  accY = accel.acceleration.y;
  accZ = accel.acceleration.z;

  // Get gyroscope data
  gyroX = gyro.gyro.x;
  gyroY = gyro.gyro.y;
  gyroZ = gyro.gyro.z;

  // Calculate roll and pitch from accelerometer data
#ifdef RESTRICT_PITCH
  double roll = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else
  double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  // Convert gyroscope data to degrees per second
  double gyroXrate = gyroX / 131.0;
  double gyroYrate = gyroY / 131.0;

#ifdef RESTRICT_PITCH
  // Kalman filter for roll and pitch
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    compAngleX = roll;
    kalAngleX = roll;
    gyroXangle = roll;
  } else {
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Kalman filter for roll
  }

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate for restricted accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Kalman filter for pitch
#else
  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
    compAngleY = pitch;
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else {
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Kalman filter for pitch
  }

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate for restricted accelerometer reading
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Kalman filter for roll
#endif

  gyroXangle += gyroXrate * dt; // Gyro angle without filter
  gyroYangle += gyroYrate * dt;

  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Complimentary filter for roll
  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; // Complimentary filter for pitch

  // Print the results to serial monitor
  Serial.print("Roll: ") ; Serial.print(roll); Serial.print("\t");
  Serial.print("Gyro_Roll: ") ; Serial.print(gyroXangle); Serial.print("\t");
  Serial.print("Comp_Roll: ") ; Serial.print(compAngleX); Serial.print("\t");
  Serial.print("Kalman_Roll: ") ; Serial.print(kalAngleX); Serial.print("\t");

  Serial.print("\t");

  Serial.print("Pitch: ") ; Serial.print(pitch); Serial.print("\t");
  Serial.print("Gyro_Pitch: ") ; Serial.print(gyroYangle); Serial.print("\t");
  Serial.print("Comp_Pitch: ") ; Serial.print(compAngleY); Serial.print("\t");
  Serial.print("Kalman_Pitch: ") ; Serial.print(kalAngleY); Serial.print("\t");

  Serial.print("\r\n");

  delay(1000);
}

I would really appreciate any advice or suggestions on the following:

1. Is the Kalman filter setup correct? Are there any improvements I can make to better tune the filter?
2. How can I ensure more accurate angle calculations, especially when integrating accelerometer and gyroscope data?
3. If you've used the LSM6DSOX with similar filters, are there any best practices or common pitfalls I should watch out for?

Thank you in advance for your help!