Euler Angles for Attitude Representation

Attitude describes an object's orientation in 3D space, often represented by Euler angles. These angles define rotations around the body's principal axes.

• Yaw: Rotation around the vertical (Z) axis, indicating horizontal direction.
• Pitch: Rotation around the lateral (Y) axis, corresponding to nodding motion.
• Roll: Rotation around the longitudinal (X) axis, similar to a wing-flapping motion.

Accelerometer Principles

An accelerometer measures proper acceleration along three orthogonal axes. It is effective for determining static orientation relative to gravity but is highly susceptible to noise from vibrations.

Gyroscope Principles

A gyroscope measures angular velocity around the three axes, expressed in degrees per second. The current angle is estimated by integrating the angular velocity over a short time interval:

angle_new = angle_previous + angular_velocity * delta_time

This integration accumulates errors over time, leading to drift.

Complementary Filter for Sensor Fusion

To mitigate individual sensor weaknesses, a complementary filter combines accelerometer and gyroscope data. The accelerometer provides a stable reference for slow-changing angles, while the gyroscope captures rapid movements accurate.

The fused angle is computed as:

fused_angle = α * (gyro_angle) + (1 - α) * (accel_angle)

Where α is a weighting factor (typically ~0.98), giving more trust to the gyroscope. The accelerometer cannot determine yaw.

The weighting factor can be dynamically adjusted:

α = τ / (τ + Δt)

Where τ is a time constant (e.g., 0.1 seconds) and Δt is the sampling interval.

Digital Motion Processor (DMP)

The MPU6050 includes a DMP, which offloads complex calculations like sensor fusion. The DMP can be configured to output processed quaternion or Euler angle data. External sensors like a magnetometer can be added via I2C to create a 9-axis system.

Implemantation Outline

Initialization

Configure the device registers:

• Reset and wake the chip.
• Set sample rate divider.
• Configure accelerometer and gyroscope full-scale ranges.

Data Acquisition

Read raw 16-bit signed integer values from accelerometer and gyroscope output registers.

Angle Calculation

• From Accelerometer: Use trigonometric functions on the acceleration components.
  • roll_acc = atan2(accel_y, accel_z) * (180.0 / π)
  • pitch_acc = atan2(-accel_x, accel_z) * (180.0 / π)
• From Gyroscope: Integrate the angular rates.
  • roll_gyro = roll_prev + gyro_x * dt
  • pitch_gyro = pitch_prev + gyro_y * dt
  • yaw_gyro = yaw_prev + gyro_z * dt

Sensor Fusion

Aply the complementary filter to the calculated angles.

// Simplified C code example
#include <math.h>

#define ALPHA 0.98
#define DT 0.005f // 5ms sampling interval

typedef struct {
    float x, y, z;
} Vector3;

void update_attitude(Vector3 *accel, Vector3 *gyro, float *roll, float *pitch, float *yaw) {
    // Calculate angles from accelerometer
    float roll_acc = atan2(accel->y, accel->z) * 57.2958f;
    float pitch_acc = atan2(-accel->x, accel->z) * 57.2958f;

    // Integrate gyroscope data
    *roll += gyro->x * DT;
    *pitch += gyro->y * DT;
    *yaw += gyro->z * DT;

    // Fuse measurements
    *roll = ALPHA * (*roll) + (1.0f - ALPHA) * roll_acc;
    *pitch = ALPHA * (*pitch) + (1.0f - ALPHA) * pitch_acc;
    // Yaw is not corrected by accelerometer
}

Raw sensor readings require scaling based on the configured measurement range. Temperature compensation may also be applied.