Split out some quaternion utility functions
This allows them to be used from elsewhere.
Change-Id: I5f691c5e78811916411a19e37dd666cf916b78fd
diff --git a/aos/controls/BUILD b/aos/controls/BUILD
index c45a345..7172cbb 100644
--- a/aos/controls/BUILD
+++ b/aos/controls/BUILD
@@ -87,3 +87,31 @@
"//aos/util:log_interval",
],
)
+
+cc_library(
+ name = "quaternion_utils",
+ srcs = [
+ "quaternion_utils.cc",
+ ],
+ hdrs = [
+ "quaternion_utils.h",
+ ],
+ deps = [
+ "@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "quarternion_utils_test",
+ srcs = [
+ "quaternion_utils_test.cc",
+ ],
+ deps = [
+ ":quaternion_utils",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/aos/controls/quaternion_utils.cc b/aos/controls/quaternion_utils.cc
new file mode 100644
index 0000000..088c699
--- /dev/null
+++ b/aos/controls/quaternion_utils.cc
@@ -0,0 +1,139 @@
+#include "aos/controls/quaternion_utils.h"
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+
+namespace aos {
+namespace controls {
+
+Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
+ const Eigen::Matrix<double, 3, 1> &X, const double max_angle_cap) {
+ const double unclipped_angle = X.norm();
+ const double angle_scalar =
+ (unclipped_angle > max_angle_cap) ? max_angle_cap / unclipped_angle : 1.0;
+ const double angle = unclipped_angle * angle_scalar;
+ const double half_angle = angle * 0.5;
+
+ const double half_angle_squared = half_angle * half_angle;
+
+ // sin(x)/x = 1
+ double sinx_x = 1.0;
+
+ // - x^2/3!
+ double value = half_angle_squared / 6.0;
+ sinx_x -= value;
+
+ // + x^4/5!
+ value = value * half_angle_squared / 20.0;
+ sinx_x += value;
+
+ // - x^6/7!
+ value = value * half_angle_squared / (6.0 * 7.0);
+ sinx_x -= value;
+
+ // + x^8/9!
+ value = value * half_angle_squared / (8.0 * 9.0);
+ sinx_x += value;
+
+ // - x^10/11!
+ value = value * half_angle_squared / (10.0 * 11.0);
+ sinx_x -= value;
+
+ // + x^12/13!
+ value = value * half_angle_squared / (12.0 * 13.0);
+ sinx_x += value;
+
+ // - x^14/15!
+ value = value * half_angle_squared / (14.0 * 15.0);
+ sinx_x -= value;
+
+ // + x^16/17!
+ value = value * half_angle_squared / (16.0 * 17.0);
+ sinx_x += value;
+
+ // To plot the residual in matplotlib, run:
+ // import numpy
+ // import scipy
+ // from matplotlib import pyplot
+ // x = numpy.arange(-numpy.pi, numpy.pi, 0.01)
+ // pyplot.plot(x, 1 - x**2 / scipy.misc.factorial(3) +
+ // x**4 / scipy.misc.factorial(5) -
+ // x**6 / scipy.misc.factorial(7) +
+ // x**8 / scipy.misc.factorial(9) -
+ // x ** 10 / scipy.misc.factorial(11) +
+ // x ** 12 / scipy.misc.factorial(13) -
+ // x ** 14 / scipy.misc.factorial(15) +
+ // x ** 16 / scipy.misc.factorial(17) -
+ // numpy.sin(x) / x)
+
+ const double scalar = sinx_x * 0.5;
+
+ Eigen::Matrix<double, 4, 1> result;
+ result.block<3, 1>(0, 0) = X * scalar * angle_scalar;
+ result(3, 0) = std::cos(half_angle);
+ return result;
+}
+
+inline Eigen::Matrix<double, 4, 1> MaybeFlipX(
+ const Eigen::Matrix<double, 4, 1> &X) {
+ if (X(3, 0) < 0.0) {
+ return -X;
+ } else {
+ return X;
+ }
+}
+
+Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+ const Eigen::Matrix<double, 4, 1> &X) {
+ // TODO(austin): Verify we still need it.
+ const Eigen::Matrix<double, 4, 1> corrected_X = MaybeFlipX(X);
+ const double half_angle =
+ std::atan2(corrected_X.block<3, 1>(0, 0).norm(), corrected_X(3, 0));
+
+ const double half_angle_squared = half_angle * half_angle;
+
+ // TODO(austin): We are doing a division at the end of this. Do the taylor
+ // series expansion of x/sin(x) instead to avoid this.
+
+ // sin(x)/x = 1
+ double sinx_x = 1.0;
+
+ // - x^2/3!
+ double value = half_angle_squared / 6.0;
+ sinx_x -= value;
+
+ // + x^4/5!
+ value = value * half_angle_squared / 20.0;
+ sinx_x += value;
+
+ // - x^6/7!
+ value = value * half_angle_squared / (6.0 * 7.0);
+ sinx_x -= value;
+
+ // + x^8/9!
+ value = value * half_angle_squared / (8.0 * 9.0);
+ sinx_x += value;
+
+ // - x^10/11!
+ value = value * half_angle_squared / (10.0 * 11.0);
+ sinx_x -= value;
+
+ // + x^12/13!
+ value = value * half_angle_squared / (12.0 * 13.0);
+ sinx_x += value;
+
+ // - x^14/15!
+ value = value * half_angle_squared / (14.0 * 15.0);
+ sinx_x -= value;
+
+ // + x^16/17!
+ value = value * half_angle_squared / (16.0 * 17.0);
+ sinx_x += value;
+
+ const double scalar = 2.0 / sinx_x;
+
+ return corrected_X.block<3, 1>(0, 0) * scalar;
+}
+
+} // namespace controls
+} // namespace aos
diff --git a/aos/controls/quaternion_utils.h b/aos/controls/quaternion_utils.h
new file mode 100644
index 0000000..1455821
--- /dev/null
+++ b/aos/controls/quaternion_utils.h
@@ -0,0 +1,71 @@
+#ifndef AOS_CONTROLS_QUATERNION_UTILS_H_
+#define AOS_CONTROLS_QUATERNION_UTILS_H_
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "glog/logging.h"
+
+namespace aos {
+namespace controls {
+
+// Function to compute the quaternion average of a bunch of quaternions. Each
+// column in the input matrix is a quaternion (optionally scaled by it's
+// weight).
+template <int SM>
+inline Eigen::Matrix<double, 4, 1> QuaternionMean(
+ Eigen::Matrix<double, 4, SM> input) {
+ // Algorithm to compute the average of a bunch of quaternions:
+ // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
+
+ Eigen::Matrix<double, 4, 4> m = input * input.transpose();
+
+ Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver;
+ solver.compute(m);
+
+ Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
+ eigenvectors = solver.eigenvectors();
+ Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType eigenvalues =
+ solver.eigenvalues();
+
+ int max_index = 0;
+ double max_eigenvalue = 0.0;
+ for (int i = 0; i < 4; ++i) {
+ const double eigenvalue = std::abs(eigenvalues(i, 0));
+ if (eigenvalue > max_eigenvalue) {
+ max_eigenvalue = eigenvalue;
+ max_index = i;
+ }
+ }
+
+ // Assume that there shouldn't be any imaginary components to the eigenvector.
+ // I can't prove this is true, but everyone else seems to assume it...
+ // TODO(james): Handle this more rigorously.
+ for (int i = 0; i < 4; ++i) {
+ CHECK_LT(eigenvectors(i, max_index).imag(), 1e-4)
+ << eigenvectors(i, max_index);
+ }
+ return eigenvectors.col(max_index).real().normalized();
+}
+
+// Converts from a quaternion to a rotation vector, where the rotation vector's
+// direction represents the axis to rotate around and its magnitude represents
+// the number of radians to rotate.
+Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+ const Eigen::Matrix<double, 4, 1> &X);
+
+inline Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+ const Eigen::Quaternion<double> &X) {
+ return ToRotationVectorFromQuaternion(X.coeffs());
+}
+
+// Converts from a rotation vector to a quaternion. If you supply max_angle_cap,
+// then the rotation vector's magnitude will be clipped to be no more than
+// max_angle_cap before being converted to a quaternion.
+Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
+ const Eigen::Matrix<double, 3, 1> &X,
+ const double max_angle_cap = std::numeric_limits<double>::infinity());
+
+} // namespace controls
+} // namespace aos
+
+#endif // AOS_CONTROLS_QUATERNION_UTILS_H_
diff --git a/aos/controls/quaternion_utils_test.cc b/aos/controls/quaternion_utils_test.cc
new file mode 100644
index 0000000..ec410db
--- /dev/null
+++ b/aos/controls/quaternion_utils_test.cc
@@ -0,0 +1,152 @@
+#include "Eigen/Dense"
+
+#include <random>
+
+#include "aos/controls/quaternion_utils.h"
+#include "aos/testing/random_seed.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace controls {
+namespace testing {
+
+// Tests that small perturbations around a couple quaternions averaged out
+// return the original quaternion.
+TEST(DownEstimatorTest, QuaternionMean) {
+ Eigen::Matrix<double, 4, 7> vectors;
+ vectors.col(0) << 0, 0, 0, 1;
+ for (int i = 0; i < 3; ++i) {
+ Eigen::Matrix<double, 4, 1> perturbation;
+ perturbation.setZero();
+ perturbation(i, 0) = 0.1;
+
+ vectors.col(i * 2 + 1) = vectors.col(0) + perturbation;
+ vectors.col(i * 2 + 2) = vectors.col(0) - perturbation;
+ }
+
+ for (int i = 0; i < 7; ++i) {
+ vectors.col(i).normalize();
+ }
+
+ Eigen::Matrix<double, 4, 1> mean = QuaternionMean(vectors);
+
+ for (int i = 0; i < 4; ++i) {
+ EXPECT_NEAR(mean(i, 0), vectors(i, 0), 0.001) << ": Failed on index " << i;
+ }
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a 0 rotation.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternionAtZero) {
+ Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
+ .coeffs());
+
+ EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::Zero()).norm(), 1e-4);
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a real rotation.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternion) {
+ Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
+ .coeffs());
+
+ EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
+ 1e-4);
+}
+
+// Tests that ToRotationVectorFromQuaternion works for a solution with negative
+// coefficients.
+TEST(DownEstimatorTest, ToRotationVectorFromQuaternionNegative) {
+ Eigen::Matrix<double, 3, 1> vector = ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(
+ -Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
+ .coeffs())
+ .coeffs());
+
+ EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
+ 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector works for a 0 rotation.
+TEST(DownEstimatorTest, ToQuaternionFromRotationVectorAtZero) {
+ Eigen::Matrix<double, 4, 1> quaternion =
+ ToQuaternionFromRotationVector(Eigen::Vector3d::Zero());
+
+ EXPECT_NEAR(
+ 0.0,
+ (quaternion - Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
+ .coeffs())
+ .norm(),
+ 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector works for a real rotation.
+TEST(DownEstimatorTest, ToQuaternionFromRotationVector) {
+ Eigen::Matrix<double, 4, 1> quaternion =
+ ToQuaternionFromRotationVector(Eigen::Vector3d::UnitX() * M_PI * 0.5);
+
+ EXPECT_NEAR(0.0,
+ (quaternion - Eigen::Quaternion<double>(
+ Eigen::AngleAxis<double>(
+ M_PI * 0.5, Eigen::Vector3d::UnitX()))
+ .coeffs())
+
+ .norm(),
+ 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector correctly clips a rotation vector
+// that is too large in magnitude.
+TEST(DownEstimatorTest, ToQuaternionFromLargeRotationVector) {
+ const double kMaxAngle = 2.0;
+ const Eigen::Vector3d rotation_vector =
+ Eigen::Vector3d::UnitX() * kMaxAngle * 2.0;
+ const Eigen::Matrix<double, 3, 1> clipped_vector =
+ ToRotationVectorFromQuaternion(
+ ToQuaternionFromRotationVector(rotation_vector, kMaxAngle));
+
+ EXPECT_NEAR(0.0, (rotation_vector / 2.0 - clipped_vector).norm(), 1e-4);
+}
+
+// Tests that ToQuaternionFromRotationVector and ToRotationVectorFromQuaternion
+// works for random rotations.
+TEST(DownEstimatorTest, RandomQuaternions) {
+ std::mt19937 generator(aos::testing::RandomSeed());
+ std::uniform_real_distribution<double> random_scalar(-1.0, 1.0);
+
+ for (int i = 0; i < 1000; ++i) {
+ Eigen::Matrix<double, 3, 1> axis;
+ axis << random_scalar(generator), random_scalar(generator),
+ random_scalar(generator);
+ EXPECT_GE(axis.norm(), 1e-6);
+ axis.normalize();
+
+ const double angle = random_scalar(generator) * M_PI;
+
+ Eigen::Matrix<double, 4, 1> quaternion =
+ ToQuaternionFromRotationVector(axis * angle);
+
+ Eigen::Quaternion<double> answer(Eigen::AngleAxis<double>(angle, axis));
+
+ EXPECT_NEAR(quaternion(3, 0), std::cos(angle / 2.0), 1e-8);
+ EXPECT_NEAR(answer.w(), std::cos(angle / 2.0), 1e-8);
+
+ EXPECT_NEAR(1.0, (answer.coeffs() * quaternion.transpose()).norm(), 1e-6);
+
+ const Eigen::Matrix<double, 3, 1> recalculated_axis =
+ ToRotationVectorFromQuaternion(quaternion);
+
+ EXPECT_NEAR(std::abs(angle), recalculated_axis.norm(), 1e-8);
+
+ EXPECT_NEAR(0.0, (axis * angle - recalculated_axis).norm(), 1e-8);
+ }
+}
+
+} // namespace testing
+} // namespace controls
+} // namespace aos
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index ac2a2e8..ffa2c11 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -702,6 +702,7 @@
deps = [
":drivetrain_config",
":drivetrain_status_fbs",
+ "//aos/controls:quaternion_utils",
"//aos/events:event_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:runge_kutta",
@@ -718,6 +719,7 @@
],
deps = [
":drivetrain_test_lib",
+ "//aos/controls:quaternion_utils",
"//aos/testing:googletest",
"//aos/testing:random_seed",
"//frc971/control_loops/drivetrain:improved_down_estimator",
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index cdb1587..665bf32 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -3,139 +3,12 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
+#include "aos/controls/quaternion_utils.h"
+
namespace frc971 {
namespace control_loops {
namespace drivetrain {
-Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
- const Eigen::Matrix<double, 3, 1> &X, const double max_angle_cap) {
- const double unclipped_angle = X.norm();
- const double angle_scalar =
- (unclipped_angle > max_angle_cap) ? max_angle_cap / unclipped_angle : 1.0;
- const double angle = unclipped_angle * angle_scalar;
- const double half_angle = angle * 0.5;
-
- const double half_angle_squared = half_angle * half_angle;
-
- // sin(x)/x = 1
- double sinx_x = 1.0;
-
- // - x^2/3!
- double value = half_angle_squared / 6.0;
- sinx_x -= value;
-
- // + x^4/5!
- value = value * half_angle_squared / 20.0;
- sinx_x += value;
-
- // - x^6/7!
- value = value * half_angle_squared / (6.0 * 7.0);
- sinx_x -= value;
-
- // + x^8/9!
- value = value * half_angle_squared / (8.0 * 9.0);
- sinx_x += value;
-
- // - x^10/11!
- value = value * half_angle_squared / (10.0 * 11.0);
- sinx_x -= value;
-
- // + x^12/13!
- value = value * half_angle_squared / (12.0 * 13.0);
- sinx_x += value;
-
- // - x^14/15!
- value = value * half_angle_squared / (14.0 * 15.0);
- sinx_x -= value;
-
- // + x^16/17!
- value = value * half_angle_squared / (16.0 * 17.0);
- sinx_x += value;
-
- // To plot the residual in matplotlib, run:
- // import numpy
- // import scipy
- // from matplotlib import pyplot
- // x = numpy.arange(-numpy.pi, numpy.pi, 0.01)
- // pyplot.plot(x, 1 - x**2 / scipy.misc.factorial(3) +
- // x**4 / scipy.misc.factorial(5) -
- // x**6 / scipy.misc.factorial(7) +
- // x**8 / scipy.misc.factorial(9) -
- // x ** 10 / scipy.misc.factorial(11) +
- // x ** 12 / scipy.misc.factorial(13) -
- // x ** 14 / scipy.misc.factorial(15) +
- // x ** 16 / scipy.misc.factorial(17) -
- // numpy.sin(x) / x)
-
- const double scalar = sinx_x * 0.5;
-
- Eigen::Matrix<double, 4, 1> result;
- result.block<3, 1>(0, 0) = X * scalar * angle_scalar;
- result(3, 0) = std::cos(half_angle);
- return result;
-}
-
-inline Eigen::Matrix<double, 4, 1> MaybeFlipX(
- const Eigen::Matrix<double, 4, 1> &X) {
- if (X(3, 0) < 0.0) {
- return -X;
- } else {
- return X;
- }
-}
-
-Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
- const Eigen::Matrix<double, 4, 1> &X) {
- // TODO(austin): Verify we still need it.
- const Eigen::Matrix<double, 4, 1> corrected_X = MaybeFlipX(X);
- const double half_angle =
- std::atan2(corrected_X.block<3, 1>(0, 0).norm(), corrected_X(3, 0));
-
- const double half_angle_squared = half_angle * half_angle;
-
- // TODO(austin): We are doing a division at the end of this. Do the taylor
- // series expansion of x/sin(x) instead to avoid this.
-
- // sin(x)/x = 1
- double sinx_x = 1.0;
-
- // - x^2/3!
- double value = half_angle_squared / 6.0;
- sinx_x -= value;
-
- // + x^4/5!
- value = value * half_angle_squared / 20.0;
- sinx_x += value;
-
- // - x^6/7!
- value = value * half_angle_squared / (6.0 * 7.0);
- sinx_x -= value;
-
- // + x^8/9!
- value = value * half_angle_squared / (8.0 * 9.0);
- sinx_x += value;
-
- // - x^10/11!
- value = value * half_angle_squared / (10.0 * 11.0);
- sinx_x -= value;
-
- // + x^12/13!
- value = value * half_angle_squared / (12.0 * 13.0);
- sinx_x += value;
-
- // - x^14/15!
- value = value * half_angle_squared / (14.0 * 15.0);
- sinx_x -= value;
-
- // + x^16/17!
- value = value * half_angle_squared / (16.0 * 17.0);
- sinx_x += value;
-
- const double scalar = 2.0 / sinx_x;
-
- return corrected_X.block<3, 1>(0, 0) * scalar;
-}
-
void QuaternionUkf::Predict(const Eigen::Matrix<double, 3, 1> &U,
const Eigen::Matrix<double, 3, 1> &measurement,
const aos::monotonic_clock::duration dt) {
@@ -183,7 +56,7 @@
// We now have the sigma points after the model update.
// Compute the mean of the transformed sigma point
- X_hat_ = Eigen::Quaternion<double>(QuaternionMean(Y));
+ X_hat_ = Eigen::Quaternion<double>(aos::controls::QuaternionMean(Y));
// And the covariance.
Eigen::Matrix<double, 3, 2 * 3 + 1> Wprime;
@@ -262,8 +135,9 @@
P_xz * P_vv.inverse();
// Update X_hat and the covariance P
- X_hat_ = X_hat_ * Eigen::Quaternion<double>(ToQuaternionFromRotationVector(
- K * (measurement - Z_hat_)));
+ X_hat_ = X_hat_ * Eigen::Quaternion<double>(
+ aos::controls::ToQuaternionFromRotationVector(
+ K * (measurement - Z_hat_)));
P_ = P_prior - K * P_vv * K.transpose();
}
@@ -317,9 +191,10 @@
for (int i = 0; i < 7; ++i) {
// Compute the error vector for each sigma point.
- Eigen::Matrix<double, 3, 1> Wprimei = ToRotationVectorFromQuaternion(
- Eigen::Quaternion<double>(mean).conjugate() *
- Eigen::Quaternion<double>(points.col(i)));
+ Eigen::Matrix<double, 3, 1> Wprimei =
+ aos::controls::ToRotationVectorFromQuaternion(
+ Eigen::Quaternion<double>(mean).conjugate() *
+ Eigen::Quaternion<double>(points.col(i)));
// Now, compute the contribution of this sigma point to P_prior.
P_prior += 1.0 / 6.0 * (Wprimei * Wprimei.transpose());
// Save the error for the cross-correlation matrix.
@@ -343,7 +218,7 @@
Eigen::Matrix<double, 4, 3 * 2 + 1> X;
for (int i = 0; i < 3; ++i) {
Eigen::Quaternion<double> perturbation(
- ToQuaternionFromRotationVector(S.col(i), M_PI_2));
+ aos::controls::ToQuaternionFromRotationVector(S.col(i), M_PI_2));
X.col(i * 2) = (mean * perturbation).coeffs();
X.col(i * 2 + 1) = (mean * perturbation.conjugate()).coeffs();
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index c1c8e1b..457bbef 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -15,63 +15,6 @@
namespace control_loops {
namespace drivetrain {
-// Function to compute the quaternion average of a bunch of quaternions. Each
-// column in the input matrix is a quaternion (optionally scaled by it's
-// weight).
-template <int SM>
-Eigen::Matrix<double, 4, 1> QuaternionMean(
- Eigen::Matrix<double, 4, SM> input) {
- // Algorithm to compute the average of a bunch of quaternions:
- // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
-
- Eigen::Matrix<double, 4, 4> m = input * input.transpose();
-
- Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver;
- solver.compute(m);
-
- Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
- eigenvectors = solver.eigenvectors();
- Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType eigenvalues =
- solver.eigenvalues();
-
- int max_index = 0;
- double max_eigenvalue = 0.0;
- for (int i = 0; i < 4; ++i) {
- const double eigenvalue = std::abs(eigenvalues(i, 0));
- if (eigenvalue > max_eigenvalue) {
- max_eigenvalue = eigenvalue;
- max_index = i;
- }
- }
-
- // Assume that there shouldn't be any imaginary components to the eigenvector.
- // I can't prove this is true, but everyone else seems to assume it...
- // TODO(james): Handle this more rigorously.
- for (int i = 0; i < 4; ++i) {
- CHECK_LT(eigenvectors(i, max_index).imag(), 1e-4)
- << eigenvectors(i, max_index);
- }
- return eigenvectors.col(max_index).real().normalized();
-}
-
-// Converts from a quaternion to a rotation vector, where the rotation vector's
-// direction represents the axis to rotate around and its magnitude represents
-// the number of radians to rotate.
-Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
- const Eigen::Matrix<double, 4, 1> &X);
-
-inline Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
- const Eigen::Quaternion<double> &X) {
- return ToRotationVectorFromQuaternion(X.coeffs());
-};
-
-// Converts from a rotation vector to a quaternion. If you supply max_angle_cap,
-// then the rotation vector's magnitude will be clipped to be no more than
-// max_angle_cap before being converted to a quaternion.
-Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
- const Eigen::Matrix<double, 3, 1> &X,
- const double max_angle_cap = std::numeric_limits<double>::infinity());
-
// Generates the sigma points to use in the UKF given the current estimate and
// covariance.
Eigen::Matrix<double, 4, 3 * 2 + 1> GenerateSigmaPoints(
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 2edce5a..c4643d9 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -1,11 +1,12 @@
-#include "frc971/control_loops/runge_kutta.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include <Eigen/Geometry>
#include <random>
+#include "aos/controls/quaternion_utils.h"
#include "aos/testing/random_seed.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/runge_kutta.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
@@ -220,31 +221,6 @@
<< dtukf.H(dtukf.X_hat().coeffs());
}
-// Tests that small perturbations around a couple quaternions averaged out
-// return the original quaternion.
-TEST(DownEstimatorTest, QuaternionMean) {
- Eigen::Matrix<double, 4, 7> vectors;
- vectors.col(0) << 0, 0, 0, 1;
- for (int i = 0; i < 3; ++i) {
- Eigen::Matrix<double, 4, 1> perturbation;
- perturbation.setZero();
- perturbation(i, 0) = 0.1;
-
- vectors.col(i * 2 + 1) = vectors.col(0) + perturbation;
- vectors.col(i * 2 + 2) = vectors.col(0) - perturbation;
- }
-
- for (int i = 0; i < 7; ++i) {
- vectors.col(i).normalize();
- }
-
- Eigen::Matrix<double, 4, 1> mean = drivetrain::QuaternionMean(vectors);
-
- for (int i = 0; i < 4; ++i) {
- EXPECT_NEAR(mean(i, 0), vectors(i, 0), 0.001) << ": Failed on index " << i;
- }
-}
-
// Tests that computing sigma points, and then computing the mean and covariance
// returns the original answer.
TEST(DownEstimatorTest, SigmaPoints) {
@@ -259,7 +235,7 @@
drivetrain::GenerateSigmaPoints(mean, covariance);
const Eigen::Matrix<double, 4, 1> calculated_mean =
- drivetrain::QuaternionMean(vectors);
+ aos::controls::QuaternionMean(vectors);
VLOG(1) << "actual mean: " << mean.coeffs();
VLOG(1) << "calculated mean: " << calculated_mean;
@@ -290,7 +266,7 @@
drivetrain::GenerateSigmaPoints(mean, covariance);
const Eigen::Matrix<double, 4, 1> calculated_mean =
- drivetrain::QuaternionMean(vectors);
+ aos::controls::QuaternionMean(vectors);
Eigen::Matrix<double, 3, 3 * 2 + 1> Wprime;
Eigen::Matrix<double, 3, 3> calculated_covariance =
@@ -308,120 +284,6 @@
"covariance.";
}
-// Tests that ToRotationVectorFromQuaternion works for a 0 rotation.
-TEST(DownEstimatorTest, ToRotationVectorFromQuaternionAtZero) {
- Eigen::Matrix<double, 3, 1> vector =
- drivetrain::ToRotationVectorFromQuaternion(
- Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
- .coeffs());
-
- EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::Zero()).norm(), 1e-4);
-}
-
-// Tests that ToRotationVectorFromQuaternion works for a real rotation.
-TEST(DownEstimatorTest, ToRotationVectorFromQuaternion) {
- Eigen::Matrix<double, 3, 1> vector =
- drivetrain::ToRotationVectorFromQuaternion(
- Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
- .coeffs());
-
- EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
- 1e-4);
-}
-
-// Tests that ToRotationVectorFromQuaternion works for a solution with negative
-// coefficients.
-TEST(DownEstimatorTest, ToRotationVectorFromQuaternionNegative) {
- Eigen::Matrix<double, 3, 1> vector =
- drivetrain::ToRotationVectorFromQuaternion(
- Eigen::Quaternion<double>(
- -Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(M_PI * 0.5,
- Eigen::Vector3d::UnitX()))
- .coeffs())
- .coeffs());
-
- EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
- 1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector works for a 0 rotation.
-TEST(DownEstimatorTest, ToQuaternionFromRotationVectorAtZero) {
- Eigen::Matrix<double, 4, 1> quaternion =
- drivetrain::ToQuaternionFromRotationVector(Eigen::Vector3d::Zero());
-
- EXPECT_NEAR(0.0, (quaternion -
- Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
- .coeffs()).norm(),
- 1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector works for a real rotation.
-TEST(DownEstimatorTest, ToQuaternionFromRotationVector) {
- Eigen::Matrix<double, 4, 1> quaternion =
- drivetrain::ToQuaternionFromRotationVector(Eigen::Vector3d::UnitX() *
- M_PI * 0.5);
-
- EXPECT_NEAR(0.0, (quaternion -
- Eigen::Quaternion<double>(
- Eigen::AngleAxis<double>(
- M_PI * 0.5, Eigen::Vector3d::UnitX())).coeffs())
-
- .norm(),
- 1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector correctly clips a rotation vector
-// that is too large in magnitude.
-TEST(DownEstimatorTest, ToQuaternionFromLargeRotationVector) {
- const double kMaxAngle = 2.0;
- const Eigen::Vector3d rotation_vector =
- Eigen::Vector3d::UnitX() * kMaxAngle * 2.0;
- const Eigen::Matrix<double, 3, 1> clipped_vector =
- drivetrain::ToRotationVectorFromQuaternion(
- drivetrain::ToQuaternionFromRotationVector(rotation_vector,
- kMaxAngle));
-
- EXPECT_NEAR(0.0, (rotation_vector / 2.0 - clipped_vector).norm(), 1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector and ToRotationVectorFromQuaternion
-// works for random rotations.
-TEST(DownEstimatorTest, RandomQuaternions) {
- std::mt19937 generator(aos::testing::RandomSeed());
- std::uniform_real_distribution<double> random_scalar(-1.0, 1.0);
-
- for (int i = 0; i < 1000; ++i) {
- Eigen::Matrix<double, 3, 1> axis;
- axis << random_scalar(generator), random_scalar(generator),
- random_scalar(generator);
- EXPECT_GE(axis.norm(), 1e-6);
- axis.normalize();
-
- const double angle = random_scalar(generator) * M_PI;
-
- Eigen::Matrix<double, 4, 1> quaternion =
- drivetrain::ToQuaternionFromRotationVector(axis * angle);
-
- Eigen::Quaternion<double> answer(Eigen::AngleAxis<double>(angle, axis));
-
- EXPECT_NEAR(quaternion(3, 0), std::cos(angle / 2.0), 1e-8);
- EXPECT_NEAR(answer.w(), std::cos(angle / 2.0), 1e-8);
-
- EXPECT_NEAR(1.0, (answer.coeffs() * quaternion.transpose()).norm(), 1e-6);
-
- const Eigen::Matrix<double, 3, 1> recalculated_axis =
- drivetrain::ToRotationVectorFromQuaternion(quaternion);
-
- EXPECT_NEAR(std::abs(angle), recalculated_axis.norm(), 1e-8);
-
- EXPECT_NEAR(0.0, (axis * angle - recalculated_axis).norm(), 1e-8);
- }
-}
-
} // namespace testing
} // namespace control_loops
} // namespace frc971