polodealvarado
commented
6 years ago In addition , the quaternion code which I'm using is this:
// Implementation of Sebastian Madgwick's "...efficient orientation filter // for... inertial/magnetic sensor arrays" // (see http://www.x-io.co.uk/category/open-source/ for examples & more details) // which fuses acceleration, rotation rate, and magnetic moments to produce a // quaternion-based estimate of absolute device orientation -- which can be // converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. // The performance of the orientation filter is at least as good as conventional // Kalman-based filtering algorithms but is much less computationally // intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
include "quaternionFilters.h"
// These are the free parameters in the Mahony filter and fusion scheme, Kp // for proportional feedback, Ki for integral
define Kp 2.0f * 5.0f
define Ki 0.0f
static float GyroMeasError = PI (40.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) static float GyroMeasDrift = PI (0.0f / 180.0f); // There is a tradeoff in the beta parameter between accuracy and response // speed. In the original Madgwick study, beta of 0.041 (corresponding to // GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. // However, with this value, the LSM9SD0 response time is about 10 seconds // to a stable initial quaternion. Subsequent changes also require a // longish lag time to a stable output, not fast enough for a quadcopter or // robot car! By increasing beta (GyroMeasError) by about a factor of // fifteen, the response time constant is reduced to ~2 sec. I haven't // noticed any reduction in solution accuracy. This is essentially the I // coefficient in a PID control sense; the bigger the feedback coefficient, // the faster the solution converges, usually at the expense of accuracy. // In any case, this is the free parameter in the Madgwick filtering and // fusion scheme. static float beta = sqrt(3.0f / 4.0f) GyroMeasError; // Compute beta // Compute zeta, the other free parameter in the Madgwick scheme usually // set to a small or zero value static float zeta = sqrt(3.0f / 4.0f) GyroMeasDrift;
// Vector to hold integral error for Mahony method static float eInt[3] = {0.0f, 0.0f, 0.0f}; // Vector to hold quaternion static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat) { // short name local variable for readability float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; 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 = sqrt(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 = sqrt(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 = sqrt(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 = sqrt(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 = sqrt(q1 q1 + q2 q2 + q3 q3 + q4 q4); // normalise quaternion norm = 1.0f/norm; q[0] = q1 norm; q[1] = q2 norm; q[2] = q3 norm; q[3] = q4 norm; }
// Similar to Madgwick scheme but uses proportional and integral filtering on // the error between estimated reference vectors and measured ones. void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat) { // short name local variable for readability float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; float norm; float hx, hy, bx, bz; float vx, vy, vz, wx, wy, wz; float ex, ey, ez; float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic 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 = sqrt(ax ax + ay ay + az az); if (norm == 0.0f) return; // Handle NaN norm = 1.0f / norm; // Use reciprocal for division ax = norm; ay = norm; az = norm;
// Normalise magnetometer measurement norm = sqrt(mx mx + my my + mz mz); if (norm == 0.0f) return; // Handle NaN norm = 1.0f / norm; // Use reciprocal for division mx = norm; my = norm; mz = norm;
// Reference direction of Earth's magnetic field hx = 2.0f mx (0.5f - q3q3 - q4q4) + 2.0f my (q2q3 - q1q4) + 2.0f mz (q2q4 + q1q3); hy = 2.0f mx (q2q3 + q1q4) + 2.0f my (0.5f - q2q2 - q4q4) + 2.0f mz (q3q4 - q1q2); bx = sqrt((hx hx) + (hy hy)); bz = 2.0f mx (q2q4 - q1q3) + 2.0f my (q3q4 + q1q2) + 2.0f mz (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field vx = 2.0f (q2q4 - q1q3); vy = 2.0f (q1q2 + q3q4); vz = q1q1 - q2q2 - q3q3 + q4q4; wx = 2.0f bx (0.5f - q3q3 - q4q4) + 2.0f bz (q2q4 - q1q3); wy = 2.0f bx (q2q3 - q1q4) + 2.0f bz (q1q2 + q3q4); wz = 2.0f bx (q1q3 + q2q4) + 2.0f bz (0.5f - q2q2 - q3q3);
// Error is cross product between estimated direction and measured direction of gravity ex = (ay vz - az vy) + (my wz - mz wy); ey = (az vx - ax vz) + (mz wx - mx wz); ez = (ax vy - ay vx) + (mx wy - my wx); if (Ki > 0.0f) { eInt[0] += ex; // accumulate integral error eInt[1] += ey; eInt[2] += ez; } else { eInt[0] = 0.0f; // prevent integral wind up eInt[1] = 0.0f; eInt[2] = 0.0f; }
// Apply feedback terms gx = gx + Kp ex + Ki eInt[0]; gy = gy + Kp ey + Ki eInt[1]; gz = gz + Kp ez + Ki eInt[2];
// Integrate rate of change of quaternion pa = q2; pb = q3; pc = q4; q1 = q1 + (-q2 gx - q3 gy - q4 gz) (0.5f deltat); q2 = pa + (q1 gx + pb gz - pc gy) (0.5f deltat); q3 = pb + (q1 gy - pa gz + pc gx) (0.5f deltat); q4 = pc + (q1 gz + pa gy - pb gx) (0.5f deltat);
// Normalise quaternion norm = sqrt(q1 q1 + q2 q2 + q3 q3 + q4 q4); norm = 1.0f / norm; q[0] = q1 norm; q[1] = q2 norm; q[2] = q3 norm; q[3] = q4 norm; }
const float * getQ () { return q; }
kriswiner
Hi Kris. First of all, thank you very much for this library, it has really helped me.
I have a problem with my yaw angle. When the pitch or roll angle reach 90º or -90º , the yaw angle changes taking an illogical value.
The magnetometor has been calibrated correctly, but I am not sure if the problem comes form the filter or the yaw,pitch,roll calculation. My coordinate system is this: (https://www.google.es/search?q=mpu9250+coordinates&client=opera&hs=2vh&source=lnms&tbm=isch&sa=X&ved=2ahUKEwj--fndwMTbAhWLQBQKHTZKB0MQ_AUoAXoECAEQAw&biw=1650&bih=870#imgrc=oPcs2IW3wPfIbM:)
Thank you in advance.
/* MPU9250 Basic Example Code by: Kris Winer date: April 1, 2014 license: Beerware - Use this code however you'd like. If you find it useful you can buy me a beer some time. Modified by Brent Wilkins July 19, 2016
Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
SDA and SCL should have external pull-up resistors (to 3.3V). 10k resistors are on the EMSENSR-9250 breakout board.
Hardware setup: MPU9250 Breakout --------- Arduino VDD ---------------------- 3.3V VDDI --------------------- 3.3V SDA ----------------------- A4 SCL ----------------------- A5 GND ---------------------- GND */
include "quaternionFilters.h"
include "MPU9250.h"
define AHRS true // Set to false for basic data read
define SerialDebug true // Set to true to get Serial output for debugging
uint8_t Mmode = 0x06;
MPU9250 myIMU;
void setup() { Wire.begin(); // TWBR = 12; // 400 kbit/sec I2C speed Serial.begin(115200);
// Read the WHO_AM_I register, this is a good test of communication byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX);
if (c == 0x71) // WHO_AM_I should always be 0x68 { Serial.println("MPU9250 is online...");
} // if (c == 0x71) else { Serial.print("Could not connect to MPU9250: 0x"); Serial.println(c, HEX); while(1) ; // Loop forever if communication doesn't happen } }
void loop() { // If intPin goes high, all data registers have new data // On interrupt, check if data ready interrupt if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values myIMU.getAres();
} // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
// Must be called before updating quaternions! myIMU.updateTime();
// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis // (+ up) of accelerometer and gyro! We have to make some allowance for this // orientationmismatch in feeding the output to the quaternion filter. For the // MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward // along the x-axis just like in the LSM9DS0 sensor. This rotation can be // modified to allow any convenient orientation convention. This is ok by // aircraft orientation standards! Pass gyro rate as rad/s //MadgwickQuaternionUpdate(-myIMU.ax, myIMU.ay, myIMU.az, myIMU.gxPI/180.0f, -myIMU.gyPI/180.0f, -myIMU.gz*PI/180.0f, myIMU.my, -myIMU.mx, myIMU.mz,myIMU.deltat);
MahonyQuaternionUpdate(myIMU.ax, -myIMU.ay, -myIMU.az, myIMU.gxDEG_TO_RAD, -myIMU.gyDEG_TO_RAD, -myIMU.gz*DEG_TO_RAD, myIMU.my,-myIMU.mx, myIMU.mz, myIMU.deltat);
if (!AHRS) { myIMU.delt_t = millis() - myIMU.count; if (myIMU.delt_t > 500) { if(SerialDebug) { // Print acceleration values in milligs! Serial.print("X-acceleration: "); Serial.print(1000myIMU.ax); Serial.print(" mg "); Serial.print("Y-acceleration: "); Serial.print(1000myIMU.ay); Serial.print(" mg "); Serial.print("Z-acceleration: "); Serial.print(1000*myIMU.az); Serial.println(" mg ");
} // if (!AHRS) else { // Serial print and/or display at 0.5 s rate independent of data rates myIMU.delt_t = millis() - myIMU.count;
// Define output variables from updated quaternion---these are Tait-Bryan // angles, commonly used in aircraft orientation. In this coordinate system, // the positive z-axis is down toward Earth. Yaw is the angle between Sensor // x-axis and Earth magnetic North (or true North if corrected for local // declination, looking down on the sensor positive yaw is counterclockwise. // Pitch is angle between sensor x-axis and Earth ground plane, toward the // Earth is positive, up toward the sky is negative. Roll is angle between // sensor y-axis and Earth ground plane, y-axis up is positive roll. These // arise from the definition of the homogeneous rotation matrix constructed // from quaternions. Tait-Bryan angles as well as Euler angles are // non-commutative; that is, the get the correct orientation the rotations // must be applied in the correct order which for this configuration is yaw, // pitch, and then roll. // For more see // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles // which has additional links. myIMU.yaw = atan2(2.0f ((getQ()+1) (getQ()+2) + getQ() (getQ()+3)), getQ() getQ() + (getQ()+1) *(getQ()+1)
(getQ()+2) (getQ()+2) - (getQ()+3) (getQ()+3));
myIMU.pitch = -asin(2.0f ((getQ()+1) (getQ()+3) - getQ() *(getQ()+2)));
myIMU.roll = atan2(2.0f (getQ() (getQ()+1) + (getQ()+2) (getQ()+3)), getQ() getQ() - (getQ()+1) *(getQ()+1)
(getQ()+2) (getQ()+2) + (getQ()+3) (getQ()+3)); myIMU.pitch = RAD_TO_DEG; myIMU.yaw = RAD_TO_DEG; // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 // - http://www.ngdc.noaa.gov/geomag-web/#declination myIMU.yaw -= 1.5; myIMU.roll *= RAD_TO_DEG;
if(SerialDebug) { Serial.print("Yaw, Pitch, Roll: "); Serial.print(myIMU.yaw, 2); Serial.print(", "); Serial.print(myIMU.pitch, 2);
}
myIMU.count = millis(); myIMU.sumCount = 0; myIMU.sum = 0; } // if (myIMU.delt_t > 500) } // if (AHRS) }
void magcalMPU9250(float dest1, float dest2) { uint16_t ii = 0, sample_count = 0; int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0}; int16_t mag_max[3] = {32767, 32767, 32767}, mag_min[3] = {-32767, -32767, -32767}, mag_temp[3] = {0, 0, 0};
Serial.println("Mag Calibration: Wave device in a figure eight until done!"); delay(4000);
for(ii = 0; ii < sample_count; ii++) { myIMU.readMagData(mag_temp); // Read the mag data
for (int jj = 0; jj < 3; jj++) { if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; } if(Mmode == 0x02) delay(135); // at 8 Hz ODR, new mag data is available every 125 ms if(Mmode == 0x06) delay(12); // at 100 Hz ODR, new mag data is available every 10 ms }
// Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]); // Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]); // Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]);
Serial.println("Mag Calibration done!"); }