Fix computational errors in quaternion implementation and Madgwick IMU filter #62

deganii · 2024-09-12T22:28:43Z

A few errors were found in the zscilib implementation the Madgwick filter which this pull request addresses.
The python AHRS library was used as a reference for comparison, and the outputs of this implementation were compared with AHRS. The following discrepancies were found:

In quaternions.c, the order of operations in zsl_quat_from_ang_vel is reversed. The orientation of the Earth frame relative to the sensor frame must be computed as qw, not wq. One can refer to the AHRS documentation at https://ahrs.readthedocs.io/en/latest/filters/madgwick.html for more details:

In the Magwick algorithm, we found that the objective function f is computed incorrectly due to an incorrect rotation to the earth frame:

We fix this and compute f explicitly as per equation 25 of the Madgwick paper. This pull request aligns the implementation with that of AHRS and includes the equation numbers from the original Madgwick paper so that it is easy to troubleshoot.

microbuilder · 2024-09-17T08:26:15Z

Thanks for the fixes. Much appreciated. Can you raise a PR in the Zephyr project that updates the optional west manifest (submanifests/optional.yaml), so that the changes can be tested there?

Let me know if you're not aware of how to do that, but this is the normal process to test sub-modules, replacing the current commit sha with this PR, so something like:

    - name: zscilib
      path: modules/lib/zscilib
      revision: pull/62/head
      remote: upstream
      groups:
        - optional

And then once this is merged in the zscilib repo, you can update the Zephyr PR to point to the proper commit sha.

microbuilder · 2024-09-17T08:28:01Z

src/orientation/fusion/madgwick.c

+	// AHRS Convention Variables (https://github.com/Mayitzin/ahrs/)
+	struct zsl_quat q_dot;
+	struct zsl_quat q_gyr;
+
+	q_gyr.r = 0.0;
+	q_gyr.i = g->data[0];
+	q_gyr.j = g->data[1];
+	q_gyr.k = g->data[2];
+
+	// AHRS: qDot = 0.5 * q_prod(q, [0, *gyr]) # (eq. 12)


For consistency sake, do you mind using /* ... */ for comments?

Sure, this is now done

microbuilder · 2024-09-17T08:28:16Z

src/orientation/fusion/madgwick.c

-	q->j -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[2]);
-	q->k -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[3]);
+	/* Update the input quaternion */
+	// AHRS: q += qDot*self.Dt  # (eq. 13)


microbuilder · 2024-09-17T08:28:23Z

src/orientation/fusion/madgwick.c

