Added attitude determination

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

@@ -0,0 +1,37 @@
+/*
+ * integrate.hpp
+ *
+ *  Created on: Jun 17, 2024
+ *      Author: Dario
+ */
+
+#ifndef STA_MATHS_ATTITUDE_INTEGRATE_HPP
+#define STA_MATHS_ATTITUDE_INTEGRATE_HPP
+
+#include <sta/math/quaternion.hpp>
+#include <sta/time.hpp>
+
+
+namespace sta
+{
+	namespace math
+	{
+		class AttitudeModel
+	    {
+	    public:
+	        AttitudeModel(Quaternion state, float alpha = 1.0f);
+
+	        Quaternion update(float dt, float ox, float oy, float oz);
+
+	        Quaternion update(float ox, float oy, float oz);
+
+	        Quaternion getAttitude();
+	    private:
+	        Quaternion state_;
+	        float alpha_;
+	        float time_;
+	    };
+	}  // namespace math
+} // namespace sta
+
+#endif // STA_MATHS_ATTITUDE_INTEGRATE_HPP

include/sta/math/linalg/matrix.hpp

@@ -58,9 +58,7 @@ struct matrix
 			matrix operator-(matrix);
 
 		};
-
 	} // namespace math
-
 } // namespace sta
 
 #endif /* INC_MATRIX_HPP_ */

include/sta/math/quaternion.hpp

@@ -0,0 +1,35 @@
+/*
+ * quaternion.hpp
+ *
+ *  Created on: Jun 17, 2024
+ *      Author: Dario
+ */
+
+#ifndef STA_PEAK_QUATERNION_HPP
+#define STA_PEAK_QUATERNION_HPP
+
+namespace sta
+{
+	namespace math {
+		class Quaternion
+		{
+		public:
+			Quaternion(float w, float x, float y, float z);
+
+			Quaternion();
+
+			static Quaternion unit();
+
+			float norm();
+
+			Quaternion normalized();
+
+			Quaternion operator+(const Quaternion& quat);
+			Quaternion operator*(float scalar);
+		public:
+			float x, y, z, w;
+		};
+	}  // namespace math
+}  // namespace sta
+
+#endif /* STA_PEAK_QUATERNION_HPP */

src/algorithms/attitude/integrate.cpp

@@ -0,0 +1,75 @@
+/*
+ * integrate.cpp
+ *
+ *  Created on: Jun 17, 2024
+ *      Author: Dario
+ */
+
+#include <sta/math/algorithms/attitude/integrate.hpp>
+#include <sta/math/linalg/matrix.hpp>
+
+#include <math.h>
+
+namespace sta
+{
+	namespace math
+	{
+		AttitudeModel::AttitudeModel(Quaternion state, float alpha /* = 1.0f */)
+			: state_{state},
+			  alpha_{alpha},
+			  time_{sta::timeUs() / 1000000.0f}
+		{
+
+		}
+
+		Quaternion AttitudeModel::update(float dt, float ox, float oy, float oz)
+		{
+			if (dt < 0.0000001)
+				return state_;
+
+			time_ += dt;
+
+			ox *= (M_PI / 180.0f);
+			oy *= (M_PI / 180.0f);
+			oz *= (M_PI / 180.0f);
+
+			float norm = sqrt(fmax(ox*ox + oy*oy + oz*oz, 0.000001));
+			float dt2 = dt / 2;
+			float cosDt2 = cos(norm * dt2);
+			float sinDt2 = 1/norm * sin(norm * dt2);
+
+			float mat[16] = {
+				cosDt2,     -sinDt2*ox, -sinDt2*oy, -sinDt2*oz,
+				sinDt2*ox,  cosDt2,     sinDt2*oz,  -sinDt2*oy,
+				sinDt2*oy,  -sinDt2*oz, cosDt2,     sinDt2*ox,
+				sinDt2*oz,  sinDt2*oy,  -sinDt2*ox, cosDt2
+			};
+			matrix F(4, 4, mat);
+
+			float quat[4] = {
+				state_.w, state_.x, state_.y, state_.z
+			};
+			matrix qold(4, 1, quat);
+			matrix qnew = F * qold;
+
+			state_ = Quaternion(qnew[0], qnew[1], qnew[2], qnew[3]).normalized();
+
+			return state_;
+		}
+
+		Quaternion AttitudeModel::update(float ox, float oy, float oz)
+		{
+			return update(timeUs() / 1000000.0f - time_, ox, oy, oz);
+		}
+
+    	Quaternion AttitudeModel::getAttitude()
+        {
+            return state_;
+        }
+
+	}  // namespace math
+
+} // namespace sta
+
+
+

src/quaternion.cpp

@@ -0,0 +1,57 @@
+/*
+ * quaternion.cpp
+ *
+ *  Created on: Jun 17, 2024
+ *      Author: Dario
+ */
+
+#include <sta/math/quaternion.hpp>
+
+#include <math.h>
+
+namespace sta
+{
+	namespace math
+	{
+		Quaternion::Quaternion(float w, float x, float y, float z)
+	        : x{x}, y{y}, z{z}, w{w}
+	    {
+
+	    }
+
+	    Quaternion::Quaternion()
+	    	: x{0}, y{0}, z{0}, w{1}
+	    {
+
+	    }
+
+	    Quaternion Quaternion::unit()
+	    {
+	    	return Quaternion();
+	    }
+
+	    float Quaternion::norm()
+	    {
+	        return sqrtf(x*x + y*y + z*z + w*w);
+	    }
+
+	    Quaternion Quaternion::normalized()
+	    {
+	        float n = norm();
+
+	        return Quaternion(w / n, x / n, y / n, z / n);
+	    }
+
+	    Quaternion Quaternion::operator+(const Quaternion& quat)
+	    {
+	        return Quaternion(x + quat.x, y + quat.y, z + quat.z, w + quat.w);
+	    }
+
+	    Quaternion Quaternion::operator*(float scalar)
+	    {
+	        return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar);
+	    }
+	}  // namespace math
+} // namespace sta
+
+