From 39a51986ed10932f5bbe4e90c9f7c5cf8ea72cbc Mon Sep 17 00:00:00 2001 From: Ismail Degani Date: Thu, 12 Sep 2024 17:42:36 -0400 Subject: [PATCH] Fix computational error in quaternion/Madgwick implementation 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 --- src/orientation/fusion/madgwick.c | 57 ++++++++++++++++++------------- src/orientation/quaternions.c | 14 ++++---- 2 files changed, 40 insertions(+), 31 deletions(-) diff --git a/src/orientation/fusion/madgwick.c b/src/orientation/fusion/madgwick.c index 2a22f7e..247ad50 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 */ @@ -38,6 +38,20 @@ static int zsl_fus_madgwick_imu(struct zsl_vec *g, struct zsl_vec *a, ZSL_VECTOR_DEF(grad, 4); zsl_vec_init(&grad); + + /* 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) */ + zsl_quat_mult(q, &q_gyr, &q_dot); + zsl_quat_scale_d(&q_dot, 0.5); + /* Continue with the calculations only if the data from the accelerometer * is valid (non zero). */ if ((a != NULL) && ZSL_ABS(zsl_vec_norm(a)) > 1E-6) { @@ -45,24 +59,12 @@ static int zsl_fus_madgwick_imu(struct zsl_vec *g, struct zsl_vec *a, /* Normalize the acceleration vector. */ zsl_vec_to_unit(a); - /* Define the normalized quaternion of acceleration on the earth's - * reference frame, which only has a vertical component. */ - struct zsl_quat qa = { - .r = 0.0, - .i = 0.0, - .j = 0.0, - .k = 1.0 - }; - - /* Calculate the function f by using the input quaternion to rotate the - * normalised acceleration vector in the earth's reference frame, and - * then substracting the acceleration vector. */ + /* 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); - struct zsl_quat qaq; - zsl_quat_rot(q, &qa, &qaq); - f.data[0] = qaq.i - a->data[0]; - f.data[1] = qaq.j - a->data[1]; - f.data[2] = qaq.k - 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. */ @@ -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) */ + q_dot.r -= *beta * grad.data[0]; + q_dot.i -= *beta * grad.data[1]; + q_dot.j -= *beta * grad.data[2]; + q_dot.k -= *beta * grad.data[3]; } - /* Update the input quaternion with a modified quaternion integration. */ - zsl_quat_from_ang_vel(g, q, 1.0 / zsl_fus_madg_freq, q); - q->r -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[0]); - q->i -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[1]); - 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) */ + 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; /* Normalize the output quaternion. */ zsl_quat_to_unit_d(q); diff --git a/src/orientation/quaternions.c b/src/orientation/quaternions.c index 2995eee..492c351 100644 --- a/src/orientation/quaternions.c +++ b/src/orientation/quaternions.c @@ -460,7 +460,7 @@ int zsl_quat_from_ang_vel(struct zsl_vec *w, struct zsl_quat *qin, struct zsl_quat qin2; struct zsl_quat qout2; - struct zsl_quat wq; + struct zsl_quat qw; struct zsl_quat wquat = { .r = 0.0, .i = w->data[0], @@ -469,12 +469,12 @@ int zsl_quat_from_ang_vel(struct zsl_vec *w, struct zsl_quat *qin, }; zsl_quat_to_unit(qin, &qin2); - zsl_quat_mult(&wquat, &qin2, &wq); - zsl_quat_scale_d(&wq, 0.5 * t); - qout2.r = qin2.r + wq.r; - qout2.i = qin2.i + wq.i; - qout2.j = qin2.j + wq.j; - qout2.k = qin2.k + wq.k; + zsl_quat_mult(&qin2, &wquat, &qw); + zsl_quat_scale_d(&qw, 0.5 * t); + qout2.r = qin2.r + qw.r; + qout2.i = qin2.i + qw.i; + qout2.j = qin2.j + qw.j; + qout2.k = qin2.k + qw.k; zsl_quat_to_unit(&qout2, qout);