@@ -84,14 +86,21 @@ static int zsl_fus_madgwick_imu(struct zsl_vec *g, struct zsl_vec *a,
 		/* Normalize the gradient vector. */
 		zsl_vec_from_arr(&grad, jtf.data);
 		zsl_vec_to_unit(&grad);
+
+		// AHRS: qDot -= self.gain*gradient  # (eq. 33)


microbuilder · 2024-09-17T08:29:26Z

src/orientation/quaternions.c

+	/* Make sure that the input accel is valid and q0 is given */
+	if (a == NULL) {
+		// failed to initialize quaternion due to NULL accel vector
+		rc = -EINVAL;
+		goto err;
+	}
+	if (a->sz != 3) {
+		// failed to initialize quaternion due to malformed accel vector
+		rc = -EINVAL;
+		goto err;
+	}
+	if (ZSL_ABS(zsl_vec_norm(a)) < 1E-6) {
+		// failed to initialize quaternion due to zero accel vector
+		rc = -EINVAL;
+		goto err;
+	}
+	if (q == NULL) {
+		// failed to initialize quaternion due to NULL q0
+		rc = -EINVAL;
+		goto err;
+	}


Please be consistent with comments, and also captilize (nit, but may as well fix it while you're there)

All comments here are capitalized and changed to /* ... */

microbuilder · 2024-09-17T09:27:05Z

Commits need descriptive comments, and signed-off-by before they can be merged

zscilib: fix computational errors This diff adds a top-level Zephyr PR for the zscilib quaternion/Madgwick fixes described in detail here: zephyrproject-rtos/zscilib#62 Signed-off-by: Ismail Degani <deganii@gmail.com>

deganii · 2024-09-17T21:26:11Z

Thanks for the review. Please see my updates:

I've modified to a single commit which has all the details of the fixes made
I've added the signoff requested
I've fixed all of the comment / lint issues
I've raised a top-level commit which refers to this pull request in the submanifest optional.yaml :

zephyrproject-rtos/zephyr#78595

A few errors were found in the zscilib implementation the Madgwick filter which this pull request addresses. The python AHRS library was used as a reference for comparison, and the outputs of this implementation were compared with AHRS. The following discrepancies were found: 1) In quaternions.c, the order of operations in zsl_quat_from_ang_vel is reversed. The orientation of the Earth frame relative to the sensor frame must be computed as qw, not wq. One can refer to the AHRS documentation at https://ahrs.readthedocs.io/en/latest/filters/madgwick.html for more details: https://github.com/user-attachments/assets/9d2f1d98-33c3-4d3d-9729-f89e9d12206c 2) In the Magwick algorithm, we found that the objective function f is computed incorrectly due to an incorrect rotation to the earth frame: https://github.com/user-attachments/assets/920afc04-73ac-42f3-92b2-722396107127 We fix this and compute f explicitly as per equation 25 of the Madgwick paper. This pull request aligns the implementation with that of AHRS and includes the equation numbers from the original Madgwick paper so that it is easy to troubleshoot. Signed-off-by: Ismail Degani <deganii@gmail.com>

microbuilder · 2024-09-22T12:03:03Z

Thanks again for the PR. I left a comment on the CI failures on Discord, but looked into it a bit more this morning myself.

There are a number of problems with this commit that you'll need to clean up before it can be merged in, and one that isn't related directly to the PR but that would be good to fix at the same time (in a distinct commit).

I was going to push these changes directly to your fork and branch, but I'm hesitant to do so since you're raising this PR off yourmaster branch, and I didn't want to touch that.

Commit Message

You should comply with Zephyr requirements for commit messages. Here is a rough attempt at fixing that. You can ammend your last commit message with $ git commit --amend then for push to your branch to update just the commit message $ git push -f origin master (updating for whatever branch names you use):

The subject line is important to get right in large open source projects like Zephyr due to the quantity of commits involved.

orientation: fusion: Fix an error in Madgwick

A few errors were found in the zscilib implementation the Madgwick
filter which this pull request addresses.

The python AHRS library was used as a reference for comparison, and the
outputs of this implementation were compared with AHRS. The following
discrepancies were found:

1) In quaternions.c, the order of operations in zsl_quat_from_ang_vel
   is reversed. The orientation of the Earth frame relative to the
   sensor frame must be computed as qw, not wq. One can refer to the
   AHRS documentation at
   https://ahrs.readthedocs.io/en/latest/filters/madgwick.html for
   more details:

https://github.com/user-attachments/assets/9d2f1d98-33c3-4d3d-9729-f89e9d12206c

2) In the Magwick algorithm, we found that the objective function f is
   computed incorrectly due to an incorrect rotation to the earth frame:

https://github.com/user-attachments/assets/920afc04-73ac-42f3-92b2-722396107127

   We fix this and compute f explicitly as per equation 25 of the
   Madgwick paper. This pull request aligns the implementation with
   that of AHRS and includes the equation numbers from the original
   Madgwick paper so that it is easy to troubleshoot.

madgwick.c Issues

There are a number of issues here. You should be able to catch most of these by running compliance checks, as described in the readme.md in zscilib, and looking at the error messages in the zephyr PR to find the failures: https://github.com/zephyrproject-rtos/zephyr/actions/runs/10911326987/job/30283906252?pr=78595

You may need to run $ pip3 install -r requirements-compliance.txt if you get an error about missing python depdencies

$ ../../../zephyr/scripts/ci/check_compliance.py \
  -m Gitlint -m Identity -m Nits -m pylint -m checkpatch \
  -c HEAD~2..

