mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git
synced 2025-06-12 19:05:58 +00:00
49 lines
1.0 KiB
C++
49 lines
1.0 KiB
C++
/*
|
|
* madgwick.hpp
|
|
*
|
|
* Created on: Jun 20, 2024
|
|
* Author: Dario
|
|
*/
|
|
|
|
#ifndef STA_MATHS_ATTITUDE_INTEGRATE_HPP
|
|
#define STA_MATHS_ATTITUDE_INTEGRATE_HPP
|
|
|
|
#include <sta/math/quaternion.hpp>
|
|
|
|
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 */
|