ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-06-11 18:35:59 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Updated quaternion class, added attitude from tilt, started madgwick filter

Browse Source
This commit is contained in:
dario 2024-12-11 20:12:29 +01:00
parent 47ca310551
commit 3b11501f3b
2 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/algorithms/attitude/madgwick.cpp
View File

@ -6,15 +6,13 @@
*/
#include <sta/math/algorithms/attitude/madgwick.hpp>
#include <sta/math/linalg/matrix.hpp>
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}
{

12
src/quaternion.cpp
View File

@ -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)