Here's a diff on some suggestions on how to clean some of these issues up:

diff --git a/src/orientation/fusion/madgwick.c b/src/orientation/fusion/madgwick.c
index 247ad50..f1bb5f7 100644
--- a/src/orientation/fusion/madgwick.c
+++ b/src/orientation/fusion/madgwick.c
@@ -1,5 +1,5 @@
 /*
-       * Copyright (c) 2021 Marti Riba Pons
+ * Copyright (c) 2021 Marti Riba Pons
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -59,12 +59,13 @@ static int zsl_fus_madgwick_imu(struct zsl_vec *g, struct zsl_vec *a,
                /* Normalize the acceleration vector. */
                zsl_vec_to_unit(a);
 
-               /* Calculate the objective function f by simplifying the sensor field s to
-                * g=[0,0,0,1] and subtracting the acceleration vector. (Equation 25) */
+               /* Calculate the objective function f by simplifying the sensor field s
+                * to g=[0,0,0,1] and subtracting the acceleration vector. (Equation 25)
+                */
                ZSL_MATRIX_DEF(f, 3, 1);
-               f.data[0] = 2.0*(q->i*q->k - q->r *q->j) - a->data[0];
-               f.data[1] = 2.0*(q->r*q->i + q->j *q->k) - a->data[1];
-               f.data[2] = 2.0*(0.5 - q->i*q->i - q->j *q->j) - a->data[2];
+               f.data[0] = 2.0*(q->i*q->k - q->r * q->j) - a->data[0];
+               f.data[1] = 2.0*(q->r*q->i + q->j * q->k) - a->data[1];
+               f.data[2] = 2.0*(0.5 - q->i*q->i - q->j * q->j) - a->data[2];
 
                /* Define and compute the transpose of the Jacobian matrix of the
                 * function f. */
@@ -94,13 +95,13 @@ static int zsl_fus_madgwick_imu(struct zsl_vec *g, struct zsl_vec *a,
                q_dot.k -= *beta * grad.data[3];
        }
 
-       /* Update the input quaternion 
+       /* Update the input quaternion
         * AHRS: q += qDot*self.Dt  # (eq. 13) */
-       zsl_real_t dt = 1.0 / zsl_fus_madg_freq;
-       q -> r += q_dot.r * dt;
-       q -> i += q_dot.i * dt;
-       q -> j += q_dot.j * dt;
-       q -> k += q_dot.k * dt;
+       zsl_real_t dt = (zsl_real_t)1.0 / zsl_fus_madg_freq;
+       q->r += q_dot.r * dt;
+       q->i += q_dot.i * dt;
+       q->j += q_dot.j * dt;
+       q->k += q_dot.k * dt;
 
        /* Normalize the output quaternion. */
        zsl_quat_to_unit_d(q);

Madwick Test Failure

One of the madgwick tests is failing, perhaps because we need to lower the precision in the value comparison to account for differences between single and double-precision calculations, or small differences across compilers, emulators, etc. Here is a diff of how that can be changed, which should go in it's own commit.

This may also be caused, however, by the changes in your PR as well, though, meaning the test itself needs to be updated. Please verify?

diff --git a/tests/src/fusion_tests.c b/tests/src/fusion_tests.c
index 55e8a83..72a8bd2 100644
--- a/tests/src/fusion_tests.c
+++ b/tests/src/fusion_tests.c
@@ -906,18 +906,18 @@ ZTEST(zsl_tests, test_fus_kalman)
        /* Run the kalman algorithm. */
        rc = kalm_drv.feed_handler(&a, &m, &g, NULL, &q, kalm_drv.config);
        zassert_true(rc == 0, NULL);
