[indi] used proper quaternion 2nd order filtering (#3651)

The filtering of euler angles was not correct, even with the correction for yaw from #3624

sw/airborne/filters/quaternion_filter.h

@@ -0,0 +1,191 @@
+/*
+ * Copyright (C) 2026 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/** @file filters/quaternion_filter.h
+ *  @brief Quaternion second order filter
+ *
+ */
+
+#ifndef QUATERNION_FILTER_H
+#define QUATERNION_FILTER_H
+
+#include "std.h"
+#include "math/pprz_algebra_float.h"
+
+/** Quaternion second order filter model (float) */
+struct QuatSecondOrderLowPass {
+  struct FloatQuat  quat;
+  struct FloatRates rate;
+  struct FloatRates accel;
+
+  float two_zeta_omega;
+  float two_omega2;
+  float dt;
+};
+
+/** Init second order quaternion low pass filter.
+ *
+ * @param filter second order low pass filter structure
+ * @param omega cutoff frequency of the second order low pass filter
+ * @param damp damping coefficient of the second order low pass filter
+ * @param sample_time sampling period of the signal
+ * @param q0 initial orientation of the filter
+ */
+static inline void init_quat_second_order_low_pass(
+    struct QuatSecondOrderLowPass *filter,
+    const float omega,
+    const float damp,
+    const float sample_time,
+    const struct FloatQuat q0)
+{
+  filter->quat = q0;
+  FLOAT_RATES_ZERO(filter->rate);
+  FLOAT_RATES_ZERO(filter->accel);
+  filter->two_zeta_omega = 2.f * damp * omega;
+  filter->two_omega2 = 2.f * omega * omega;
+  filter->dt = sample_time;
+}
+
+/** Reset the second order quaternion low-pass filter to a specific value.
+ *
+ * @param filter filter structure
+ * @param q0 quaternion to reset the filter to
+ */
+static inline void reset_quat_second_order_low_pass(
+    struct QuatSecondOrderLowPass *filter,
+    const struct FloatQuat q0)
+{
+  filter->quat = q0;
+  FLOAT_RATES_ZERO(filter->rate);
+  FLOAT_RATES_ZERO(filter->accel);
+}
+
+/** Update second order quaterion low pass filter state with a new value.
+ *
+ * @param filter filter structure
+ * @param q quaternion input value of the filter
+ * @return new filtered quaternion
+ */
+static inline struct FloatQuat update_quat_second_order_low_pass(
+    struct QuatSecondOrderLowPass *filter,
+    const struct FloatQuat q)
+{
+  /* integrate quaternion */
+  struct FloatQuat qdot;
+  float_quat_derivative(&qdot, &filter->rate, &filter->quat);
+  QUAT_SMUL(qdot, qdot, filter->dt);
+  QUAT_ADD(filter->quat, qdot);
+  float_quat_normalize(&filter->quat);
+
+  /* integrate rotational speed */
+  struct FloatRates delta_rate;
+  RATES_SMUL(delta_rate, filter->accel, filter->dt);
+  RATES_ADD(filter->rate, delta_rate);
+
+  /* compute angular accelerations */
+  struct FloatQuat err;
+  /* attitude error */
+  float_quat_inv_comp(&err, &q, &filter->quat);
+  /* wrap it in the shortest direction */
+  float_quat_wrap_shortest(&err);
+  /* propagate the 2nd order linear model: xdotdot = -2*zeta*omega*xdot - omega^2*x */
+  /* since error quaternion contains the half-angles we get 2*omega^2*err */
+  filter->accel.p = -filter->two_zeta_omega * filter->rate.p - filter->two_omega2 * err.qx;
+  filter->accel.q = -filter->two_zeta_omega * filter->rate.q - filter->two_omega2 * err.qy;
+  filter->accel.r = -filter->two_zeta_omega * filter->rate.r - filter->two_omega2 * err.qz;
+
+  return filter->quat;
+}
+
+/** Get current value of the second order quaterion low pass filter.
+ *
+ * @param filter filter structure
+ * @return current orientation of the filter
+ */
+static inline struct FloatQuat get_quat_second_order_low_pass(
+    const struct QuatSecondOrderLowPass *filter)
+{
+  return filter->quat;
+}
+
+
+/** Second order Butterworth low pass filter.
+ */
+typedef struct QuatSecondOrderLowPass QuatButterworthLowPass;
+
+/** Init a second order quaternion Butterworth filter.
+ *
+ * based on the generic second order filter
+ * with zeta = 0.7071 = 1/sqrt(2)
+ *
+ * @param filter second order quaternion Butterworth low pass filter structure
+ * @param omega cutoff frequency of the second order low pass filter
+ * @param sample_time sampling period of the signal
+ * @param q0 initial orientation of the filter
+ */
+static inline void init_quat_butterworth_low_pass(
+    QuatButterworthLowPass *filter,
+    const float omega,
+    const float sample_time,
+    const struct FloatQuat q0)
+{
+  init_quat_second_order_low_pass((struct QuatSecondOrderLowPass *)filter, omega, 0.7071f, sample_time, q0);
+}
+
+/** Update second order quaternion Butterworth low pass filter state with a new value.
+ *
+ * @param filter second order quaternion Butterworth low pass filter structure
+ * @param q new input orientation of the filter
+ * @return new filtered quaternion
+ */
+static inline struct FloatQuat update_quat_butterworth_low_pass(
+    QuatButterworthLowPass *filter,
+    const struct FloatQuat quat)
+{
+  return update_quat_second_order_low_pass((struct QuatSecondOrderLowPass *)filter, quat);
+}
+
+/**
+ * @brief Reset a quaternion Butterworth low-pass filter to a specific value.
+ *
+ * @param[out] filter QuatButterworth2LowPass filter instance to reset.
+ * @param[in] quat Value to reset the filter to.
+ */
+static inline void reset_quat_butterworth_low_pass(
+    QuatButterworthLowPass *filter,
+    const struct FloatQuat quat)
+{
+  reset_quat_second_order_low_pass((struct QuatSecondOrderLowPass *)filter, quat);
+}
+
+/** Get current value of the second order quaternion Butterworth low pass filter.
+ *
+ * @param filter second order quaternion Butterworth low pass filter structure
+ * @return current orientation of the filter
+ */
+static inline struct FloatQuat get_quat_butterworth_low_pass(
+    const QuatButterworthLowPass *filter)
+{
+  return get_quat_second_order_low_pass((const struct QuatSecondOrderLowPass *)filter);
+}
+
+#endif
+

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c

@@ -41,6 +41,7 @@
 #include "state.h"
 #include "autopilot.h"
 #include "filters/low_pass_filter.h"
+#include "filters/quaternion_filter.h"
 #include "modules/core/abi.h"
 
 // The acceleration reference is calculated with these gains. If you use GPS,
@@ -104,10 +105,8 @@ static void guidance_indi_filter_thrust(void);
 float thrust_dyn = 0.f;
 float thrust_act = 0.f;
 Butterworth2LowPass filt_accel_ned[3];
-Butterworth2LowPass roll_filt;
-Butterworth2LowPass pitch_filt;
-Butterworth2LowPass yaw_filt;
 Butterworth2LowPass thrust_filt;
+QuatButterworthLowPass quat_filt;
 
 static float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU];
 
