Is the orientation absolute or relative?
haley0918 opened this issue · 5 comments
Hi,
I'm trying out your Madgwick filter using my Crius AIO Multiwii flight controller. It has Mega2560, MPU6050 and HMC5883L on board. Positive X and Y axes of both sensors are aligned in same directions respectively.
I have manually ran calibrations on MPU6050 for the offsets and HMC5883L for hard iron offset. Upon initialize, the pitch and roll orientation seems to be relative to initial orientation instead of gravity, heading would always align with magnetic north. What am I doing wrong and how to correct it to get absolute orientation? Or is my understanding of absolute orientation (orientation relative to gravity and magnetic north) wrong?
Here is my test clip showing the orientation tracking. Reset button was pressed at 0:30 and 1:08.
Below is my sketch. Please let me know if you require the libraries used. Thanks.
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"
#include "HMC5883L.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
HMC5883L mag;
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 gy; // [x, y, z] gyro sensor measurements
VectorInt16 mg; // [x, y, z] magneto sensor measurements
VectorFloat areal; // [x, y, z] accel in g
VectorFloat greal; // [x, y, z] gyro in degree/s
VectorFloat mreal; // [x, y, z] magneto in mG
uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate
float pitch, yaw, roll;
float a12, a22, a31, a32, a33; // rotation matrix coefficients for Euler angles and gravity components
float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes
uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
uint32_t Now = 0; // used to calculate integration interval
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float beta = 0.06;
float q1 = q.w, q2 = q.x, q3 = q.y, q4 = q.z; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrtf(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrtf(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * deltat;
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q.w = q1 * norm;
q.x = q2 * norm;
q.y = q3 * norm;
q.z = q4 * norm;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready
/Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again/
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
mpu.setIntI2CMasterEnabled(false);
mpu.setI2CBypassEnabled(true);
mpu.setSleepEnabled(false);
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(-25);
mpu.setYGyroOffset(57);
mpu.setZGyroOffset(34);
mpu.setXAccelOffset(-668);
mpu.setYAccelOffset(-1506);
mpu.setZAccelOffset(1955);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
Serial.println();
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// initialize device
Serial.println("Initializing I2C devices...");
mag.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed");
// configure LED for output
pinMode(LED_PIN, OUTPUT);
q.w = 1.0f;
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// read a packet from FIFO
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
// display quaternion values in easy matrix form: w x y z
//mpu.dmpGetQuaternion(&q, fifoBuffer);
//mpu.dmpGetEuler(euler, &q);
//mpu.dmpGetGyro(&gy, fifoBuffer);
//mpu.dmpGetAccel(&aa, fifoBuffer);
//gy.rotate(&q);
mpu.getRotation(&gy.x, &gy.y, &gy.z);
mpu.getAcceleration(&aa.x, &aa.y, &aa.z);
// read raw heading measurements from device
mag.getHeading(&mg.x, &mg.y, &mg.z);
areal.x = (float)(aa.x) / 16384;
areal.y = (float)(aa.y) / 16384;
areal.z = (float)(aa.z) / 16384;
greal.x = (float)(gy.x) / 16.4;
greal.y = (float)(gy.y) / 16.4;
greal.z = (float)(gy.z) / 16.4;
mreal.x = (float)(mg.x+4.35) * 0.92;
mreal.y = (float)(mg.y+15.65) * 0.92;
mreal.z = (float)(mg.z-28.08) * 0.92;
Now = micros();
deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;
sum += deltat; // sum for averaging filter update rate
sumCount++;
MadgwickQuaternionUpdate(areal.x, areal.y, areal.z, greal.x*PI/180.0f, greal.y*PI/180.0f, greal.z*PI/180.0f, mreal.x, mreal.y, mreal.z);
a12 = 2.0f * (q.x * q.y + q.w * q.z);
a22 = q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z;
a31 = 2.0f * (q.w * q.x + q.y * q.z);
a32 = 2.0f * (q.x * q.z - q.w * q.y);
a33 = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z;
pitch = -asinf(a32);
roll = atan2f(a31, a33);
yaw = atan2f(a12, a22);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
yaw -= 61.5f; // Desk heading
if(yaw < 0) yaw += 360.0f; // Ensure yaw stays between 0 and 360
roll *= 180.0f / PI;
delt_t = millis() - count;
if (delt_t > 500) {
//Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz");
count = millis();
sumCount = 0;
sum = 0;
}
//MotionCal
/*Serial.print("Raw:");
Serial.print(aa.x);
Serial.print(',');
Serial.print(aa.y);
Serial.print(',');
Serial.print(aa.z);
Serial.print(',');
Serial.print(gy.x);
Serial.print(',');
Serial.print(gy.y);
Serial.print(',');
Serial.print(gy.z);
Serial.print(',');
Serial.print(mg.x);
Serial.print(',');
Serial.print(mg.y);
Serial.print(',');
Serial.print(mg.z);
Serial.println();*/
//Visualizer
Serial.print("Orientation: ");
Serial.print(yaw);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.println(roll);
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
Thank you for assisting.
I had checked again the sensors' orientation and directions with their datasheet. Turns out the MPU6050 accelerometer positive directions are all inverted when compared to actual reading. Gyros are correct though. Mags' positive directions are correct too. This drawing should summarize my sensors' readings.
N( -acc[y] / mag[y] ) U
| |
| |
| |
W-------+-------E( -acc[x] / mag[x] ) |
| |
| |
| |
S D( acc[z] / -mag[z] )
So with that , and following NED convention, I'm passing the data in this order:
-acc[y], -acc[x], acc[z], gy[y], gy[x], -gy[z], mag[y], mag[x], -mag[z]
Yaw, pitch and roll all behave properly. Yaw does align to magnetic north on initialize. However, pitch and roll initialize with their starting position, not aligning to earth gravity direction. If I initialize the sensors at pitch of 15 degrees, the output shows 0 degree at such angle. It doesn't account for that 15 degrees.
Does Madgwick filter absolute orientation only tracks relative to magnetic north and disregard of gravity?
Found the cause of my problem!
I shouldn't have allowed the accel and gyro calibrations to run after I put in my manually obtained offsets.
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
Now the filter works as intended to give absolute orientation.
Thanks!