# Untitled

unknown
plain_text
a month ago
6.2 kB
2
Indexable
Never
```#include <Arduino.h>
#include <Wire.h>

// 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

// 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);
}```