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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
include/sta/math/algorithms/attitude/madgwick.hpp
View File

@ -10,6 +10,8 @@
#include <sta/math/quaternion.hpp>
#include <cstdint>
namespace sta
{
namespace math
@ -22,7 +24,7 @@ namespace sta
* @param n
* @param alpha
*/
MadgwickFilter(Quaternion state, uint32_t n, float alpha);
MadgwickFilter(Quaternion state, Quaternion direction, uint32_t n, float alpha);
/**
* @brief Predicts the next step using rotation rate ingegration.
@ -36,7 +38,10 @@ namespace sta
void correct(float dx, float dy, float dz);
private:
Quaternion objective(Quaternion q, Quaternion s);
Quaternion state_;
Quaternion direction_;
uint32_t n_;
float alpha_;
};

42
include/sta/math/quaternion.hpp
View File

@ -14,24 +14,60 @@ namespace sta
class Quaternion
{
public:
/**
* @brief Construct a new Quaternion object
*
* @param w
* @param x
* @param y
* @param z
*/
Quaternion(float w, float x, float y, float z);
/**
* @brief Construct a new Quaternion object
*
*/
Quaternion();
/**
* @brief
*
* @param dt
* @param ox
* @param oy
* @param oz
* @return Quaternion
*/
Quaternion integrate(float dt, float ox, float oy, float oz);
/**
* @brief
*
* @return Quaternion
*/
static Quaternion unit();
/**
* @brief
*
* @return float
*/
float norm();
/**
* @brief
*
* @return Quaternion
*/
Quaternion normalized();
Quaternion operator*(const Quaternion& quat);
Quaternion operator+(const Quaternion& quat);
Quaternion conjugate();
public:
float x, y, z, w;
};
Quaternion operator-(const Quaternion& q1, const Quaternion& q2);
Quaternion operator+(const Quaternion& q1, const Quaternion& q2);
Quaternion operator*(const Quaternion& q1, const Quaternion& q2);
Quaternion operator*(const Quaternion& quat, float scalar);

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++)
{
}
}

12
src/quaternion.cpp
View File

@ -71,14 +71,14 @@ namespace sta
return Quaternion(w / n, x / n, y / n, z / n);
}
Quaternion Quaternion::operator*(const Quaternion& quat)
Quaternion Quaternion::conjugate()
{
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;
return Quaternion(w, -x, -y, -z);
}
return Quaternion(rw, rx, ry, rz);
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);
}
Quaternion operator*(const Quaternion& q1, const Quaternion& q2)