@@ -164,7 +163,7 @@ struct ThrustSetpoint thrust_sp;
 float thrust_in;
 float thrust_vect[3];
 
-static void guidance_indi_propagate_filters(struct FloatEulers *eulers);
+static void guidance_indi_propagate_filters(void);
 
 //------------------------------------------------------------//
 
@@ -233,10 +232,8 @@ void guidance_indi_enter(void)
   for (int8_t i = 0; i < 3; i++) {
     init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
   }
-  init_butterworth_2_low_pass(&roll_filt, tau, sample_time, stateGetNedToBodyEulers_f()->phi);
-  init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, stateGetNedToBodyEulers_f()->theta);
-  init_butterworth_2_low_pass(&yaw_filt, tau, sample_time, stateGetNedToBodyEulers_f()->psi);
   init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
+  init_quat_butterworth_low_pass(&quat_filt, filter_cutoff, sample_time, *stateGetNedToBodyQuat_f());
 }
 
 /**
@@ -248,15 +245,14 @@ void guidance_indi_enter(void)
  */
 struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
 {
-  struct FloatEulers euler_yxz;
-  struct FloatQuat * statequat = stateGetNedToBodyQuat_f();
-  float_eulers_of_quat_yxz(&euler_yxz, statequat);
+  struct FloatEulers eulers_filtered;
+  float_eulers_of_quat_yxz(&eulers_filtered, &quat_filt.quat);
 
   // set global accel sp variable FIXME clean this
   sp_accel = *accel_sp;
 
   //filter accel to get rid of noise and filter attitude to synchronize with accel
-  guidance_indi_propagate_filters(&euler_yxz);
+  guidance_indi_propagate_filters();
 
   struct FloatVect3 a_diff = {
     sp_accel.x - filt_accel_ned[0].o[0],
@@ -281,8 +277,8 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   wls_guid_p.v[1] = m*a_diff.y;
   wls_guid_p.v[2] = m*a_diff.z;
 
-  guidance_indi_set_wls_settings(&wls_guid_p, &euler_yxz, heading_sp);
-  guidance_indi_calcG(Gmat, euler_yxz);
+  guidance_indi_set_wls_settings(&wls_guid_p, &quat_filt.quat, heading_sp);
+  guidance_indi_calcG(Gmat, &quat_filt.quat);
   for (int i = 0; i < GUIDANCE_INDI_NV; i++) {
     Bwls_gi[i] = Gmat[i];
   }
@@ -291,9 +287,9 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
     du_guidance [i] = wls_guid_p.u[i];
   }
 
-  guidance_euler_cmd.phi   = roll_filt.o[0]  + du_guidance[0];
-  guidance_euler_cmd.theta = pitch_filt.o[0] + du_guidance[1];
-  guidance_euler_cmd.psi   = yaw_filt.o[0]   + du_guidance[2];
+  guidance_euler_cmd.phi   = eulers_filtered.phi + du_guidance[0];
+  guidance_euler_cmd.theta = eulers_filtered.theta + du_guidance[1];
+  guidance_euler_cmd.psi   = eulers_filtered.psi + du_guidance[2];
   thrust_vect[0] = du_guidance[3]; // (TX)
   thrust_vect[1] = du_guidance[4]; // (TY)
   thrust_vect[2] = du_guidance[5]; // (TZ)
@@ -302,7 +298,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
 #else // !USE_WLS
 
   // Calculate matrix of partial derivatives
-  guidance_indi_calcG(Gmat, euler_yxz);
+  guidance_indi_calcG(Gmat, &quat_filt.quat);
 
   RMAT_ELMT(Ga, 0, 0) = Gmat[0][0];
   RMAT_ELMT(Ga, 1, 0) = Gmat[1][0];
@@ -318,8 +314,8 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   // Calculate roll,pitch and thrust command
   MAT33_VECT3_MUL(control_increment, Ga_inv, a_diff);
 
-  guidance_euler_cmd.theta = pitch_filt.o[0] + control_increment.x;
-  guidance_euler_cmd.phi   = roll_filt.o[0]  + control_increment.y;
+  guidance_euler_cmd.theta = eulers_filtered.theta + control_increment.x;
+  guidance_euler_cmd.phi   = eulers_filtered.phi + control_increment.y;
   guidance_euler_cmd.psi   = heading_sp;
 
   // Compute and store thust setpoint
@@ -457,16 +453,13 @@ void guidance_indi_filter_thrust(void)
  * The roll and pitch also need to be filtered to synchronize them with the
  * acceleration
  */
-void guidance_indi_propagate_filters(struct FloatEulers *eulers)
+void guidance_indi_propagate_filters(void)
 {
   struct NedCoor_f *accel = stateGetAccelNed_f();
   update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
   update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
   update_butterworth_2_low_pass(&filt_accel_ned[2], accel->z);
-
-  update_butterworth_2_low_pass(&roll_filt, eulers->phi);
-  update_butterworth_2_low_pass(&pitch_filt, eulers->theta);
-  update_butterworth_2_low_pass(&yaw_filt, eulers->psi);
+  update_quat_butterworth_low_pass(&quat_filt, *stateGetNedToBodyQuat_f());
 }
 
 /**

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h

@@ -64,10 +64,10 @@ extern struct FloatVect3 guidance_indi_controller(bool in_flight, struct Horizon
 #endif
 
 // Function to compute efficiency matrix G
-extern void guidance_indi_calcG(float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU], struct FloatEulers att);
+extern void guidance_indi_calcG(float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU], const struct FloatQuat *att);
 #if GUIDANCE_INDI_USE_WLS
 #include "math/wls/wls_alloc.h"
-extern void guidance_indi_set_wls_settings(struct WLS_t *wls, struct FloatEulers *euler_yxz, float heading_sp);
+extern void guidance_indi_set_wls_settings(struct WLS_t *wls, const struct FloatQuat *att, const float heading_sp);
 #endif
 
 extern float guidance_indi_specific_force_gain;

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c

@@ -52,8 +52,10 @@ float guidance_indi_max_v_thrust = GUIDANCE_INDI_MAX_V_THRUST;
  * w.r.t. the NED accelerations for YXZ eulers
  * ddx = G*[dphi,dtheta,dpsi,dTx,dTy,dTz]
  */
-static void guidance_indi_calcG_yxz(float Gmat[3][6], struct FloatEulers euler_yxz)
+void guidance_indi_calcG(float Gmat[3][6], const struct FloatQuat *att)
 {
+  struct FloatEulers euler_yxz;
+  float_eulers_of_quat_yxz(&euler_yxz, att);
   // Euler Angles
   const float sphi = sinf(euler_yxz.phi);
   const float cphi = cosf(euler_yxz.phi);
@@ -98,15 +100,7 @@ static void guidance_indi_calcG_yxz(float Gmat[3][6], struct FloatEulers euler_y
   Gmat[2][5] =  ctheta * cphi;
 }
 
-/** Compute effectiveness matrix for guidance
- *
- * @param Gmat Dynamics matrix
- */
-void guidance_indi_calcG(float Gmat[3][6], struct FloatEulers euler) {
-  guidance_indi_calcG_yxz(Gmat, euler);
-}
-
-void guidance_indi_set_wls_settings(struct WLS_t *wls, struct FloatEulers *euler_yxz, float heading_sp)
+void guidance_indi_set_wls_settings(struct WLS_t *wls, const struct FloatQuat *att, const float heading_sp)
 {
   struct FloatEulers euler_yxz_ref = { 0 };
 #if GUIDANCE_INDI_RC_SWITCH_EULER
@@ -115,28 +109,31 @@ void guidance_indi_set_wls_settings(struct WLS_t *wls, struct FloatEulers *euler
 #endif
   euler_yxz_ref.psi = heading_sp;
 
+  struct FloatEulers euler_yxz;
+  float_eulers_of_quat_yxz(&euler_yxz, att);
+
   // Set lower limits
-  wls->u_min[0] = -guidance_indi_max_bank - euler_yxz->phi;          // phi
-  wls->u_min[1] = -guidance_indi_max_bank - euler_yxz->theta;        // theta
-  wls->u_min[2] = -M_PI - euler_yxz->psi;                            // psi
-  wls->u_min[3] = -guidance_indi_max_h_thrust - stab_thrust_filt.x;  // Tx
-  wls->u_min[4] = -guidance_indi_max_h_thrust - stab_thrust_filt.y;  // Ty
-  wls->u_min[5] = -guidance_indi_max_v_thrust - stab_thrust_filt.z;  // Tz
+  wls->u_min[0] = -guidance_indi_max_bank - euler_yxz.phi;          // phi
+  wls->u_min[1] = -guidance_indi_max_bank - euler_yxz.theta;        // theta
+  wls->u_min[2] = -M_PI - euler_yxz.psi;                            // psi
+  wls->u_min[3] = -guidance_indi_max_h_thrust - stab_thrust_filt.x; // Tx
+  wls->u_min[4] = -guidance_indi_max_h_thrust - stab_thrust_filt.y; // Ty
+  wls->u_min[5] = -guidance_indi_max_v_thrust - stab_thrust_filt.z; // Tz
 
   // Set lower limits limits
-  wls->u_max[0] = guidance_indi_max_bank - euler_yxz->phi;          // phi
-  wls->u_max[1] = guidance_indi_max_bank - euler_yxz->theta;        // theta
-  wls->u_max[2] = M_PI - euler_yxz->psi;                            // psi
-  wls->u_max[3] = guidance_indi_max_h_thrust - stab_thrust_filt.x;  // Tx
-  wls->u_max[4] = guidance_indi_max_h_thrust - stab_thrust_filt.y;  // Ty
-  wls->u_max[5] = 9.81f * GUIDANCE_INDI_MASS - stab_thrust_filt.z;  // Tz
+  wls->u_max[0] = guidance_indi_max_bank - euler_yxz.phi;          // phi
+  wls->u_max[1] = guidance_indi_max_bank - euler_yxz.theta;        // theta
+  wls->u_max[2] = M_PI - euler_yxz.psi;                            // psi
+  wls->u_max[3] = guidance_indi_max_h_thrust - stab_thrust_filt.x; // Tx
+  wls->u_max[4] = guidance_indi_max_h_thrust - stab_thrust_filt.y; // Ty
+  wls->u_max[5] = 9.81f * GUIDANCE_INDI_MASS - stab_thrust_filt.z; // Tz
 
   // Set prefered states
-  wls->u_pref[0] = euler_yxz_ref.phi - euler_yxz->phi;     // prefered delta phi
-  wls->u_pref[1] = euler_yxz_ref.theta - euler_yxz->theta; // prefered delta theta
-  wls->u_pref[2] = euler_yxz_ref.psi - euler_yxz->psi;     // prefered delta psi
-  wls->u_pref[3] = stab_thrust_filt.x;                     // prefered delta Tx
-  wls->u_pref[4] = stab_thrust_filt.y;                     // prefered delta Ty
-  wls->u_pref[5] = stab_thrust_filt.z;                     // prefered delta Tz
+  wls->u_pref[0] = euler_yxz_ref.phi - euler_yxz.phi;     // prefered delta phi
+  wls->u_pref[1] = euler_yxz_ref.theta - euler_yxz.theta; // prefered delta theta
+  wls->u_pref[2] = euler_yxz_ref.psi - euler_yxz.psi;     // prefered delta psi
+  wls->u_pref[3] = stab_thrust_filt.x;                    // prefered delta Tx
+  wls->u_pref[4] = stab_thrust_filt.y;                    // prefered delta Ty
+  wls->u_pref[5] = stab_thrust_filt.z;                    // prefered delta Tz
 }
 

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c

@@ -36,6 +36,7 @@
 #include "autopilot.h"
 #include "stdio.h"
 #include "filters/low_pass_filter.h"
+#include "filters/quaternion_filter.h"
 #include "modules/core/abi.h"
 #include "firmwares/rotorcraft/navigation.h"
 
@@ -234,14 +235,10 @@ struct FloatEulers eulers_zxy;
 float thrust_dyn = 0.f;
 float thrust_act = 0.f;
 Butterworth2LowPass filt_accel_ned[3];
-Butterworth2LowPass roll_filt;
-Butterworth2LowPass pitch_filt;
 Butterworth2LowPass thrust_filt;
 Butterworth2LowPass accely_filt;
 Butterworth2LowPass guidance_indi_airspeed_filt;
-static Butterworth2LowPass yaw_delta_filt;
-static float previous_yaw_raw = 0.0f;
-float yaw_filt = 0.0f;
+QuatButterworthLowPass quat_filt;
 
 struct FloatVect2 desired_airspeed;
 float gi_unbounded_airspeed_sp = 0.f;
@@ -361,11 +358,9 @@ void guidance_indi_init(void)
   for(int8_t i=0; i<3; i++) {
     init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
   }
-  init_butterworth_2_low_pass(&roll_filt, tau, sample_time, 0.0);
-  init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, 0.0);
-  init_butterworth_2_low_pass(&yaw_delta_filt, tau, sample_time, 0.0);
-  previous_yaw_raw = 0.0f;
-  yaw_filt = 0.0f;
+  struct FloatQuat q0;
+  float_quat_identity(&q0);
+  init_quat_butterworth_low_pass(&quat_filt, filter_cutoff, sample_time, q0);
   init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
   init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
 
@@ -408,12 +403,7 @@ void guidance_indi_enter(void)
     init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
   }
 
-  init_butterworth_2_low_pass(&roll_filt, tau, sample_time, eulers_zxy.phi);
-  init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, eulers_zxy.theta);
-  init_butterworth_2_low_pass(&yaw_delta_filt, tau, sample_time, 0.0);
-  // Initialize yaw tracking variables
-  previous_yaw_raw = eulers_zxy.psi;
-  yaw_filt = eulers_zxy.psi;
+  init_quat_butterworth_low_pass(&quat_filt, filter_cutoff, sample_time, *stateGetNedToBodyQuat_f());
   init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
   init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
 
