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

include/sta/math/quaternion.hpp

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

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

src/quaternion.cpp

@@ -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)
+	    Quaternion operator*(const Quaternion& quat, float scalar)
-	    {
+		{
-	        return Quaternion(w * scalar, x * scalar, y * scalar, z * 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