Fix quaternion interpolation threshold

Author: Daniil Mordanov

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(

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>

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);

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