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-06-21 23:08:23 +02:00
parent 92e3d32abc
commit eda8ce9051
9 changed files with 209 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
include/sta/math/algorithms/attitude/madgwick.hpp Normal file
View File

@ -0,0 +1,48 @@
/*
* 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 */

23
include/sta/math/algorithms/attitude/slerp.hpp
View File

@ -1,23 +0,0 @@
/*
* slerp.hpp
*
* Created on: Jun 19, 2024
* Author: Dario
*/
#ifndef STA_MATHS_ATTITUDE_SLERP_HPP
#define STA_MATHS_ATTITUDE_SLERP_HPP
#include <sta/math/quaternion.hpp>
namespace sta
{
namespace math
{
Quaternion slerp(Quaternion q1, Quaternion q2, float beta);
} // namespace math
} // namespace sta
#endif /* STA_MATHS_ATTITUDE_SLERP_HPP*/

22
include/sta/math/algorithms/attitude/tilt.hpp Normal file
View File

@ -0,0 +1,22 @@
/*
* tilt.hpp
*
* Created on: Jun 20, 2024
* Author: Dario
*/
#ifndef STA_MATHS_ATTITUDE_TILT_HPP
#define STA_MATHS_ATTITUDE_TILT_HPP
#include <sta/math/quaternion.hpp>
namespace sta
{
namespace math
{
Quaternion getTiltFromAcceleration(float ax, float ay, float az);
} // namespace math
} // namespace sta
#endif /* STA_MATHS_ATTITUDE_TILT_HPP */

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

@ -26,11 +26,18 @@ namespace sta
Quaternion normalized();
Quaternion operator*(const Quaternion& quat);
Quaternion operator+(const Quaternion& quat);
Quaternion operator*(float scalar);
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& quat, float scalar);
Quaternion operator*(float scalar, const Quaternion& quat);
Quaternion operator*(const Quaternion& quat, double scalar);
Quaternion operator*(double scalar, const Quaternion& quat);
} // namespace math
} // namespace sta

4
src/algorithms/attitude/integrate.cpp
View File

@ -6,7 +6,6 @@
*/
#include <sta/math/algorithms/attitude/integrate.hpp>
#include <sta/math/algorithms/attitude/slerp.hpp>
#include <sta/math/linalg/matrix.hpp>
#include <math.h>
@ -37,7 +36,8 @@ namespace sta
oy *= (M_PI / 180.0f);
oz *= (M_PI / 180.0f);
float norm = sqrt(fmax(ox*ox + oy*oy + oz*oz, 0.000001));
float norm = sqrt(ox*ox + oy*oy + oz*ox);
float dt2 = dt / 2;
float cosDt2 = cos(norm * dt2);
float sinDt2 = 1/norm * sin(norm * dt2);

35
src/algorithms/attitude/madgwick.cpp Normal file
View File

@ -0,0 +1,35 @@
/*
* 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

36
src/algorithms/attitude/slerp.cpp
View File

@ -1,36 +0,0 @@
/*
* slerp.cpp
*
* Created on: Jun 19, 2024
* Author: Dario
*/
#include <sta/math/algorithms/attitude/slerp.hpp>
#include <sta/math/utils.hpp>
namespace sta
{
namespace math
{
Quaternion slerp(Quaternion q1, Quaternion q2, float alpha)
{
float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
if (dot < 0.0)
{
q1 = q1 * -1;
dot = -1;
}
float theta = std::acos(clip(dot, -1, 1));
if (theta < 0.00001)
{
return q1;
}
return (q1 * std::sin((1-alpha) * theta) + q2 * std::sin(alpha * theta)) * (1 / std::sin(theta));
}
} // namespace math
} // namespace sta

42
src/algorithms/attitude/tilt.cpp Normal file
View File

@ -0,0 +1,42 @@
/*
* tilt.cpp
*
* Created on: Jun 20, 2024
* Author: Dario
*/
#include <sta/math/algorithms/attitude/tilt.hpp>
#include <sta/math/utils.hpp>
namespace sta
{
namespace math
{
Quaternion getTiltFromAcceleration(float ax, float ay, float az)
{
float norm = std::sqrt(ax*ax + ay*ay + az*az);
if (norm != 0)
{
ax /= norm;
ay /= norm;
az /= norm;
}
float theta = std::atan(ay / az);
float phi = std::atan(-ax / std::sqrt(ay*ay + az*az));
Quaternion tilt(
std::cos(phi / 2.0f) * std::cos(theta / 2.0f),
std::sin(phi / 2.0f) * std::cos(theta / 2.0f),
std::cos(phi / 2.0f) * std::sin(theta / 2.0f),
- std::sin(phi / 2.0f) * std::sin(theta / 2.0f)
);
return tilt;
}
} // namespace math
} // namespace sta

60
src/quaternion.cpp
View File

@ -31,18 +31,27 @@ namespace sta
oy *= (M_PI / 180.0f);
oz *= (M_PI / 180.0f);
float norm = sqrt(fmax(ox*ox + oy*oy + oz*oz, 0.000001));
float norm = sqrt(ox*ox + oy*oy + oz*oz);
if (norm == 0)
{
return *this;
}
float dt2 = dt / 2;
float cosDt2 = cos(norm * dt2);
float sinDt2 = 1/norm * sin(norm * dt2);
// ox /= norm;
// oy /= norm;
// oz /= norm;
float qw = cosDt2*w + -sinDt2*ox*x + -sinDt2*oy*y + -sinDt2*oz*z;
float qx = sinDt2*ox*w + cosDt2*x + sinDt2*oz*y + -sinDt2*oy*z;
float qy = sinDt2*oy*w + -sinDt2*oz*x + cosDt2*y + sinDt2*ox*z;
float qz = sinDt2*oz*w + sinDt2*oy*x + -sinDt2*ox*y + cosDt2*z;
return Quaternion(qw, qx, qy, qz);
return Quaternion(qw, qx, qy, qz).normalized();
}
Quaternion Quaternion::unit()
@ -62,15 +71,50 @@ namespace sta
return Quaternion(w / n, x / n, y / n, z / n);
}
Quaternion Quaternion::operator+(const Quaternion& quat)
Quaternion Quaternion::operator*(const Quaternion& quat)
{
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(rw, rx, ry, rz);
}
Quaternion operator*(const Quaternion& q1, const Quaternion& q2)
{
float rw = q1.w * q2.w - q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
float rx = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
float ry = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
float rz = q1.w * q2.z - q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
return Quaternion(rw, rx, ry, rz);
}
Quaternion operator+(const Quaternion& q1, const Quaternion& q2)
{
return Quaternion(w + quat.w, x + quat.x, y + quat.y, z + quat.z);
return Quaternion(q1.w + q2.w, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z);
}
Quaternion Quaternion::operator*(float scalar)
{
return Quaternion(w * scalar, x * scalar, y * scalar, z * scalar);
}
Quaternion operator*(const Quaternion& quat, float scalar)
{
return Quaternion(scalar * quat.w, scalar * quat.x, scalar * quat.y, scalar * quat.z);
}
Quaternion operator*(float scalar, const Quaternion& quat)
{
return quat * scalar;
}
Quaternion operator*(const Quaternion& quat, double scalar)
{
return Quaternion(scalar * quat.w, scalar * quat.x, scalar * quat.y, scalar * quat.z);
}
Quaternion operator*(double scalar, const Quaternion& quat)
{
return quat * scalar;
}
} // namespace math
} // namespace sta