Untitled

#include <Arduino.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;

// Variables to store calculated angles
float pitch = 0;
float roll = 0;
float yaw = 0;

// Arrays to store offsets
float accelOffset[3] = {0, 0, 0}; // X, Y, Z offsets for accelerometer
float gyroOffset[3] = {0, 0, 0};  // X, Y, Z offsets for gyroscope

// Previous time in milliseconds
unsigned long previousTime = 0;

// Kalman filter variables
float Q_angle = 0.001;
float Q_bias = 0.003;
float R_measure = 0.03;

float angle = 0; // Reset the angle
float bias = 0; // Reset bias
float rate = 0; // Unbiased rate calculated from the rate and the calculated bias
float P[2][2] = {{0, 0}, {0, 0}}; // Error covariance matrix

float kalmanFilter(float newAngle, float newRate, float dt) {
  // Discrete Kalman filter time update equations - Time Update ("Predict")
  // Update xhat - Project the state ahead
  rate = newRate - bias;
  angle += dt * rate;

  // Update estimation error covariance - Project the error covariance ahead
  P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;

  // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
  // Calculate Kalman gain - Compute the Kalman gain
  float S = P[0][0] + R_measure; // Estimate error
  float K[2]; // Kalman gain - This is a 2x1 vector
  K[0] = P[0][0] / S;
  K[1] = P[1][0] / S;

  // Calculate angle and bias - Update estimate with measurement zk (newAngle)
  float y = newAngle - angle; // Angle difference
  angle += K[0] * y;
  bias += K[1] * y;

  // Calculate estimation error covariance - Update the error covariance
  float P00_temp = P[0][0];
  float P01_temp = P[0][1];

  P[0][0] -= K[0] * P00_temp;
  P[0][1] -= K[0] * P01_temp;
  P[1][0] -= K[1] * P00_temp;
  P[1][1] -= K[1] * P01_temp;

  return angle;
}

void calibrateMPU6050() {
  const int sampleSize = 1000;
  float accelSum[3] = {0, 0, 0}; // X, Y, Z sum for accelerometer
  float gyroSum[3] = {0, 0, 0};  // X, Y, Z sum for gyroscope

  for (int i = 0; i < sampleSize; i++) {
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    accelSum[0] += a.acceleration.x;
    accelSum[1] += a.acceleration.y;
    accelSum[2] += a.acceleration.z;
    gyroSum[0] += g.gyro.x;
    gyroSum[1] += g.gyro.y;
    gyroSum[2] += g.gyro.z;

    delay(3);
  }

  for (int i = 0; i < 3; i++) {
    accelOffset[i] = accelSum[i] / sampleSize;
    gyroOffset[i] = gyroSum[i] / sampleSize;
  }

  // Adjust the accelerometer Z offset to account for gravity (1g)
  accelOffset[2] -= 9.81;
}

void printMPUdata(){
    //print_counter = micros();
    Serial.print(F(">Pitch:"));
    Serial.println(pitch);
    Serial.print(F(">Row:"));
    Serial.println(roll);
    Serial.print(F(">Yaw:"));
    Serial.println(yaw);
}

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

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

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
    case MPU6050_RANGE_250_DEG:
      Serial.println("+- 250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+- 500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+- 1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+- 2000 deg/s");
      break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 Hz");
      break;
  }

  Serial.println("Calibrating MPU6050...");
  calibrateMPU6050();
  Serial.println("Calibration done!");

  Serial.println("");
  delay(100);
}

void loop() {
  // Get the current time
  unsigned long currentTime = millis();
  float elapsedTime = (currentTime - previousTime) / 1000.0; // Convert to seconds
  previousTime = currentTime;

  // Get new sensor events with the readings
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Apply the offsets
  a.acceleration.x -= accelOffset[0];
  a.acceleration.y -= accelOffset[1];
  a.acceleration.z -= accelOffset[2];
  g.gyro.x -= gyroOffset[0];
  g.gyro.y -= gyroOffset[1];
  g.gyro.z -= gyroOffset[2];

  // Calculate pitch and roll using accelerometer data
  float accelPitch = atan2(a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180.0 / PI;
  float accelRoll = atan2(-a.acceleration.y, sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.z * a.acceleration.z)) * 180.0 / PI;

  pitch = accelPitch;
  roll = accelRoll; 
  // Update yaw using gyroscope data
  yaw += g.gyro.z * elapsedTime; // Integrate to get yaw

  // Print out the values
  printMPUdata();

  delay(100);
}