Continued Madgwick filter implementation

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

@@ -10,6 +10,8 @@
 
 #include <sta/math/quaternion.hpp>
 
+#include <cstdint>
+
 namespace sta
 {
 	namespace math
@@ -22,7 +24,7 @@ namespace sta
 			 * @param n 
 			 * @param alpha 
 			 */
-			MadgwickFilter(Quaternion state, uint32_t n, float alpha);
+			MadgwickFilter(Quaternion state, Quaternion direction, uint32_t n, float alpha);
 
 			/**
 			 * @brief Predicts the next step using rotation rate ingegration.
@@ -36,7 +38,10 @@ namespace sta
 
 			void correct(float dx, float dy, float dz);
 		private:
+			Quaternion objective(Quaternion q, Quaternion s);
+
 			Quaternion state_;
+			Quaternion direction_;
 			uint32_t n_;
 			float alpha_;
 		};

include/sta/math/quaternion.hpp

@@ -14,24 +14,60 @@ namespace sta
 		class Quaternion
 		{
 		public:
+			/**
+			 * @brief Construct a new Quaternion object
+			 * 
+			 * @param w 
+			 * @param x 
+			 * @param y 
+			 * @param z 
+			 */
 			Quaternion(float w, float x, float y, float z);
 
+			/**
+			 * @brief Construct a new Quaternion object
+			 * 
+			 */
 			Quaternion();
 
+			/**
+			 * @brief 
+			 * 
+			 * @param dt 
+			 * @param ox 
+			 * @param oy 
+			 * @param oz 
+			 * @return Quaternion 
+			 */
 			Quaternion integrate(float dt, float ox, float oy, float oz);
 
+			/**
+			 * @brief 
+			 * 
+			 * @return Quaternion 
+			 */
 			static Quaternion unit();
 
+			/**
+			 * @brief 
+			 * 
+			 * @return float 
+			 */
 			float norm();
 
+			/**
+			 * @brief 
+			 * 
+			 * @return Quaternion 
+			 */
 			Quaternion normalized();
 
-			Quaternion operator*(const Quaternion& quat);
-			Quaternion operator+(const Quaternion& quat);
-			
+			Quaternion conjugate();			
 		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& q1, const Quaternion& q2);
 		Quaternion operator*(const Quaternion& quat, float scalar);