@@ -454,6 +444,8 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
 
   /* Obtain eulers with zxy rotation order */
   float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+  struct FloatEulers eulers_filtered;
+  float_eulers_of_quat_zxy(&eulers_filtered, &quat_filt.quat);
 
   /* Calculate the transition ratio so that the ctrl_effecitveness scheduling works */
   stabilization.transition_ratio = eulers_zxy.theta / RadOfDeg(-75.0f);
@@ -502,7 +494,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
 #if GUIDANCE_INDI_HYBRID_USE_WLS
 
   // Calculate the maximum deflections
-  guidance_indi_hybrid_set_wls_settings(v_gih, roll_filt.o[0], pitch_filt.o[0]);
+  guidance_indi_hybrid_set_wls_settings(v_gih, eulers_filtered.phi, eulers_filtered.theta);
 
   float du_gih[GUIDANCE_INDI_HYBRID_U]; // = {0.0f, 0.0f, 0.0f};
 
@@ -537,8 +529,8 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   // We are dividing by the airspeed, so a lower bound is important
   Bound(airspeed_turn, gih_coordinated_turn_min_airspeed, gih_coordinated_turn_max_airspeed);
 
-  guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x;
-  guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y;
+  guidance_euler_cmd.phi = eulers_filtered.phi + euler_cmd.x;
+  guidance_euler_cmd.theta = eulers_filtered.theta + euler_cmd.y;
 
   //Bound euler angles to prevent flipping
   Bound(guidance_euler_cmd.phi, -guidance_indi_max_bank, guidance_indi_max_bank);
