Added slerp and updated quaternion class

src/algorithms/attitude/integrate.cpp

@@ -6,6 +6,7 @@
  */
 
 #include <sta/math/algorithms/attitude/integrate.hpp>
+#include <sta/math/algorithms/attitude/slerp.hpp>
 #include <sta/math/linalg/matrix.hpp>
 
 #include <math.h>
@@ -16,6 +17,9 @@ namespace sta
 	{
 		AttitudeModel::AttitudeModel(Quaternion state, float alpha /* = 1.0f */)
 			: state_{state},
+			  ox_{0.0},
+			  oy_{0.0},
+			  oz_{0.0},
 			  alpha_{alpha},
 			  time_{sta::timeUs() / 1000000.0f}
 		{

src/algorithms/attitude/slerp.cpp

@@ -0,0 +1,36 @@
+/*
+ * 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
+