From de4b389f43044e5c13a2cccc5b73635fc81a820a Mon Sep 17 00:00:00 2001
From: Daniil Mordanov <153565951+Daniiiil1@users.noreply.github.com>
Date: Sun, 9 Aug 2026 20:18:19 +0700
Subject: [PATCH] Fix quaternion interpolation threshold

---
 imu_complementary_filter/CMakeLists.txt       |  7 +++
 imu_complementary_filter/package.xml          |  1 +
 .../src/complementary_filter.cpp              |  3 +-
 .../test/complementary_filter_test.cpp        | 54 +++++++++++++++++++
 4 files changed, 64 insertions(+), 1 deletion(-)
 create mode 100644 imu_complementary_filter/test/complementary_filter_test.cpp

diff --git a/imu_complementary_filter/CMakeLists.txt b/imu_complementary_filter/CMakeLists.txt
index 83620724..19d1e65f 100644
--- a/imu_complementary_filter/CMakeLists.txt
+++ b/imu_complementary_filter/CMakeLists.txt
@@ -71,6 +71,13 @@ install(DIRECTORY launch config
   DESTINATION share/${PROJECT_NAME}
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(complementary_filter_test
+    test/complementary_filter_test.cpp)
+  target_link_libraries(complementary_filter_test complementary_filter)
+endif()
+
 ament_export_include_directories(include)
 ament_export_libraries(complementary_filter)
 ament_export_targets(
diff --git a/imu_complementary_filter/package.xml b/imu_complementary_filter/package.xml
index faf71180..d1f9c183 100644
--- a/imu_complementary_filter/package.xml
+++ b/imu_complementary_filter/package.xml
@@ -18,6 +18,7 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>tf2</build_depend>
   <build_depend>tf2_ros</build_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/imu_complementary_filter/src/complementary_filter.cpp b/imu_complementary_filter/src/complementary_filter.cpp
index e44525ea..3c4a6d81 100644
--- a/imu_complementary_filter/src/complementary_filter.cpp
+++ b/imu_complementary_filter/src/complementary_filter.cpp
@@ -496,7 +496,8 @@ void invertQuaternion(double q0, double q1, double q2, double q3,
 void scaleQuaternion(double gain, double& dq0, double& dq1, double& dq2,
                      double& dq3)
 {
-    if (dq0 < 0.0)  // 0.9
+    constexpr double interpolation_threshold = 0.9;
+    if (dq0 <= interpolation_threshold)
     {
         // Slerp (Spherical linear interpolation):
         double angle = acos(dq0);
diff --git a/imu_complementary_filter/test/complementary_filter_test.cpp b/imu_complementary_filter/test/complementary_filter_test.cpp
new file mode 100644
index 00000000..fc54f44e
--- /dev/null
+++ b/imu_complementary_filter/test/complementary_filter_test.cpp
@@ -0,0 +1,54 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "imu_complementary_filter/complementary_filter.h"
+
+namespace {
+
+constexpr double kTolerance = 1e-12;
+constexpr double kGain = 0.25;
+
+TEST(ScaleQuaternion, UsesSlerpForLargeCorrections)
+{
+    double q0 = 0.8;
+    double q1 = 0.6;
+    double q2 = 0.0;
+    double q3 = 0.0;
+
+    const double angle = std::acos(q0);
+    const double expected_q0 = std::cos(kGain * angle);
+    const double expected_q1 = std::sin(kGain * angle);
+
+    imu_tools::scaleQuaternion(kGain, q0, q1, q2, q3);
+
+    EXPECT_NEAR(q0, expected_q0, kTolerance);
+    EXPECT_NEAR(q1, expected_q1, kTolerance);
+    EXPECT_DOUBLE_EQ(q2, 0.0);
+    EXPECT_DOUBLE_EQ(q3, 0.0);
+}
+
+TEST(ScaleQuaternion, UsesNormalizedLerpForSmallCorrections)
+{
+    double q0 = 0.95;
+    double q1 = std::sqrt(1.0 - q0 * q0);
+    double q2 = 0.0;
+    double q3 = 0.0;
+
+    const double lerp_q0 = (1.0 - kGain) + kGain * q0;
+    const double lerp_q1 = kGain * q1;
+    const double norm = std::hypot(lerp_q0, lerp_q1);
+
+    imu_tools::scaleQuaternion(kGain, q0, q1, q2, q3);
+
+    EXPECT_NEAR(q0, lerp_q0 / norm, kTolerance);
+    EXPECT_NEAR(q1, lerp_q1 / norm, kTolerance);
+    EXPECT_DOUBLE_EQ(q2, 0.0);
+    EXPECT_DOUBLE_EQ(q3, 0.0);
+}
+
+}  // namespace