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

include/sta/math/algorithms/attitude/madgwick.hpp

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

include/sta/math/algorithms/attitude/slerp.hpp

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

include/sta/math/algorithms/attitude/tilt.hpp

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