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 <deganii@gmail.com>

Author: deganii

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,31 +38,33 @@ 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) {
 
 		/* 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);

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);