@@ -873,8 +865,7 @@ void guidance_indi_filter_thrust(void)
 
 /**
  * Low pass the accelerometer measurements to remove noise from vibrations.
- * The roll and pitch also need to be filtered to synchronize them with the
- * acceleration
+ * The orientation needs to be filtered to synchronize with the acceleration
  * Called as a periodic function with PERIODIC_FREQ
  */
 void guidance_indi_propagate_filters(void)
@@ -884,17 +875,7 @@ void guidance_indi_propagate_filters(void)
   update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
   update_butterworth_2_low_pass(&filt_accel_ned[2], accel->z);
 
-  update_butterworth_2_low_pass(&roll_filt, eulers_zxy.phi);
-  update_butterworth_2_low_pass(&pitch_filt, eulers_zxy.theta);
-  
-  // Filter yaw delta instead of raw angle to handle wrapping properly
-  float yaw_delta = eulers_zxy.psi - previous_yaw_raw;
-  FLOAT_ANGLE_NORMALIZE(yaw_delta);  // Normalize to [-pi, pi]
-  update_butterworth_2_low_pass(&yaw_delta_filt, yaw_delta);
-  previous_yaw_raw = eulers_zxy.psi;
-  // Accumulate filtered delta
-  yaw_filt += yaw_delta_filt.o[0];
-  FLOAT_ANGLE_NORMALIZE(yaw_filt);
+  update_quat_butterworth_low_pass(&quat_filt, *stateGetNedToBodyQuat_f());
 
   // Propagate filter for sideslip correction
   float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h

