kriswiner
Math question
MPU9250/quaternionFilters.ino at line 177 // 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);
Is this accurate math since when computing p2, p3,p4, you are using an "udated" p1. I think q1, pa,pb,pc ; does not form a valid quaternion.
Thanks Gerard.
Perhaps, then modify to suit your own methods....
On Mon, Dec 21, 2020 at 2:04 AM g-yvr [email protected] wrote:
MPU9250/quaternionFilters.ino at line 177 // 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);
Is this accurate math since when computing p2, p3,p4, you are using an "udated" p1. I think q1, pa,pb,pc ; does not form a valid quaternion.
Thanks Gerard.
— 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/447, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKX65UF2LIFDYAW55RDSV4MUNANCNFSM4VD7ZR3Q .
In fact, it would be easy to define a pd = q1 and use this to calculate q2
- 4 instead of the updated q1. When you try this please let me know if you see any differences in the results. I would be surprised if you do
The current method is a simple copy of the original method published in Madgwick's paper.
On Mon, Dec 21, 2020 at 8:59 AM Tlera Corporation [email protected] wrote:
Perhaps, then modify to suit your own methods....
On Mon, Dec 21, 2020 at 2:04 AM g-yvr [email protected] wrote:
MPU9250/quaternionFilters.ino at line 177 // 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);
Is this accurate math since when computing p2, p3,p4, you are using an "udated" p1. I think q1, pa,pb,pc ; does not form a valid quaternion.
Thanks Gerard.
— 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/447, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKX65UF2LIFDYAW55RDSV4MUNANCNFSM4VD7ZR3Q .
I agree that the implementation of the math is incorrect in quaternionFilters.ino. The math in the Madgwick's original report (p. 30) is correct.
Note that the block of code in question is for Euler integration of the gyro data in the MahonyQuaternionUpdate function:
// 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);
which presumably was copied and altered from the MadgwickQuaternionUpdate function:
// 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;
Perhaps what happened was the code was incorrectly simplified -- beta set to 0, temporary qDotx variables eliminated
the Euler integration of the gyro data could be correctly implemented as
// Integrate rate of change of quaternion
gx = gx * (0.5*deltat); // pre-multiply common factors
gy = gy * (0.5*deltat);
gz = gz * (0.5*deltat);
qa = q1;
qb = q2;
qc = q3;
q1 += (-qb * gx - qc * gy - q4 * gz);
q2 += (qa * gx + qc * gz - q4 * gy);
q3 += (qa * gy - qb * gz + q4 * gx);
q4 += (qa * gz + qb * gy - qc * gx);
which comes from Madgwick's 2010 implementation of the 2008 nonlinear complementary filter by Mahony et al.
What specifically is the error here?
On Tue, Jan 11, 2022 at 10:25 AM Parker Lusk @.***> wrote:
I agree that the implementation of the math is incorrect in quaternionFilters.ino. The math in the Madgwick's original report https://courses.cs.washington.edu/courses/cse466/14au/labs/l4/madgwick_internal_report.pdf (p. 30) is correct.
Note that the block of code in question is for Euler integration of the gyro data in the MahonyQuaternionUpdate https://github.com/kriswiner/MPU9250/blob/master/quaternionFilters.ino#L104 function:
// 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);
which presumably was copied and altered from the MadgwickQuaternionUpdate https://github.com/kriswiner/MPU9250/blob/master/quaternionFilters.ino#L8 function:
// 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;
Perhaps what happened was the code was incorrectly simplified -- beta set to 0, temporary qDotx variables eliminated
the Euler integration of the gyro data could be correctly implemented as
// Integrate rate of change of quaternion gx = gx * (0.5deltat); // pre-multiply common factors gy = gy * (0.5deltat); gz = gz * (0.5*deltat); qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); q3 += (qa * gz + qb * gy - qc * gx);
which comes from Madgwick's 2010 implementation of the 2008 nonlinear complementary filter by Mahony et al https://hal.archives-ouvertes.fr/hal-00488376/document.
— Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/447#issuecomment-1010241522, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKRYJ2N7M22L5SXUYOLUVRYYPANCNFSM4VD7ZR3Q . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.
You are receiving this because you commented.Message ID: @.***>
as @g-yvr stated, you are using an updated q1 for the calculation of q2, q3,, q4 - but this is wrong.
Ah, quite right. Yes, this was probably sloppily copied. I never use the Mahony formulation so probably never noticed this.
On Tue, Jan 11, 2022 at 10:43 AM Parker Lusk @.***> wrote:
as @g-yvr https://github.com/g-yvr stated, you are using an updated q1 for the calculation of q2, q3,, q4 - but this is wrong.
— Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/447#issuecomment-1010254883, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKXTVXQWATDI5MEA4GLUVR23NANCNFSM4VD7ZR3Q . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.
You are receiving this because you commented.Message ID: @.***>