Continued Madgwick filter implementation

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)
-			: state_{state.normalized()},
+		MadgwickFilter::MadgwickFilter(Quaternion state, Quaternion direction, uint32_t n, float alpha)
+			: 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;
-			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(w, -x, -y, -z);
+		}
 
-			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)