@@ -34,6 +34,7 @@
 #include "math/pprz_algebra_int.h"
 #include "math/pprz_algebra_float.h"
 #include "filters/low_pass_filter.h"
+#include "filters/quaternion_filter.h"
 #include "firmwares/rotorcraft/guidance.h"
 #include "firmwares/rotorcraft/stabilization.h"
 
@@ -110,8 +111,6 @@ extern bool force_forward;       ///< forward flight for hybrid nav
 extern bool guidance_indi_airspeed_filtering;
 extern bool coordinated_turn_use_accel;
 
-extern Butterworth2LowPass roll_filt;
-extern Butterworth2LowPass pitch_filt;
-extern float yaw_filt;
+extern QuatButterworthLowPass quat_filt;
 
 #endif /* GUIDANCE_INDI_HYBRID_H */

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c

@@ -84,13 +84,16 @@ void guidance_indi_quadplane_propagate_filters(void) {
  * @param body_v 3D vector to write the control objective v
  */
 void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float body_v[GUIDANCE_INDI_HYBRID_V]) {
+
+  struct FloatEulers eulers_filtered;
+  float_eulers_of_quat_zxy(&eulers_filtered, &quat_filt.quat);
   /*Pre-calculate sines and cosines*/
-  float sphi = sinf(roll_filt.o[0]);
-  float cphi = cosf(roll_filt.o[0]);
-  float stheta = sinf(pitch_filt.o[0]);
-  float ctheta = cosf(pitch_filt.o[0]);
-  float spsi = sinf(yaw_filt);
-  float cpsi = cosf(yaw_filt);
+  float sphi = sinf(eulers_filtered.phi);
+  float cphi = cosf(eulers_filtered.phi);
+  float stheta = sinf(eulers_filtered.theta);
+  float ctheta = cosf(eulers_filtered.theta);
+  float spsi = sinf(eulers_filtered.psi);
+  float cpsi = cosf(eulers_filtered.psi);
 
 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
 #define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c

@@ -40,13 +40,15 @@
 
 void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_gih[GUIDANCE_INDI_HYBRID_V]) {
 
+  struct FloatEulers eulers_filtered;
+  float_eulers_of_quat_zxy(&eulers_filtered, &quat_filt.quat);
   /*Pre-calculate sines and cosines*/
-  float sphi = sinf(roll_filt.o[0]);
-  float cphi = cosf(roll_filt.o[0]);
-  float stheta = sinf(pitch_filt.o[0]);
-  float ctheta = cosf(pitch_filt.o[0]);
-  float spsi = sinf(yaw_filt);
-  float cpsi = cosf(yaw_filt);
+  float sphi = sinf(eulers_filtered.phi);
+  float cphi = cosf(eulers_filtered.phi);
+  float stheta = sinf(eulers_filtered.theta);
+  float ctheta = cosf(eulers_filtered.theta);
+  float spsi = sinf(eulers_filtered.psi);
+  float cpsi = cosf(eulers_filtered.psi);
   //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
 
 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
@@ -54,13 +56,13 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
 #endif
 
   /*Amount of lift produced by the wing*/
-  float pitch_lift = pitch_filt.o[0];
+  float pitch_lift = eulers_filtered.theta;
   Bound(pitch_lift,-M_PI_2,0);
   float lift = sinf(pitch_lift)*9.81;
   float T = cosf(pitch_lift)*-9.81;
 
   // get the derivative of the lift wrt to theta
-  float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), pitch_filt.o[0]);
+  float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_filtered.theta);
 
   Gmat[0][0] =  cphi*ctheta*spsi*T + cphi*spsi*lift;
   Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_quadrotor.c

@@ -89,11 +89,15 @@ static void guidance_indi_calcG_yxz(float Gmat[3][3], struct FloatEulers euler_y
  *
  * @param Gmat Dynamics matrix
  */
-void guidance_indi_calcG(float Gmat[3][3], struct FloatEulers att) {
+void guidance_indi_calcG(float Gmat[3][3], const struct FloatQuat *att) {
 #ifdef GUIDANCE_INDI_CALC_G_ZYX
-  guidance_indi_calcG_zyx(Gmat, att);
+  struct FloatEulers eulers_zyx;
+  float_eulers_of_quat_zxy(&eulers_zyx, att);
+  guidance_indi_calcG_zyx(Gmat, eulers_zyx);
 #else
-  guidance_indi_calcG_yxz(Gmat, att); // default case
+  struct FloatEulers eulers_yxz;
+  float_eulers_of_quat_yxz(&eulers_yxz, att);
+  guidance_indi_calcG_yxz(Gmat, eulers_yxz); // default case
 #endif
 }