diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
index 91e62e0f91..380ab3a662 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -236,10 +236,12 @@ float thrust_act = 0.f;
 Butterworth2LowPass filt_accel_ned[3];
 Butterworth2LowPass roll_filt;
 Butterworth2LowPass pitch_filt;
-Butterworth2LowPass yaw_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;
 
 struct FloatVect2 desired_airspeed;
 float gi_unbounded_airspeed_sp = 0.f;
@@ -361,7 +363,9 @@ void guidance_indi_init(void)
   }
   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_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;
   init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
   init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
 
@@ -406,7 +410,10 @@ void guidance_indi_enter(void)
 
   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_filt, tau, sample_time, eulers_zxy.psi);
+  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_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
   init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
 
@@ -879,7 +886,15 @@ void guidance_indi_propagate_filters(void)
 
   update_butterworth_2_low_pass(&roll_filt, eulers_zxy.phi);
   update_butterworth_2_low_pass(&pitch_filt, eulers_zxy.theta);
-  update_butterworth_2_low_pass(&yaw_filt, eulers_zxy.psi);
+  
+  // 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);
 
   // Propagate filter for sideslip correction
   float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
index 149b967b42..67ad56315c 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
@@ -112,6 +112,6 @@ extern bool coordinated_turn_use_accel;
 
 extern Butterworth2LowPass roll_filt;
 extern Butterworth2LowPass pitch_filt;
-extern Butterworth2LowPass yaw_filt;
+extern float yaw_filt;
 
 #endif /* GUIDANCE_INDI_HYBRID_H */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
index 64c2abf4f4..3d8216f6b5 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
@@ -89,8 +89,8 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
   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.o[0]);
-  float cpsi = cosf(yaw_filt.o[0]);
+  float spsi = sinf(yaw_filt);
+  float cpsi = cosf(yaw_filt);
 
 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
 #define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
@@ -102,21 +102,22 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
   // get the derivative of the lift wrt to theta
   float liftd = guidance_indi_get_liftd(0.0f, 0.0f);
 
-  Gmat[0][0] = -sphi*stheta*lift_thrust_bz;
-  Gmat[1][0] = -cphi*lift_thrust_bz;
-  Gmat[2][0] = -sphi*ctheta*lift_thrust_bz;
+  // ZXY Euler order
+  Gmat[0][0] =  0.0f;
+  Gmat[1][0] = -cphi*ctheta*lift_thrust_bz;
+  Gmat[2][0] = -ctheta*sphi*lift_thrust_bz;
 
-  Gmat[0][1] =  cphi*ctheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING;
+  Gmat[0][1] =  ctheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING;
   Gmat[1][1] =  sphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*liftd;
   Gmat[2][1] = -cphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
 
-  Gmat[0][2] =  cphi*stheta;
-  Gmat[1][2] = -sphi;
+  Gmat[0][2] =  stheta;
+  Gmat[1][2] = -sphi*ctheta;
   Gmat[2][2] =  cphi*ctheta;
 
   Gmat[0][3] =  ctheta;
-  Gmat[1][3] =  0;
-  Gmat[2][3] = -stheta;
+  Gmat[1][3] =  sphi*stheta;
+  Gmat[2][3] = -cphi*stheta;
   // Make this term zero to prevent switching 'exploits'
   // Gmat[2][3] = 0;
 
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c
index c8cb88b1ba..5966294997 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c
@@ -45,8 +45,8 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
   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.o[0]);
-  float cpsi = cosf(yaw_filt.o[0]);
+  float spsi = sinf(yaw_filt);
+  float cpsi = cosf(yaw_filt);
   //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
 
 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
@@ -102,5 +102,3 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_an
 
 
 #endif
-
-