Math question

[g-yvr]

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.

[kriswiner]

Perhaps, then modify to suit your own methods....

On Mon, Dec 21, 2020 at 2:04 AM g-yvr notifications@github.com 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 .

[kriswiner]

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 tleracorp@gmail.com wrote:

Perhaps, then modify to suit your own methods....

On Mon, Dec 21, 2020 at 2:04 AM g-yvr notifications@github.com 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 .

[plusk01]

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.

[kriswiner]

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.5deltat); 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: @.***>

[plusk01]

as @g-yvr stated, you are using an updated q1 for the calculation of q2, q3,, q4 - but this is wrong.

[kriswiner]

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: @.***>