diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 38dfdee169..e134d7fb01 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -797,15 +797,12 @@ static float accelerationLimit(int axis, float currentPidSetpoint)
 
 static void rotateVector(float v[XYZ_AXIS_COUNT], const float rotation[XYZ_AXIS_COUNT])
 {
-    // rotate v around rotation vector rotation
+    // rotate v around yaw component of vector rotation
     // rotation in radians, all elements must be small
-    for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
-        int i_1 = (i + 1) % 3;
-        int i_2 = (i + 2) % 3;
-        float newV = v[i_1] + v[i_2] * rotation[i];
-        v[i_2] -= v[i_1] * rotation[i];
-        v[i_1] = newV;
-    }
+    float pRatio = pidRuntime.pidCoefficient[FD_ROLL].Kp / pidRuntime.pidCoefficient[FD_PITCH].Kp;
+    float newV = v[FD_ROLL] + v[FD_PITCH] * rotation[FD_YAW] * pRatio;
+    v[FD_PITCH] -= v[FD_ROLL] * rotation[FD_YAW] / pRatio;
+    v[FD_ROLL] = newV;
 }
 
 STATIC_UNIT_TESTED void rotateItermAndAxisError(void)
diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc
index 658ac55534..f01157b021 100644
--- a/src/test/unit/pid_unittest.cc
+++ b/src/test/unit/pid_unittest.cc
@@ -917,11 +917,11 @@ TEST(pidControllerTest, testItermRotationHandling)
     pidData[FD_ROLL].I = 10;
     pidData[FD_PITCH].I = 1000;
     pidData[FD_YAW].I = 1000;
-    gyro.gyroADCf[FD_ROLL] = -1000;
+    gyro.gyroADCf[FD_YAW] = -1000;
     rotateItermAndAxisError();
-    EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10);
-    EXPECT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37));
-    EXPECT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6));
+    EXPECT_NEAR(-86.294029235839844, pidData[FD_ROLL].I, calculateTolerance(-86.294029235839844));
+    EXPECT_NEAR(1001.396240234375, pidData[FD_PITCH].I, calculateTolerance(1001.396240234375));
+    EXPECT_FLOAT_EQ(1000, pidData[FD_YAW].I);
 
     pidProfile->abs_control_gain = 10;
     pidInit(pidProfile);
@@ -934,9 +934,9 @@ TEST(pidControllerTest, testItermRotationHandling)
     axisError[FD_PITCH] = 1000;
     axisError[FD_YAW] = 1000;
     rotateItermAndAxisError();
-    EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10);
-    EXPECT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37));
-    EXPECT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6));
+    EXPECT_NEAR(-86.294029235839844, pidData[FD_ROLL].I, calculateTolerance(-86.294029235839844));
+    EXPECT_NEAR(1001.396240234375, pidData[FD_PITCH].I, calculateTolerance(1001.396240234375));
+    EXPECT_FLOAT_EQ(1000, pidData[FD_YAW].I);
 }
 
 TEST(pidControllerTest, testLaunchControl)