Updated quaternion class, added attitude from tilt, started madgwick filter

src/algorithms/attitude/integrate.cpp

@@ -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);

src/algorithms/attitude/madgwick.cpp

@@ -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
+

src/algorithms/attitude/slerp.cpp

@@ -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
-

src/algorithms/attitude/tilt.cpp

@@ -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
+
+