Added attitude determination

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

@@ -0,0 +1,37 @@
+/*
+ * integrate.hpp
+ *
+ *  Created on: Jun 17, 2024
+ *      Author: Dario
+ */
+
+#ifndef STA_MATHS_ATTITUDE_INTEGRATE_HPP
+#define STA_MATHS_ATTITUDE_INTEGRATE_HPP
+
+#include <sta/math/quaternion.hpp>
+#include <sta/time.hpp>
+
+
+namespace sta
+{
+	namespace math
+	{
+		class AttitudeModel
+	    {
+	    public:
+	        AttitudeModel(Quaternion state, float alpha = 1.0f);
+
+	        Quaternion update(float dt, float ox, float oy, float oz);
+
+	        Quaternion update(float ox, float oy, float oz);
+
+	        Quaternion getAttitude();
+	    private:
+	        Quaternion state_;
+	        float alpha_;
+	        float time_;
+	    };
+	}  // namespace math
+} // namespace sta
+
+#endif // STA_MATHS_ATTITUDE_INTEGRATE_HPP

include/sta/math/algorithms/dynamicKalmanFilter.hpp

@@ -11,16 +11,16 @@
 namespace sta
 {
 
-namespace math
+	namespace math
-{
+	{
 
-/**
+		/**
 		 * @class DynamicKalmanFilter
 		 * @brief Represents a Kalman filter with dynamic time interval between steps.
 		 */
-class DynamicKalmanFilter
+		class DynamicKalmanFilter
-{
+		{
-private:
+		private:
 			matrix A_; ///< The interval time independent part of state transition matrix.
 			matrix TA_; ///< The time interval matrix for the state transition matrix.
 			matrix B_; ///< The control input matrix.
@@ -33,7 +33,7 @@ private:
 
 
 
-public:
+		public:
 			/**
 			 * @brief Constructs a DynamicKalmanFilter object. The time interval will be dynamic. The state transition matrix will be build from A, T and the time interval dt during the prediction step.
 			 * Where F(i,j) = A(i,j) + dt^T(i,j).
@@ -70,9 +70,9 @@ public:
 			 * @return The corrected state of the Kalman filter.
 			 */
 			KalmanState correct(KalmanState state, matrix z);
-};
+		};
 
-} // namespace math
+	} // namespace math
 
 } // namespace sta
 

include/sta/math/linalg/matrix.hpp

@@ -5,11 +5,11 @@
 namespace sta
 {
 
-namespace math
+	namespace math
-{
+	{
 
-struct matrix 
+		struct matrix
-{
+		{
 
 			float * datafield = nullptr;
 			uint8_t * shape = nullptr;
@@ -57,10 +57,8 @@ struct matrix
 			matrix operator+(matrix);
 			matrix operator-(matrix);
 
-};
+		};
-
+	} // namespace math
-} // namespace math
-
 } // namespace sta
 
 #endif /* INC_MATRIX_HPP_ */

include/sta/math/quaternion.hpp

@@ -0,0 +1,35 @@
+/*
+ * quaternion.hpp
+ *
+ *  Created on: Jun 17, 2024
+ *      Author: Dario
+ */
+
+#ifndef STA_PEAK_QUATERNION_HPP
+#define STA_PEAK_QUATERNION_HPP
+
+namespace sta
+{
+	namespace math {
+		class Quaternion
+		{
+		public:
+			Quaternion(float w, float x, float y, float z);
+
+			Quaternion();
+
+			static Quaternion unit();
+
+			float norm();
+
+			Quaternion normalized();
+
+			Quaternion operator+(const Quaternion& quat);
+			Quaternion operator*(float scalar);
+		public:
+			float x, y, z, w;
+		};
+	}  // namespace math
+}  // namespace sta
+
+#endif /* STA_PEAK_QUATERNION_HPP */

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

src/quaternion.cpp

@@ -0,0 +1,57 @@
+/*
+ * quaternion.cpp
+ *
+ *  Created on: Jun 17, 2024
+ *      Author: Dario
+ */
+
+#include <sta/math/quaternion.hpp>
+
+#include <math.h>
+
+namespace sta
+{
+	namespace math
+	{
+		Quaternion::Quaternion(float w, float x, float y, float z)
+	        : x{x}, y{y}, z{z}, w{w}
+	    {
+
+	    }
+
+	    Quaternion::Quaternion()
+	    	: x{0}, y{0}, z{0}, w{1}
+	    {
+
+	    }
+
+	    Quaternion Quaternion::unit()
+	    {
+	    	return Quaternion();
+	    }
+
+	    float Quaternion::norm()
+	    {
+	        return sqrtf(x*x + y*y + z*z + w*w);
+	    }
+
+	    Quaternion Quaternion::normalized()
+	    {
+	        float n = norm();
+
+	        return Quaternion(w / n, x / n, y / n, z / n);
+	    }
+
+	    Quaternion Quaternion::operator+(const Quaternion& quat)
+	    {
+	        return Quaternion(x + quat.x, y + quat.y, z + quat.z, w + quat.w);
+	    }
+
+	    Quaternion Quaternion::operator*(float scalar)
+	    {
+	        return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar);
+	    }
+	}  // namespace math
+} // namespace sta
+
+