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 conjugate();			
-			Quaternion operator+(const Quaternion& quat);
-			
 		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);

src/algorithms/attitude/madgwick.cpp

@@ -6,13 +6,15 @@
  */
 
 #include <sta/math/algorithms/attitude/madgwick.hpp>
+#include <sta/math/linalg/matrix.hpp>
 
 namespace sta
 {
 	namespace math
 	{
-		MadgwickFilter::MadgwickFilter(Quaternion state, uint32_t n, float alpha)
+		MadgwickFilter::MadgwickFilter(Quaternion state, Quaternion direction, uint32_t n, float alpha)
-			: state_{state.normalized()},
+			: state_{state},
+			  direction_{direction},
 			  n_{n},
 			  alpha_{alpha}
 		{
@@ -24,9 +26,20 @@ namespace sta
 			state_ = state_ + 0.5f * (state_ * Quaternion(0, ox, oy, oz)) * dt;
 		}
 
+		Quaternion MadgwickFilter::objective(Quaternion q, Quaternion s)
+		{
+			return q.conjugate() * direction_ * q - s;
+		}
+
+
 		void MadgwickFilter::correct(float dx, float dy, float dz)
 		{
+			Quaternion q;
 
+			for (uint32_t i = 0; i < n_; i++)
+			{
+
+			}
 		}
 
 

src/quaternion.cpp

@@ -71,14 +71,14 @@ namespace sta
 	        return Quaternion(w / n, x / n, y / n, z / n);
 	    }
 
-		Quaternion Quaternion::operator*(const Quaternion& quat)
+		Quaternion Quaternion::conjugate()
 		{
-			float rw = w * quat.w - x * quat.x + y * quat.y + z * quat.z;
+			return Quaternion(w, -x, -y, -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)
+		{
+			return Quaternion(q1.w - q2.w, q1.x - q2.x, q1.y - q2.y, q1.z - q2.z);
 		}
 
 		Quaternion operator*(const Quaternion& q1, const Quaternion& q2)