ethan42411 / imu-algorithm

Sensor fusion algorithm to determine roll and pitch in 6-DOF IMUs

IMU-algorithm

A sensor fusion algorithm to determine roll and pitch in 6-DOF IMU.

Demo

Demonstration using an Arduino Uno, MPU6050 (without DMP support) and Processing software.

Working

The orientation is calculated as a quaternion that rotates the gravity vector from earth frame to sensor frame. The gravity vector in the sensor frame is the accelerometer readings and the gravity vector in earth frame is (0,0,-1).

The accelerometer values are sensitive to vibrations. The gyroscope is used to keep track of the gravity vector and correct the accelerometer readings.

Usage

To use the library, read the accelerometer, gyro values, and calculate the time taken to complete a loop.

/*
fused_vector - corrected accelerometer readings
delta - delay or time taken to complete a loop
wx,wy,wz - gyro values in rad/s
AcX, AcY, AcZ - raw accelerometer values
q_acc - quaternion representing orientation
angles - euler angles
*/

delta = 0.001*(millis()-Start);
fused_vector = update_fused_vector(fused_vector,AcX,AcY,AcZ,wx,wy,wz,delta);
  
q_acc = quaternion_from_accelerometer(fused_vector.a,fused_vector.b,fused_vector.c);
angles = quaternion_to_euler_angles(q_acc);

The library uses an approximation for 1/sqrt(x) (in vector_3d.c) which may not work on all machines. The definition of the function may have to be changed in some hardware.

Reference/Credits/Sources

Beautiful maths simplification: quaternion from two vectors

Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs

Phillip's Technology Corner - Fast Quaternion Integration for Attitude Estimation

Pizer’s Weblog - Fast Inverse Square Root

Processing Code