Added attitude determination

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