kriswiner
commented
8 years ago I simply referred to the C code (MARG filter) at the end of Madgwick's paper and transcribed it, removing a lot of redundancy and improving the flow. I spent a lot of time checking for consistency and accuracy and I have been using the version on the github repository with success for more than two years. I do not want to repeat this process.
I don't expect a one-to-one correspondence between what I have been using and Madgwick's original program since there are a lot of factors of two and one-half as well as minus signs that end up canceling at the end.
If you want to be sure, I recommend you use Madgwick's original program, transcribe it your self, or fix whatever errors you conclude exist in my code.
There is always the possibility I have made an error and I would appreciate it if you could find it and alert me to it.
Kris
-----Original Message----- From: ponny213 [mailto:notifications@github.com] Sent: September 16, 2016 2:52 PM To: kriswiner/MPU-9250 Subject: [kriswiner/MPU-9250] Madgwick Quaternion (#76)
Hi Kris,
I was trying to understand Madgwick Quaternion filter by comparing your code with the Madgwick's original paper. And i found some inconsistency. Please correct me if i am wrong.
- In the code: " _2bx = sqrtf(hx * hx + hy * hy); _2bz = -_2q1mx * q3
- _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz q3q3 + mz \ q4q4;"
Is it supposed to be ? _2bx = 2.0fsqrtf(hx * hx + hy * hy); _2bz = 2.0f(-_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2
- _2q3 * my * q4 - mz * q3q3 + mz * q4q4);
- In the code: "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; ... _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx
- q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;"
But based on the equation (21) in the paper. By substituting (dx, dy, dz) for (mx, my, mz). is it supposed to be (two signed were inversed in each equation): 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; ... _2bz = +_2q1mx * q3 - _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz \ q4q4;
You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU-9250/issues/76 , or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qhzVZR17peGjSWQmUZl6w VA0UgyFks5qqw9ogaJpZM4J_YbF . https://github.com/notifications/beacon/AGY1qpB4w44ybnjKJ6SifZUS0NYOUUm5ks5 qqw9ogaJpZM4J_YbF.gif
Hi Kris,
I was trying to understand Madgwick Quaternion filter by comparing your code with the Madgwick's original paper. And i found some inconsistency. Please correct me if i am wrong.
Is it supposed to be ? _2bx = 2.0f_sqrtf(hx * hx + hy * hy); 2bz = 2.0f(-_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4);
But based on the equation (21) in the paper. By substituting (dx, dy, dz) for (mx, my, mz). is it supposed to be (two signed were inversed in each equation): 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; ... _2bz = +_2q1mx * q3 - _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;