mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git
synced 2025-06-12 19:05:58 +00:00
36 lines
583 B
C++
36 lines
583 B
C++
/*
|
|
* madgwick.cpp
|
|
*
|
|
* Created on: Jun 20, 2024
|
|
* Author: Dario
|
|
*/
|
|
|
|
#include <sta/math/algorithms/attitude/madgwick.hpp>
|
|
|
|
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
|
|
|