From eda8ce9051f9cc73ee5d958326f3f16b7cf88897 Mon Sep 17 00:00:00 2001 From: dario Date: Fri, 21 Jun 2024 23:08:23 +0200 Subject: [PATCH] Updated quaternion class, added attitude from tilt, started madgwick filter --- .../sta/math/algorithms/attitude/madgwick.hpp | 48 +++++++++++++++ .../sta/math/algorithms/attitude/slerp.hpp | 23 ------- include/sta/math/algorithms/attitude/tilt.hpp | 22 +++++++ include/sta/math/quaternion.hpp | 9 ++- src/algorithms/attitude/integrate.cpp | 4 +- src/algorithms/attitude/madgwick.cpp | 35 +++++++++++ src/algorithms/attitude/slerp.cpp | 36 ----------- src/algorithms/attitude/tilt.cpp | 42 +++++++++++++ src/quaternion.cpp | 60 ++++++++++++++++--- 9 files changed, 209 insertions(+), 70 deletions(-) create mode 100644 include/sta/math/algorithms/attitude/madgwick.hpp delete mode 100644 include/sta/math/algorithms/attitude/slerp.hpp create mode 100644 include/sta/math/algorithms/attitude/tilt.hpp create mode 100644 src/algorithms/attitude/madgwick.cpp delete mode 100644 src/algorithms/attitude/slerp.cpp create mode 100644 src/algorithms/attitude/tilt.cpp diff --git a/include/sta/math/algorithms/attitude/madgwick.hpp b/include/sta/math/algorithms/attitude/madgwick.hpp new file mode 100644 index 0000000..475a4fa --- /dev/null +++ b/include/sta/math/algorithms/attitude/madgwick.hpp @@ -0,0 +1,48 @@ +/* + * madgwick.hpp + * + * Created on: Jun 20, 2024 + * Author: Dario + */ + +#ifndef STA_MATHS_ATTITUDE_INTEGRATE_HPP +#define STA_MATHS_ATTITUDE_INTEGRATE_HPP + +#include + +namespace sta +{ + namespace math + { + class MadgwickFilter { + public: + /** + * @brief Construct a new Madgwick Filter object + * + * @param n + * @param alpha + */ + MadgwickFilter(Quaternion state, uint32_t n, float alpha); + + /** + * @brief Predicts the next step using rotation rate ingegration. + * + * @param dt The time that passed since the last predict. + * @param ox Rotation rate around the x axis in rad/s. + * @param oy Rotation rate around the y axis in rad/s. + * @param oz Rotation rate around the z axis in rad/s. + */ + void predict(float dt, float ox, float oy, float oz); + + void correct(float dx, float dy, float dz); + private: + Quaternion state_; + uint32_t n_; + float alpha_; + }; + } // namespace math +} // namespace sta + + + +#endif /* STA_MATHS_ATTITUDE_INTEGRATE_HPP */ diff --git a/include/sta/math/algorithms/attitude/slerp.hpp b/include/sta/math/algorithms/attitude/slerp.hpp deleted file mode 100644 index 96b2fe1..0000000 --- a/include/sta/math/algorithms/attitude/slerp.hpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * slerp.hpp - * - * Created on: Jun 19, 2024 - * Author: Dario - */ - -#ifndef STA_MATHS_ATTITUDE_SLERP_HPP -#define STA_MATHS_ATTITUDE_SLERP_HPP - -#include - -namespace sta -{ - namespace math - { - - Quaternion slerp(Quaternion q1, Quaternion q2, float beta); - - } // namespace math -} // namespace sta - -#endif /* STA_MATHS_ATTITUDE_SLERP_HPP*/ diff --git a/include/sta/math/algorithms/attitude/tilt.hpp b/include/sta/math/algorithms/attitude/tilt.hpp new file mode 100644 index 0000000..2cfa970 --- /dev/null +++ b/include/sta/math/algorithms/attitude/tilt.hpp @@ -0,0 +1,22 @@ +/* + * tilt.hpp + * + * Created on: Jun 20, 2024 + * Author: Dario + */ + +#ifndef STA_MATHS_ATTITUDE_TILT_HPP +#define STA_MATHS_ATTITUDE_TILT_HPP + +#include + +namespace sta +{ + namespace math + { + Quaternion getTiltFromAcceleration(float ax, float ay, float az); + + } // namespace math +} // namespace sta + +#endif /* STA_MATHS_ATTITUDE_TILT_HPP */ diff --git a/include/sta/math/quaternion.hpp b/include/sta/math/quaternion.hpp index 181e823..70f4111 100644 --- a/include/sta/math/quaternion.hpp +++ b/include/sta/math/quaternion.hpp @@ -26,11 +26,18 @@ namespace sta Quaternion normalized(); + Quaternion operator*(const Quaternion& quat); Quaternion operator+(const Quaternion& quat); - Quaternion operator*(float scalar); + public: float x, y, z, w; }; + Quaternion operator+(const Quaternion& q1, const Quaternion& q2); + Quaternion operator*(const Quaternion& q1, const Quaternion& q2); + Quaternion operator*(const Quaternion& quat, float scalar); + Quaternion operator*(float scalar, const Quaternion& quat); + Quaternion operator*(const Quaternion& quat, double scalar); + Quaternion operator*(double scalar, const Quaternion& quat); } // namespace math } // namespace sta diff --git a/src/algorithms/attitude/integrate.cpp b/src/algorithms/attitude/integrate.cpp index 20da6e1..127abc0 100644 --- a/src/algorithms/attitude/integrate.cpp +++ b/src/algorithms/attitude/integrate.cpp @@ -6,7 +6,6 @@ */ #include -#include #include #include @@ -37,7 +36,8 @@ namespace sta oy *= (M_PI / 180.0f); oz *= (M_PI / 180.0f); - float norm = sqrt(fmax(ox*ox + oy*oy + oz*oz, 0.000001)); + float norm = sqrt(ox*ox + oy*oy + oz*ox); + float dt2 = dt / 2; float cosDt2 = cos(norm * dt2); float sinDt2 = 1/norm * sin(norm * dt2); diff --git a/src/algorithms/attitude/madgwick.cpp b/src/algorithms/attitude/madgwick.cpp new file mode 100644 index 0000000..a28f4ae --- /dev/null +++ b/src/algorithms/attitude/madgwick.cpp @@ -0,0 +1,35 @@ +/* + * madgwick.cpp + * + * Created on: Jun 20, 2024 + * Author: Dario + */ + +#include + +namespace sta +{ + namespace math + { + MadgwickFilter::MadgwickFilter(Quaternion state, uint32_t n, float alpha) + : state_{state.normalized()}, + n_{n}, + alpha_{alpha} + { + + } + + void MadgwickFilter::predict(float dt, float ox, float oy, float oz) + { + state_ = state_ + 0.5f * (state_ * Quaternion(0, ox, oy, oz)) * dt; + } + + void MadgwickFilter::correct(float dx, float dy, float dz) + { + + } + + + } // namespace math +} // namespace sta + diff --git a/src/algorithms/attitude/slerp.cpp b/src/algorithms/attitude/slerp.cpp deleted file mode 100644 index f900a03..0000000 --- a/src/algorithms/attitude/slerp.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/* - * slerp.cpp - * - * Created on: Jun 19, 2024 - * Author: Dario - */ - -#include -#include - -namespace sta -{ - namespace math - { - Quaternion slerp(Quaternion q1, Quaternion q2, float alpha) - { - float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; - - if (dot < 0.0) - { - q1 = q1 * -1; - dot = -1; - } - - float theta = std::acos(clip(dot, -1, 1)); - - if (theta < 0.00001) - { - return q1; - } - - return (q1 * std::sin((1-alpha) * theta) + q2 * std::sin(alpha * theta)) * (1 / std::sin(theta)); - } - } // namespace math -} // namespace sta - diff --git a/src/algorithms/attitude/tilt.cpp b/src/algorithms/attitude/tilt.cpp new file mode 100644 index 0000000..174023e --- /dev/null +++ b/src/algorithms/attitude/tilt.cpp @@ -0,0 +1,42 @@ +/* + * tilt.cpp + * + * Created on: Jun 20, 2024 + * Author: Dario + */ + +#include +#include + + +namespace sta +{ + namespace math + { + Quaternion getTiltFromAcceleration(float ax, float ay, float az) + { + float norm = std::sqrt(ax*ax + ay*ay + az*az); + + if (norm != 0) + { + ax /= norm; + ay /= norm; + az /= norm; + } + + float theta = std::atan(ay / az); + float phi = std::atan(-ax / std::sqrt(ay*ay + az*az)); + + Quaternion tilt( + std::cos(phi / 2.0f) * std::cos(theta / 2.0f), + std::sin(phi / 2.0f) * std::cos(theta / 2.0f), + std::cos(phi / 2.0f) * std::sin(theta / 2.0f), + - std::sin(phi / 2.0f) * std::sin(theta / 2.0f) + ); + + return tilt; + } + } // namespace math +} // namespace sta + + diff --git a/src/quaternion.cpp b/src/quaternion.cpp index 48f00cc..54c1b16 100644 --- a/src/quaternion.cpp +++ b/src/quaternion.cpp @@ -31,18 +31,27 @@ namespace sta oy *= (M_PI / 180.0f); oz *= (M_PI / 180.0f); - float norm = sqrt(fmax(ox*ox + oy*oy + oz*oz, 0.000001)); + float norm = sqrt(ox*ox + oy*oy + oz*oz); + if (norm == 0) + { + return *this; + } + float dt2 = dt / 2; float cosDt2 = cos(norm * dt2); float sinDt2 = 1/norm * sin(norm * dt2); + // ox /= norm; + // oy /= norm; + // oz /= norm; + float qw = cosDt2*w + -sinDt2*ox*x + -sinDt2*oy*y + -sinDt2*oz*z; float qx = sinDt2*ox*w + cosDt2*x + sinDt2*oz*y + -sinDt2*oy*z; float qy = sinDt2*oy*w + -sinDt2*oz*x + cosDt2*y + sinDt2*ox*z; float qz = sinDt2*oz*w + sinDt2*oy*x + -sinDt2*ox*y + cosDt2*z; - return Quaternion(qw, qx, qy, qz); + return Quaternion(qw, qx, qy, qz).normalized(); } Quaternion Quaternion::unit() @@ -62,15 +71,50 @@ namespace sta return Quaternion(w / n, x / n, y / n, z / n); } - Quaternion Quaternion::operator+(const Quaternion& quat) + Quaternion Quaternion::operator*(const Quaternion& quat) + { + float rw = w * quat.w - x * quat.x + y * quat.y + z * quat.z; + float rx = w * quat.x + x * quat.w + y * quat.z - z * quat.y; + float ry = w * quat.y - x * quat.z + y * quat.w + z * quat.x; + float rz = w * quat.z - x * quat.y - y * quat.x + z * quat.w; + + return Quaternion(rw, rx, ry, rz); + } + + Quaternion operator*(const Quaternion& q1, const Quaternion& q2) + { + float rw = q1.w * q2.w - q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; + float rx = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y; + float ry = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x; + float rz = q1.w * q2.z - q1.x * q2.y - q1.y * q2.x + q1.z * q2.w; + + return Quaternion(rw, rx, ry, rz); + } + + Quaternion operator+(const Quaternion& q1, const Quaternion& q2) { - return Quaternion(w + quat.w, x + quat.x, y + quat.y, z + quat.z); + return Quaternion(q1.w + q2.w, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z); } - Quaternion Quaternion::operator*(float scalar) - { - return Quaternion(w * scalar, x * scalar, y * scalar, z * scalar); - } + Quaternion operator*(const Quaternion& quat, float scalar) + { + return Quaternion(scalar * quat.w, scalar * quat.x, scalar * quat.y, scalar * quat.z); + } + + Quaternion operator*(float scalar, const Quaternion& quat) + { + return quat * scalar; + } + + Quaternion operator*(const Quaternion& quat, double scalar) + { + return Quaternion(scalar * quat.w, scalar * quat.x, scalar * quat.y, scalar * quat.z); + } + + Quaternion operator*(double scalar, const Quaternion& quat) + { + return quat * scalar; + } } // namespace math } // namespace sta