-       zassert_true(val_is_equal(q.r, 0.3186651048490019, 1E-6), NULL);
-       zassert_true(val_is_equal(q.i, 0.6386866767275943, 1E-6), NULL);
-       zassert_true(val_is_equal(q.j, 0.4721142772080947, 1E-6), NULL);
-       zassert_true(val_is_equal(q.k, 0.5173393365852964, 1E-6), NULL);
+       zassert_true(val_is_equal(q.r, 0.3186651048490019, 1E-4), NULL);
+       zassert_true(val_is_equal(q.i, 0.6386866767275943, 1E-4), NULL);
+       zassert_true(val_is_equal(q.j, 0.4721142772080947, 1E-4), NULL);
+       zassert_true(val_is_equal(q.k, 0.5173393365852964, 1E-4), NULL);
 
        /* Run the kalman algorithm with dip angle provided. */
        rc = kalm_drv.feed_handler(&a, &m, &g, &incl, &q, kalm_drv.config);
        zassert_true(rc == 0, NULL);
-       zassert_true(val_is_equal(q.r, 0.7715963603719183, 1E-6), NULL);
-       zassert_true(val_is_equal(q.i, 0.0123858064715810, 1E-6), NULL);
-       zassert_true(val_is_equal(q.j, 0.5043103125990586, 1E-6), NULL);
-       zassert_true(val_is_equal(q.k, 0.3875006542769926, 1E-6), NULL);
+       zassert_true(val_is_equal(q.r, 0.7715963603719183, 1E-4), NULL);
+       zassert_true(val_is_equal(q.i, 0.0123858064715810, 1E-4), NULL);
+       zassert_true(val_is_equal(q.j, 0.5043103125990586, 1E-4), NULL);
+       zassert_true(val_is_equal(q.k, 0.3875006542769926, 1E-4), NULL);
 
        /* Run the kalman algorithm without accelerometer data. An error
         * is expected. */

There may be other issues.

I'd also suggest creating a branch for every PR you raise, not basing it off the default brain (main, etc.) If every PR has it's own branch, it's much easier to make chages to the code for that PR for maintainers, etc.

microbuilder · 2024-10-09T10:45:35Z

@deganii Any update on this? The changes are useful and I'd love to get them merged in.

microbuilder requested changes Sep 17, 2024

View reviewed changes

microbuilder mentioned this pull request Sep 17, 2024

Incorporate AHRS acc2q into zscilib as zsl_quat_from_accel #63

Open

deganii force-pushed the master branch 7 times, most recently from 74e81dc to ca90687 Compare September 17, 2024 20:57

deganii mentioned this pull request Sep 17, 2024

zscilib: Add Zephyr PR for Pull Request 62 zephyrproject-rtos/zephyr#78595

Open

deganii force-pushed the master branch from ca90687 to 39a5198 Compare September 17, 2024 22:08

deganii force-pushed the master branch from 39a5198 to b1b35fe Compare September 17, 2024 22:34

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix computational errors in quaternion implementation and Madgwick IMU filter #62

Fix computational errors in quaternion implementation and Madgwick IMU filter #62

deganii commented Sep 12, 2024

microbuilder commented Sep 17, 2024

microbuilder Sep 17, 2024

deganii Sep 17, 2024

microbuilder Sep 17, 2024

deganii Sep 17, 2024

microbuilder Sep 17, 2024

deganii Sep 17, 2024

microbuilder Sep 17, 2024

deganii Sep 17, 2024

microbuilder commented Sep 17, 2024

deganii commented Sep 17, 2024 •

edited

Loading

microbuilder commented Sep 22, 2024 •

edited

Loading

microbuilder commented Oct 9, 2024

Fix computational errors in quaternion implementation and Madgwick IMU filter #62

Are you sure you want to change the base?

Fix computational errors in quaternion implementation and Madgwick IMU filter #62

Conversation

deganii commented Sep 12, 2024

microbuilder commented Sep 17, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

microbuilder commented Sep 17, 2024

deganii commented Sep 17, 2024 • edited Loading

microbuilder commented Sep 22, 2024 • edited Loading

Commit Message

madgwick.c Issues

Madwick Test Failure

microbuilder commented Oct 9, 2024

deganii commented Sep 17, 2024 •

edited

Loading

microbuilder commented Sep 22, 2024 •

edited

Loading