From 3b11501f3b7a76f67a53410b7b4a6829fe9c711f Mon Sep 17 00:00:00 2001 From: dario Date: Wed, 11 Dec 2024 20:12:29 +0100 Subject: [PATCH] Updated quaternion class, added attitude from tilt, started madgwick filter --- src/algorithms/attitude/madgwick.cpp | 6 ++---- src/quaternion.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/algorithms/attitude/madgwick.cpp b/src/algorithms/attitude/madgwick.cpp index aedb1c6..753b855 100644 --- a/src/algorithms/attitude/madgwick.cpp +++ b/src/algorithms/attitude/madgwick.cpp @@ -6,15 +6,13 @@ */ #include -#include namespace sta { namespace math { - MadgwickFilter::MadgwickFilter(Quaternion state, Quaternion direction, uint32_t n, float alpha) - : state_{state}, - direction_{direction}, + MadgwickFilter::MadgwickFilter(Quaternion state, uint32_t n, float alpha) + : state_{state.normalized()}, n_{n}, alpha_{alpha} { diff --git a/src/quaternion.cpp b/src/quaternion.cpp index 306e3a1..54c1b16 100644 --- a/src/quaternion.cpp +++ b/src/quaternion.cpp @@ -71,14 +71,14 @@ namespace sta return Quaternion(w / n, x / n, y / n, z / n); } - Quaternion Quaternion::conjugate() + Quaternion Quaternion::operator*(const Quaternion& quat) { - return Quaternion(w, -x, -y, -z); - } + 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; - Quaternion operator-(const Quaternion& q1, const Quaternion& q2) - { - return Quaternion(q1.w - q2.w, q1.x - q2.x, q1.y - q2.y, q1.z - q2.z); + return Quaternion(rw, rx, ry, rz); } Quaternion operator*(const Quaternion& q1, const Quaternion& q2)