ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-09-29 06:37:33 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Continued Madgwick filter implementation

Browse Source
This commit is contained in:
dario
2024-06-25 23:39:29 +02:00
parent 3a9f2eff21
commit 2a0b244500
4 changed files with 67 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,13 +6,15 @@
*/
#include <sta/math/algorithms/attitude/madgwick.hpp>
#include <sta/math/linalg/matrix.hpp>
namespace sta
{
namespace math
{
MadgwickFilter::MadgwickFilter(Quaternion state, uint32_t n, float alpha)
: state_{state.normalized()},
MadgwickFilter::MadgwickFilter(Quaternion state, Quaternion direction, uint32_t n, float alpha)
: state_{state},
direction_{direction},
n_{n},
alpha_{alpha}
{
@@ -24,9 +26,20 @@ namespace sta
state_ = state_ + 0.5f * (state_ * Quaternion(0, ox, oy, oz)) * dt;
}
Quaternion MadgwickFilter::objective(Quaternion q, Quaternion s)
{
return q.conjugate() * direction_ * q - s;
}
void MadgwickFilter::correct(float dx, float dy, float dz)
{
Quaternion q;
for (uint32_t i = 0; i < n_; i++)
{
}
}