Merge pull request #1998 from leocencetti/fix-wrong-covariance-rotation

Fix 3x3 covariance matrix rotation/transformation
mavlink · Oct 10, 2024 · f5e4f1f · f5e4f1f
2 parents ce8610f + 216f1b2
commit f5e4f1f
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 18 deletions.
diff --git a/mavros/src/lib/ftf_frame_conversions.cpp b/mavros/src/lib/ftf_frame_conversions.cpp
@@ -132,7 +132,7 @@ Covariance3d transform_static_frame(const Covariance3d & cov, const StaticTF tra
 
     case StaticTF::AIRCRAFT_TO_BASELINK:
     case StaticTF::BASELINK_TO_AIRCRAFT:
-      cov_out = cov_in * AIRCRAFT_BASELINK_Q;
+      cov_out = AIRCRAFT_BASELINK_Q * cov_in * AIRCRAFT_BASELINK_Q.conjugate();
       return cov_out_;
 
     default:
@@ -279,7 +279,7 @@ Covariance3d transform_frame(const Covariance3d & cov, const Eigen::Quaterniond
   EigenMapConstCovariance3d cov_in(cov.data());
   EigenMapCovariance3d cov_out(cov_out_.data());
 
-  cov_out = cov_in * q;
+  cov_out = q * cov_in * q.conjugate();
   return cov_out_;
 }
 

diff --git a/mavros/test/test_frame_conversions.cpp b/mavros/test/test_frame_conversions.cpp
@@ -12,6 +12,7 @@
  */
 
 #include <gtest/gtest.h>
+#include <cmath>
 
 #include "rclcpp/rclcpp.hpp"
 #include "mavros/frame_tf.hpp"
@@ -154,6 +155,7 @@ TEST(FRAME_TF, transform_static_frame__quaterniond_123)
 
   EXPECT_QUATERNION(expected, out, epsilon);
 }
+#endif
 
 TEST(FRAME_TF, transform_frame__covariance3x3)
 {
@@ -163,24 +165,37 @@ TEST(FRAME_TF, transform_frame__covariance3x3)
     7.0, 8.0, 9.0
   }};
 
+  // Rotation quaternion of PI around x axis
+  const auto q = Eigen::Quaterniond(
+      Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) *
+			Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) *
+			Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())
+  )
+
   /* Calculated as:
-   *         | 1  0  0 |
-   * input * | 0 -1  0 |
-   *         | 0  0 -1 |
+   *
+   * R * input * R.transpose()
+   *
+   * with:
+   *
+   *                            | 1  0  0 |
+   * R = q.toRotationMatrix() = | 0 -1  0 |
+   *                            | 0  0 -1 |
    */
   ftf::Covariance3d expected = {{
-    1.0, -2.0, -3.0,
-    4.0, -5.0, -6.0,
-    7.0, -8.0, -9.0
+     1.0, -2.0, -3.0,
+    -4.0,  5.0,  6.0,
+    -7.0,  8.0,  9.0
   }};
 
-  auto out = ftf::transform_frame(input, enu_sensor);
+  auto out = ftf::transform_frame(input, q);
 
   for (size_t idx = 0; idx < expected.size(); idx++) {
     SCOPED_TRACE(idx);
     EXPECT_NEAR(expected[idx], out[idx], epsilon);
   }
 }
+
 TEST(FRAME_TF, transform_frame__covariance6x6)
 {
   ftf::Covariance6d input = {{
@@ -192,24 +207,42 @@ TEST(FRAME_TF, transform_frame__covariance6x6)
     31.0, 32.0, 33.0, 34.0, 35.0, 36.0
   }};
 
+  // Rotation quaternion of PI around x axis
+  const auto q = Eigen::Quaterniond(
+      Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) *
+			Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) *
+			Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())
+  )
+
+  /* Calculated as:
+   *
+   * R6 * input * R6.transpose()
+   *
+   * with:
+   *
+   *      | R  0 |
+   * R6 = | 0  R |
+   *
+   *                            | 1  0  0 |
+   * R = q.toRotationMatrix() = | 0 -1  0 |
+   *                            | 0  0 -1 |
+   */
   ftf::Covariance6d expected = {{
-    1.0, -2.0, -3.0, 4.0, -5.0, -6.0,
-    7.0, -8.0, -9.0, 10.0, -11.0, -12.0,
-    13.0, -14.0, -15.0, 16.0, -17.0, -18.0,
-    19.0, -20.0, -21.0, 22.0, -23.0, -24.0,
-    25.0, -26.0, -27.0, 28.0, -29.0, -30.0,
-    31.0, -32.0, -33.0, 34.0, -35.0, -36.0
+      1.0,  -2.0,  -3.0,   4.0,  -5.0,  -6.0,
+     -7.0,   8.0,   9.0, -10.0,  11.0,  12.0,
+    -13.0,  14.0,  15.0, -16.0,  17.0,  18.0,
+     19.0, -20.0, -21.0,  22.0, -23.0, -24.0,
+    -25.0,  26.0,  27.0, -28.0,  29.0,  30.0,
+    -31.0,  32.0,  33.0, -34.0,  35.0,  36.0
   }};
 
-  auto out = ftf::transform_frame(input);
+  auto out = ftf::transform_frame(input, q);
 
   for (size_t idx = 0; idx < expected.size(); idx++) {
     SCOPED_TRACE(idx);
     EXPECT_NEAR(expected[idx], out[idx], epsilon);
   }
 }
-#endif
-
 
 int main(int argc, char ** argv)
 {