kriswiner
commented
6 years ago Do you calibrate your sensors? How? How do you know they are calibrated?
And this is wrong:
Kinematic = MadgwickQuaternionUpdate(Kinematic.ax, Kinematic.ay, Kinematic.az, Kinematic.gx, Kinematic.gy, Kinematic.gz, Kinematic.my, Kinematic.mx, Kinematic.mz, Kinematic);
This needs to be in North, East, Down order.
On Sat, Sep 22, 2018 at 8:56 AM goosst notifications@github.com wrote:
Hello,
I'm struggling quite a bit with making this algorithm work with an MPU9250 (on a wemos d1).
I've made an implementation with some small modifications to your function but don't seem to grasp the convergence aspect (the madgwick algorithm seems to give me poor results compared to just using the gyroscope, see attached screenshot where you can see the blue vs orange line for two different values of beta.). Any help is appreciated.
something I was wondering: when reading the paper which explains the madgwick algorithm I also don't see the zeta to correct for gyro bias in the algorithm in this repo (although it's unrelated to this issue). Is it considered as not very beneficial?
[image: madgwick] https://user-images.githubusercontent.com/8907127/45918925-29c1cb80-be8e-11e8-89df-eb2a0f5a937c.png
[image: madgwick2] https://user-images.githubusercontent.com/8907127/45919031-db153100-be8f-11e8-95f9-1320517d598d.png
include
include
MPU9250 IMU(Wire, 0x68);
int taskrate2 = 20; Task t1(taskrate2, TASK_FOREVER, &MPU9250_one); Scheduler runner;
float pitch_g = 0; float roll_g = 0; float yaw_g = 0;
struct Kinematic { float pitch = 0; float roll = 0; float yaw = 0; float ax ; float ay ; float az ; float gx ; float gy ; float gz ; float mx = 1; float my = 1; float mz = 1; uint8_t counter = 0; float deltat = 0; uint32_t prev_time = 0; float q1 = 1; float q2 = 0; float q3 = 0; float q4 = 0; float beta = 0.01; }; struct Kinematic Sens1;
void MPU9250_one() { Sens1 = MPU9250_angles(IMU, Sens1); yield(); }
struct Kinematic MPU9250_angles(MPU9250 IMU, Kinematic Kinematic) { IMU.readSensor(); Kinematic.ax = IMU.getAccelX_mss(); Kinematic.ay = IMU.getAccelY_mss(); Kinematic.az = IMU.getAccelZ_mss(); Kinematic.gx = IMU.getGyroX_rads(); Kinematic.gy = IMU.getGyroY_rads(); Kinematic.gz = IMU.getGyroZ_rads();
// no need to update magnetometer as quickly as gyro and accelerometers if (Kinematic.counter == 0) { Kinematic.mx = IMU.getMagX_uT() ; Kinematic.my = IMU.getMagY_uT(); Kinematic.mz = IMU.getMagZ_uT(); Serial.print(Kinematic.pitch); Serial.print("\t"); Serial.print(Kinematic.roll); Serial.print("\t"); Serial.print(Kinematic.yaw); Serial.print("\t"); Serial.print(pitch_g); Serial.print("\t"); Serial.print(roll_g); Serial.print("\t"); Serial.print(yaw_g); Serial.print("\n"); }
Kinematic.counter++; if (Kinematic.counter > 5) { Kinematic.counter = 0; } uint32_t time_now = millis();
if (Kinematic.prev_time != 0) { Kinematic.deltat = 0.001 * (time_now - Kinematic.prev_time); } Kinematic.prev_time = time_now;
Kinematic = MadgwickQuaternionUpdate(Kinematic.ax, Kinematic.ay, Kinematic.az, Kinematic.gx, Kinematic.gy, Kinematic.gz, Kinematic.my, Kinematic.mx, Kinematic.mz, Kinematic);
Kinematic.yaw = atan2(2.0f (Kinematic.q2 Kinematic.q3 + Kinematic.q1 Kinematic.q4), Kinematic.q1 Kinematic.q1 + Kinematic.q2 Kinematic.q2 - Kinematic.q3 Kinematic.q3 - Kinematic.q4 Kinematic.q4); Kinematic.pitch = -asin(2.0f (Kinematic.q2 Kinematic.q4 - Kinematic.q1 Kinematic.q3)); Kinematic.roll = atan2(2.0f (Kinematic.q1 Kinematic.q2 + Kinematic.q3 Kinematic.q4), Kinematic.q1 Kinematic.q1 - Kinematic.q2 Kinematic.q2 - Kinematic.q3 Kinematic.q3 + Kinematic.q4 Kinematic.q4); Kinematic.pitch = 180.0f / PI; Kinematic.yaw = 180.0f / PI; // yaw -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 Kinematic.roll = 180.0f / PI;
float rad2deg = 180 / 3.1415; pitch_g = pitch_g + Kinematic.deltat Kinematic.gx rad2deg; roll_g = roll_g + Kinematic.deltat Kinematic.gy rad2deg; yaw_g = yaw_g + Kinematic.deltat Kinematic.gz rad2deg;
return Kinematic;
}
void setup() { // put your setup code here, to run once: // serial to display data Serial.begin(115200); while (!Serial) {}
// start communication with IMU int status = IMU.begin(); if (status < 0) { Serial.println("IMU initialization unsuccessful"); Serial.println("Check IMU wiring or try cycling power"); Serial.print("Status: "); Serial.println(status); while (1) {} } // setting the accelerometer full scale range to +/-8G IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G); // setting the gyroscope full scale range to +/-500 deg/s IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS); // setting DLPF bandwidth to 20 Hz IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ); // setting SRD to 19 for a 50 Hz update rate IMU.setSrd(19);
// read the sensor Serial.println("calibrating gyro, don't move"); status = IMU.calibrateGyro();
runner.init(); runner.addTask(t1); t1.enable(); }
void loop() { // put your main code here, to run repeatedly: runner.execute(); delay(0); }
struct Kinematic MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, Kinematic k) { float q1 = k.q1, q2 = k.q2, q3 = k.q3, q4 = k.q4; // 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.001) { norm = 1.0f / norm; ax = norm; ay = norm; az = norm; }
// Normalise magnetometer measurement norm = sqrtf(mx mx + my my + mz mz); if (norm > 0.001) { 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) - k.beta s1; qDot2 = 0.5f (q1 gx + q3 gz - q4 gy) - k.beta s2; qDot3 = 0.5f (q1 gy - q2 gz + q4 gx) - k.beta s3; qDot4 = 0.5f (q1 gz + q2 gy - q3 gx) - k.beta s4;
// Integrate to yield quaternion q1 += qDot1 k.deltat; q2 += qDot2 k.deltat; q3 += qDot3 k.deltat; q4 += qDot4 k.deltat; norm = sqrtf(q1 q1 + q2 q2 + q3 q3 + q4 q4); // normalise quaternion norm = 1.0f / norm; k.q1 = q1 norm; k.q2 = q2 norm; k.q3 = q3 norm; k.q4 = q4 norm;
return k;
}
— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/310, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1quTodJJYKNxNF46DburBTNlLlyS5ks5udl2QgaJpZM4W1U0x .
Hello,
I'm struggling quite a bit with making this algorithm work with an MPU9250 (on a wemos d1).
I've made an implementation with some small modifications to your function but don't seem to grasp the convergence aspect (the madgwick algorithm seems to give me poor results compared to just using the gyroscope, see attached screenshot where you can see the blue vs orange line for two different values of beta.). Any help is appreciated.
something I was wondering: when reading the paper which explains the madgwick algorithm I also don't see the zeta to correct for gyro bias in the algorithm in this repo (although it's unrelated to this issue). Is it considered as not very beneficial?