02/24/2024 Test Launch Version (BB Black) (#11)

* Adding a 90% completed, compilable but untested ADS

* Made basic changes to actuator & sensor. Also added motor class

* Removed unnecessary .cpp files

* Updated sensor & actuator classes, finished ads, added variable time step to kalman filter, set up all tests for future assertions

* Relocated 'main' to 'active-drag-system.cpp'. Added more info to README

* Removed main.cpp

* Added more details to README

* Changed some function parameters from pass-by-pointer to pass-by-reference. Also removed the std namespace

* Started writing the test cases

* Updated the .gitignore file

* Removed some files that should be gitignored

* Up to date with Jazz's pull request

* Test Launch Branch Created; PRU Servo Control with Test Program

* Added I2C device class and register IDs for MPL [INCOMPLETE SENSOR IMPLEMENTATION]

Needs actual data getting function implementation for both sensors and register IDs for BNO, will implement shortly.

* Partial implementation of MPL sensor

Added startup method, still needs fleshed out data getters and setters and finished I2C implementation. MOST LIKELY WILL HAVE COMPILATION ISSUES.

* *Hypothetically* complete MPL implementation

NEEDS HARDWARE TESTING

* IMU Header and init() method implementation

Needs like, all data handling still lol

* Hypothetically functional (Definitely won't compile)

* We ball?

---------

Co-authored-by: Jazz Jackson <[email protected]>
Co-authored-by: Cian Capacci <[email protected]>

4 files changed, 613 insertions, 0 deletions

diff --git a/include/math/imumath.h b/include/math/imumath.h
new file mode 100644
index 0000000..831df60
--- /dev/null
+++ b/include/math/imumath.h
@@ -0,0 +1,30 @@
+//  Inertial Measurement Unit Maths Library
+//
+//  Copyright 2013-2021 Sam Cowen <[email protected]>
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a
+//  copy of this software and associated documentation files (the "Software"),
+//  to deal in the Software without restriction, including without limitation
+//  the rights to use, copy, modify, merge, publish, distribute, sublicense,
+//  and/or sell copies of the Software, and to permit persons to whom the
+//  Software is furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+//  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+//  DEALINGS IN THE SOFTWARE.
+
+#ifndef IMUMATH_H
+#define IMUMATH_H
+
+#include "matrix.h"
+#include "quaternion.h"
+#include "vector.h"
+
+#endif
\ No newline at end of file
diff --git a/include/math/matrix.h b/include/math/matrix.h
new file mode 100644
index 0000000..e71b2db
--- /dev/null
+++ b/include/math/matrix.h
@@ -0,0 +1,185 @@
+//  Inertial Measurement Unit Maths Library
+//
+//  Copyright 2013-2021 Sam Cowen <[email protected]>
+//  Bug fixes and cleanups by Gé Vissers ([email protected])
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a
+//  copy of this software and associated documentation files (the "Software"),
+//  to deal in the Software without restriction, including without limitation
+//  the rights to use, copy, modify, merge, publish, distribute, sublicense,
+//  and/or sell copies of the Software, and to permit persons to whom the
+//  Software is furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+//  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+//  DEALINGS IN THE SOFTWARE.
+
+#ifndef IMUMATH_MATRIX_HPP
+#define IMUMATH_MATRIX_HPP
+
+#include <stdint.h>
+#include <string.h>
+
+#include "vector.h"
+
+namespace imu {
+
+template <uint8_t N> class Matrix {
+public:
+  Matrix() { memset(_cell_data, 0, N * N * sizeof(double)); }
+
+  Matrix(const Matrix &m) {
+    for (int ij = 0; ij < N * N; ++ij) {
+      _cell_data[ij] = m._cell_data[ij];
+    }
+  }
+
+  ~Matrix() {}
+
+  Matrix &operator=(const Matrix &m) {
+    for (int ij = 0; ij < N * N; ++ij) {
+      _cell_data[ij] = m._cell_data[ij];
+    }
+    return *this;
+  }
+
+  Vector<N> row_to_vector(int i) const {
+    Vector<N> ret;
+    for (int j = 0; j < N; j++) {
+      ret[j] = cell(i, j);
+    }
+    return ret;
+  }
+
+  Vector<N> col_to_vector(int j) const {
+    Vector<N> ret;
+    for (int i = 0; i < N; i++) {
+      ret[i] = cell(i, j);
+    }
+    return ret;
+  }
+
+  void vector_to_row(const Vector<N> &v, int i) {
+    for (int j = 0; j < N; j++) {
+      cell(i, j) = v[j];
+    }
+  }
+
+  void vector_to_col(const Vector<N> &v, int j) {
+    for (int i = 0; i < N; i++) {
+      cell(i, j) = v[i];
+    }
+  }
+
+  double operator()(int i, int j) const { return cell(i, j); }
+  double &operator()(int i, int j) { return cell(i, j); }
+
+  double cell(int i, int j) const { return _cell_data[i * N + j]; }
+  double &cell(int i, int j) { return _cell_data[i * N + j]; }
+
+  Matrix operator+(const Matrix &m) const {
+    Matrix ret;
+    for (int ij = 0; ij < N * N; ++ij) {
+      ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij];
+    }
+    return ret;
+  }
+
+  Matrix operator-(const Matrix &m) const {
+    Matrix ret;
+    for (int ij = 0; ij < N * N; ++ij) {
+      ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij];
+    }
+    return ret;
+  }
+
+  Matrix operator*(double scalar) const {
+    Matrix ret;
+    for (int ij = 0; ij < N * N; ++ij) {
+      ret._cell_data[ij] = _cell_data[ij] * scalar;
+    }
+    return ret;
+  }
+
+  Matrix operator*(const Matrix &m) const {
+    Matrix ret;
+    for (int i = 0; i < N; i++) {
+      Vector<N> row = row_to_vector(i);
+      for (int j = 0; j < N; j++) {
+        ret(i, j) = row.dot(m.col_to_vector(j));
+      }
+    }
+    return ret;
+  }
+
+  Matrix transpose() const {
+    Matrix ret;
+    for (int i = 0; i < N; i++) {
+      for (int j = 0; j < N; j++) {
+        ret(j, i) = cell(i, j);
+      }
+    }
+    return ret;
+  }
+
+  Matrix<N - 1> minor_matrix(int row, int col) const {
+    Matrix<N - 1> ret;
+    for (int i = 0, im = 0; i < N; i++) {
+      if (i == row)
+        continue;
+
+      for (int j = 0, jm = 0; j < N; j++) {
+        if (j != col) {
+          ret(im, jm++) = cell(i, j);
+        }
+      }
+      im++;
+    }
+    return ret;
+  }
+
+  double determinant() const {
+    // specialization for N == 1 given below this class
+    double det = 0.0, sign = 1.0;
+    for (int i = 0; i < N; ++i, sign = -sign)
+      det += sign * cell(0, i) * minor_matrix(0, i).determinant();
+    return det;
+  }
+
+  Matrix invert() const {
+    Matrix ret;
+    double det = determinant();
+
+    for (int i = 0; i < N; i++) {
+      for (int j = 0; j < N; j++) {
+        ret(i, j) = minor_matrix(j, i).determinant() / det;
+        if ((i + j) % 2 == 1)
+          ret(i, j) = -ret(i, j);
+      }
+    }
+    return ret;
+  }
+
+  double trace() const {
+    double tr = 0.0;
+    for (int i = 0; i < N; ++i)
+      tr += cell(i, i);
+    return tr;
+  }
+
+private:
+  double _cell_data[N * N];
+};
+
+template <> inline double Matrix<1>::determinant() const { return cell(0, 0); }
+
+}; // namespace imu
+
+#endif
\ No newline at end of file
diff --git a/include/math/quaternion.h b/include/math/quaternion.h
new file mode 100644
index 0000000..c5b907a
--- /dev/null
+++ b/include/math/quaternion.h
@@ -0,0 +1,214 @@
+//  Inertial Measurement Unit Maths Library
+//
+//  Copyright 2013-2021 Sam Cowen <[email protected]>
+//  Bug fixes and cleanups by Gé Vissers ([email protected])
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a
+//  copy of this software and associated documentation files (the "Software"),
+//  to deal in the Software without restriction, including without limitation
+//  the rights to use, copy, modify, merge, publish, distribute, sublicense,
+//  and/or sell copies of the Software, and to permit persons to whom the
+//  Software is furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+//  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+//  DEALINGS IN THE SOFTWARE.
+
+#ifndef IMUMATH_QUATERNION_HPP
+#define IMUMATH_QUATERNION_HPP
+
+#include <math.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "matrix.h"
+
+namespace imu {
+
+class Quaternion {
+public:
+  Quaternion() : _w(1.0), _x(0.0), _y(0.0), _z(0.0) {}
+
+  Quaternion(double w, double x, double y, double z)
+      : _w(w), _x(x), _y(y), _z(z) {}
+
+  Quaternion(double w, Vector<3> vec)
+      : _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {}
+
+  double &w() { return _w; }
+  double &x() { return _x; }
+  double &y() { return _y; }
+  double &z() { return _z; }
+
+  double w() const { return _w; }
+  double x() const { return _x; }
+  double y() const { return _y; }
+  double z() const { return _z; }
+
+  double magnitude() const {
+    return sqrt(_w * _w + _x * _x + _y * _y + _z * _z);
+  }
+
+  void normalize() {
+    double mag = magnitude();
+    *this = this->scale(1 / mag);
+  }
+
+  Quaternion conjugate() const { return Quaternion(_w, -_x, -_y, -_z); }
+
+  void fromAxisAngle(const Vector<3> &axis, double theta) {
+    _w = cos(theta / 2);
+    // only need to calculate sine of half theta once
+    double sht = sin(theta / 2);
+    _x = axis.x() * sht;
+    _y = axis.y() * sht;
+    _z = axis.z() * sht;
+  }
+
+  void fromMatrix(const Matrix<3> &m) {
+    double tr = m.trace();
+
+    double S;
+    if (tr > 0) {
+      S = sqrt(tr + 1.0) * 2;
+      _w = 0.25 * S;
+      _x = (m(2, 1) - m(1, 2)) / S;
+      _y = (m(0, 2) - m(2, 0)) / S;
+      _z = (m(1, 0) - m(0, 1)) / S;
+    } else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) {
+      S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
+      _w = (m(2, 1) - m(1, 2)) / S;
+      _x = 0.25 * S;
+      _y = (m(0, 1) + m(1, 0)) / S;
+      _z = (m(0, 2) + m(2, 0)) / S;
+    } else if (m(1, 1) > m(2, 2)) {
+      S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
+      _w = (m(0, 2) - m(2, 0)) / S;
+      _x = (m(0, 1) + m(1, 0)) / S;
+      _y = 0.25 * S;
+      _z = (m(1, 2) + m(2, 1)) / S;
+    } else {
+      S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
+      _w = (m(1, 0) - m(0, 1)) / S;
+      _x = (m(0, 2) + m(2, 0)) / S;
+      _y = (m(1, 2) + m(2, 1)) / S;
+      _z = 0.25 * S;
+    }
+  }
+
+  void toAxisAngle(Vector<3> &axis, double &angle) const {
+    double sqw = sqrt(1 - _w * _w);
+    if (sqw == 0) // it's a singularity and divide by zero, avoid
+      return;
+
+    angle = 2 * acos(_w);
+    axis.x() = _x / sqw;
+    axis.y() = _y / sqw;
+    axis.z() = _z / sqw;
+  }
+
+  Matrix<3> toMatrix() const {
+    Matrix<3> ret;
+    ret.cell(0, 0) = 1 - 2 * _y * _y - 2 * _z * _z;
+    ret.cell(0, 1) = 2 * _x * _y - 2 * _w * _z;
+    ret.cell(0, 2) = 2 * _x * _z + 2 * _w * _y;
+
+    ret.cell(1, 0) = 2 * _x * _y + 2 * _w * _z;
+    ret.cell(1, 1) = 1 - 2 * _x * _x - 2 * _z * _z;
+    ret.cell(1, 2) = 2 * _y * _z - 2 * _w * _x;
+
+    ret.cell(2, 0) = 2 * _x * _z - 2 * _w * _y;
+    ret.cell(2, 1) = 2 * _y * _z + 2 * _w * _x;
+    ret.cell(2, 2) = 1 - 2 * _x * _x - 2 * _y * _y;
+    return ret;
+  }
+
+  // Returns euler angles that represent the quaternion.  Angles are
+  // returned in rotation order and right-handed about the specified
+  // axes:
+  //
+  //   v[0] is applied 1st about z (ie, roll)
+  //   v[1] is applied 2nd about y (ie, pitch)
+  //   v[2] is applied 3rd about x (ie, yaw)
+  //
+  // Note that this means result.x() is not a rotation about x;
+  // similarly for result.z().
+  //
+  Vector<3> toEuler() const {
+    Vector<3> ret;
+    double sqw = _w * _w;
+    double sqx = _x * _x;
+    double sqy = _y * _y;
+    double sqz = _z * _z;
+
+    ret.x() = atan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw));
+    ret.y() = asin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw));
+    ret.z() = atan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw));
+
+    return ret;
+  }
+
+  Vector<3> toAngularVelocity(double dt) const {
+    Vector<3> ret;
+    Quaternion one(1.0, 0.0, 0.0, 0.0);
+    Quaternion delta = one - *this;
+    Quaternion r = (delta / dt);
+    r = r * 2;
+    r = r * one;
+
+    ret.x() = r.x();
+    ret.y() = r.y();
+    ret.z() = r.z();
+    return ret;
+  }
+
+  Vector<3> rotateVector(const Vector<2> &v) const {
+    return rotateVector(Vector<3>(v.x(), v.y()));
+  }
+
+  Vector<3> rotateVector(const Vector<3> &v) const {
+    Vector<3> qv(_x, _y, _z);
+    Vector<3> t = qv.cross(v) * 2.0;
+    return v + t * _w + qv.cross(t);
+  }
+
+  Quaternion operator*(const Quaternion &q) const {
+    return Quaternion(_w * q._w - _x * q._x - _y * q._y - _z * q._z,
+                      _w * q._x + _x * q._w + _y * q._z - _z * q._y,
+                      _w * q._y - _x * q._z + _y * q._w + _z * q._x,
+                      _w * q._z + _x * q._y - _y * q._x + _z * q._w);
+  }
+
+  Quaternion operator+(const Quaternion &q) const {
+    return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
+  }
+
+  Quaternion operator-(const Quaternion &q) const {
+    return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
+  }
+
+  Quaternion operator/(double scalar) const {
+    return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
+  }
+
+  Quaternion operator*(double scalar) const { return scale(scalar); }
+
+  Quaternion scale(double scalar) const {
+    return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
+  }
+
+private:
+  double _w, _x, _y, _z;
+};
+
+} // namespace imu
+
+#endif
\ No newline at end of file
diff --git a/include/math/vector.h b/include/math/vector.h
new file mode 100644
index 0000000..10345c3
--- /dev/null
+++ b/include/math/vector.h
@@ -0,0 +1,184 @@
+//  Inertial Measurement Unit Maths Library
+//
+//  Copyright 2013-2021 Sam Cowen <[email protected]>
+//  Bug fixes and cleanups by Gé Vissers ([email protected])
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a
+//  copy of this software and associated documentation files (the "Software"),
+//  to deal in the Software without restriction, including without limitation
+//  the rights to use, copy, modify, merge, publish, distribute, sublicense,
+//  and/or sell copies of the Software, and to permit persons to whom the
+//  Software is furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+//  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+//  DEALINGS IN THE SOFTWARE.
+
+#ifndef IMUMATH_VECTOR_HPP
+#define IMUMATH_VECTOR_HPP
+
+#include <math.h>
+#include <stdint.h>
+#include <string.h>
+
+namespace imu {
+
+template <uint8_t N> class Vector {
+public:
+  Vector() { memset(p_vec, 0, sizeof(double) * N); }
+
+  Vector(double a) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+  }
+
+  Vector(double a, double b) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+    p_vec[1] = b;
+  }
+
+  Vector(double a, double b, double c) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+    p_vec[1] = b;
+    p_vec[2] = c;
+  }
+
+  Vector(double a, double b, double c, double d) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+    p_vec[1] = b;
+    p_vec[2] = c;
+    p_vec[3] = d;
+  }
+
+  Vector(const Vector<N> &v) {
+    for (int x = 0; x < N; x++)
+      p_vec[x] = v.p_vec[x];
+  }
+
+  ~Vector() {}
+
+  uint8_t n() { return N; }
+
+  double magnitude() const {
+    double res = 0;
+    for (int i = 0; i < N; i++)
+      res += p_vec[i] * p_vec[i];
+
+    return sqrt(res);
+  }
+
+  void normalize() {
+    double mag = magnitude();
+    if (isnan(mag) || mag == 0.0)
+      return;
+
+    for (int i = 0; i < N; i++)
+      p_vec[i] /= mag;
+  }
+
+  double dot(const Vector &v) const {
+    double ret = 0;
+    for (int i = 0; i < N; i++)
+      ret += p_vec[i] * v.p_vec[i];
+
+    return ret;
+  }
+
+  // The cross product is only valid for vectors with 3 dimensions,
+  // with the exception of higher dimensional stuff that is beyond
+  // the intended scope of this library.
+  // Only a definition for N==3 is given below this class, using
+  // cross() with another value for N will result in a link error.
+  Vector cross(const Vector &v) const;
+
+  Vector scale(double scalar) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] * scalar;
+    return ret;
+  }
+
+  Vector invert() const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = -p_vec[i];
+    return ret;
+  }
+
+  Vector &operator=(const Vector &v) {
+    for (int x = 0; x < N; x++)
+      p_vec[x] = v.p_vec[x];
+    return *this;
+  }
+
+  double &operator[](int n) { return p_vec[n]; }
+
+  double operator[](int n) const { return p_vec[n]; }
+
+  double &operator()(int n) { return p_vec[n]; }
+
+  double operator()(int n) const { return p_vec[n]; }
+
+  Vector operator+(const Vector &v) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] + v.p_vec[i];
+    return ret;
+  }
+
+  Vector operator-(const Vector &v) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] - v.p_vec[i];
+    return ret;
+  }
+
+  Vector operator*(double scalar) const { return scale(scalar); }
+
+  Vector operator/(double scalar) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] / scalar;
+    return ret;
+  }
+
+  void toDegrees() {
+    for (int i = 0; i < N; i++)
+      p_vec[i] *= 57.2957795131; // 180/pi
+  }
+
+  void toRadians() {
+    for (int i = 0; i < N; i++)
+      p_vec[i] *= 0.01745329251; // pi/180
+  }
+
+  double &x() { return p_vec[0]; }
+  double &y() { return p_vec[1]; }
+  double &z() { return p_vec[2]; }
+  double x() const { return p_vec[0]; }
+  double y() const { return p_vec[1]; }
+  double z() const { return p_vec[2]; }
+
+private:
+  double p_vec[N];
+};
+
+template <> inline Vector<3> Vector<3>::cross(const Vector &v) const {
+  return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1],
+                p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2],
+                p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]);
+}
+
+} // namespace imu
+
+#endif
\ No newline at end of file