Incorporate AHRS acc2q into zscilib as zsl_quat_from_accel

Currently, the zscilib's Madgwick orientation filter starts with an orientation of <1, 0, 0, 0> regardless of the physical IMU's orientation.

The filter then begins the slow/painful process of converging to the correct orientation which can take literally minutes. For an IMU running at 120Hz, the worst-case Madgwick convergence time is about 47 seconds (β * Dt = 0.033 * 1/120Hz). This is far too long for a warm-up; a Madgwick filter that has not yet converged will output spurious and confusing orientations and result in a bad user experience.

A python implementation of Magwick, called AHRS, solves this bootstrapping problem with a non-iterative algorithm called acc2q (acceleration => quat)

AHRS Implementation:
https://github.com/Mayitzin/ahrs/blob/595ab9877c77f5960e39b352c0ef551d2d2efcb9/ahrs/common/orientation.py#L970
Reference:
([Trimpe et al]

https://idsc.ethz.ch/content/dam/ethz/special-interest/mavt/dynamic-systems-n-control/idsc-dam/Research_DAndrea/Balancing%20Cube/ICRA10_1597_web.pdf

This algorithm that relies only on the current accelerometer sample. The integration of the acc2q algorithm is highlighted in the below flow diagram (dotted red block).

https://github.com/user-attachments/assets/6a32fe55-2233-4b40-bec4-85e4af0750a3

Computing q0 and feeing this into Madgwick results in a very reasonable q0 orientation and a much better overall experience.

Signed-off-by: Ismail Degani <deganii@gmail.com>

include/zsl/orientation/quaternions.h

@@ -423,6 +423,29 @@ int zsl_quat_from_ang_vel(struct zsl_vec *w, struct zsl_quat *qin,
 int zsl_quat_from_ang_mom(struct zsl_vec *l, struct zsl_quat *qin,
 			  zsl_real_t *i, zsl_real_t t, struct zsl_quat *qout);
 
+
+/**
+ * @brief Sets an orientation quaternion from an input acceleration sample.
+ *
+ * Given a tri-axial acceleration vector a = <x ,y, z> in m/s^2, compute and set a
+ * quaternion that describes the current orientation (q) of a body.
+ * This function returns the estimation of the orientation of the body in the
+ * form of a unit quaternion (q).
+ *
+ * This is direct port from AHRS's orientation library. (https://ahrs.readthedocs.io/)
+ * This function is useful for "bootstrapping" the orientation of a body
+ * when there is no prior quaternion q[n-1] to estimate q[n].
+ * The output q can serve as the q0 input to the Madgwick IMU filter.
+ *
+ * @param a     Tridimensional acceleration vector, in meters per second squared.
+ * @param q     The output orientation quaternion of the body
+ *
+ * @return 0 if everything executed normally, or a negative error code if the
+ *         acceleration vector is null or its dimension is not 3.
+ */
+int zsl_quat_from_accel(struct zsl_vec *a, struct zsl_quat *q);
+
+
 /**
  * @brief Converts a unit quaternion to it's equivalent Euler angle. Euler
  *        values expressed in radians.

src/orientation/quaternions.c

@@ -509,6 +509,65 @@ int zsl_quat_from_ang_mom(struct zsl_vec *l, struct zsl_quat *qin,
 	return rc;
 }
 
+
+int zsl_quat_from_accel(struct zsl_vec *a, struct zsl_quat *q)
+{
+	int rc = 0;
+
+#if CONFIG_ZSL_BOUNDS_CHECKS
+	/* 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;
+	}
+#endif
+
+	q->r = 1.0;
+	q->i = 0.0;
+	q->j = 0.0;
+	q->k = 0.0;
+
+	float a_norm = zsl_vec_norm(a);
+	float ax = a->data[0] / a_norm;
+	float ay = a->data[1] / a_norm;
+	float az = a->data[2] / a_norm;
+
+	float ex = ZSL_ATAN2(ay, az);
+	float ey = ZSL_ATAN2(-ax, ZSL_SQRT(ay * ay + az * az));
+
+	float cx2 = ZSL_COS(ex / 2.0);
+	float sx2 = ZSL_SIN(ex / 2.0);
+	float cy2 = ZSL_COS(ey / 2.0);
+	float sy2 = ZSL_SIN(ey / 2.0);
+
+	q->r = cx2 * cy2;
+	q->i = sx2 * cy2;
+	q->j = cx2 * sy2;
+	q->k = -sx2 * sy2;
+
+#if CONFIG_ZSL_BOUNDS_CHECKS
+err:
+#endif
+	return rc;
+}
+
+
 int zsl_quat_to_euler(struct zsl_quat *q, struct zsl_euler *e)
 {
 	int rc = 0;