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,68 +11,68 @@
 namespace sta
 {
 
-namespace math
+	namespace math
-{
+	{
 
-/**
+		/**
- * @class DynamicKalmanFilter
+		 * @class DynamicKalmanFilter
- * @brief Represents a Kalman filter with dynamic time interval between steps.
+		 * @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 A_; ///< The interval time independent part of state transition matrix.
-	matrix TA_; ///< The time interval matrix for the state transition matrix.
+			matrix TA_; ///< The time interval matrix for the state transition matrix.
-	matrix B_; ///< The control input matrix.
+			matrix B_; ///< The control input matrix.
-	matrix H_; ///< The observation matrix.
+			matrix H_; ///< The observation matrix.
-	matrix Q_; ///< The interval time independent part of the process noise covariance matrix.
+			matrix Q_; ///< The interval time independent part of the process noise covariance matrix.
-	matrix TQ_; ///< The time interval matrix for the process noise covariance matrix.
+			matrix TQ_; ///< The time interval matrix for the process noise covariance matrix.
-	matrix R_; ///< The measurement noise covariance matrix.
+			matrix R_; ///< The measurement noise covariance matrix.
-	uint8_t n_; ///< The dimension of the state vector.
+			uint8_t n_; ///< The dimension of the state vector.
-	matrix identity_; ///< The identity matrix with size of the state vector.
+			matrix identity_; ///< The identity matrix with size of the state vector.
 
 
 
-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.
+			 * @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).
+			 * Where F(i,j) = A(i,j) + dt^T(i,j).
-	 * @param A The interval time independent part of state transition matrix.
+			 * @param A The interval time independent part of state transition matrix.
-	 * @param TA The time interval matrix for the state transition matrix.
+			 * @param TA The time interval matrix for the state transition matrix.
-	 * @param B The control input matrix.
+			 * @param B The control input matrix.
-	 * @param H The observation matrix.
+			 * @param H The observation matrix.
-	 * @param Q The process noise covariance matrix.
+			 * @param Q The process noise covariance matrix.
-	 * @param TQ The time interval matrix for the process noise covariance matrix.
+			 * @param TQ The time interval matrix for the process noise covariance matrix.
-	 * @param R The measurement noise covariance matrix.
+			 * @param R The measurement noise covariance matrix.
-	 */
+			 */
-	DynamicKalmanFilter(matrix A, matrix TA, matrix B, matrix H, matrix Q, matrix TQ, matrix R);
+			DynamicKalmanFilter(matrix A, matrix TA, matrix B, matrix H, matrix Q, matrix TQ, matrix R);
 
-	/**
+			/**
-	 * @brief Destroys the DynamicKalmanFilter object.
+			 * @brief Destroys the DynamicKalmanFilter object.
-	 */
+			 */
-	~DynamicKalmanFilter();
+			~DynamicKalmanFilter();
 
-	/**
+			/**
-	 * @brief Predicts the next state of the Kalman filter, based on the current state and a control input and the time interval between the current and the next step.
+			 * @brief Predicts the next state of the Kalman filter, based on the current state and a control input and the time interval between the current and the next step.
-	 * The state transition matrix is build from A, T and the time interval dt.
+			 * The state transition matrix is build from A, T and the time interval dt.
-	 * Where F(i,j) = A(i,j) + dt^T(i,j).
+			 * Where F(i,j) = A(i,j) + dt^T(i,j).
-	 * @param dt The time interval between the current and the next step.
+			 * @param dt The time interval between the current and the next step.
-	 * @param state The current state and error convariance matrix.
+			 * @param state The current state and error convariance matrix.
-	 * @param u The control input.
+			 * @param u The control input.
-	 * @return The predicted state of the Kalman filter.
+			 * @return The predicted state of the Kalman filter.
-	 */
+			 */
-	KalmanState predict(float dt , KalmanState state, matrix u);
+			KalmanState predict(float dt , KalmanState state, matrix u);
 
-	/**
+			/**
-	 * @brief Corrects the state of the Kalman filter based on a measurement.
+			 * @brief Corrects the state of the Kalman filter based on a measurement.
-	 * @param state The current state and error convariance matrix.
+			 * @param state The current state and error convariance matrix.
-	 * @param z The observed measurement.
+			 * @param z The observed measurement.
-	 * @return The corrected state of the Kalman filter.
+			 * @return The corrected state of the Kalman filter.
-	 */
+			 */
-	KalmanState correct(KalmanState state, matrix z);
+			KalmanState correct(KalmanState state, matrix z);
-};
+		};
-    
+
-} // namespace math
+	} // namespace math
 
 } // namespace sta
 

include/sta/math/linalg/matrix.hpp

@@ -5,62 +5,60 @@
 namespace sta
 {
 
-namespace math
+	namespace math
-{
+	{
-    
-struct matrix 
-{
 
-    float * datafield = nullptr;
+		struct matrix
-    uint8_t * shape = nullptr;
+		{
 
-    matrix();
+			float * datafield = nullptr;
-    matrix(const matrix&);
+			uint8_t * shape = nullptr;
-    matrix(uint8_t, uint8_t);
-    matrix(uint8_t, uint8_t, float*);
-    ~matrix();
 
-    bool is_valid();
+			matrix();
+			matrix(const matrix&);
+			matrix(uint8_t, uint8_t);
+			matrix(uint8_t, uint8_t, float*);
+			~matrix();
 
-    uint16_t get_size();
+			bool is_valid();
-    uint8_t get_rows();
-    uint8_t get_cols();
 
-    matrix clone();
+			uint16_t get_size();
-    void show_serial();
+			uint8_t get_rows();
-    void show_shape();
+			uint8_t get_cols();
 
-    matrix& operator=(matrix);
+			matrix clone();
-    void reshape(uint8_t, uint8_t);
+			void show_serial();
-    
+			void show_shape();
-    float det();
-    matrix get_block(uint8_t, uint8_t, uint8_t, uint8_t);
-    void set_block(uint8_t, uint8_t, matrix);
-    void set(uint8_t, uint8_t, float);
-    void set(uint16_t, float);
-    matrix get_submatrix(uint8_t, uint8_t);
 
-    static matrix eye(uint8_t);
+			matrix& operator=(matrix);
-    static matrix zeros(uint8_t, uint8_t);
+			void reshape(uint8_t, uint8_t);
-    static matrix full(uint8_t, uint8_t, float);
-    
-    float operator()(uint8_t, uint8_t);
-    float operator[](uint16_t);
-    uint16_t get_idx(uint8_t, uint8_t);
 
-    matrix T();
+			float det();
-    matrix flatten();
+			matrix get_block(uint8_t, uint8_t, uint8_t, uint8_t);
-    float minor(uint8_t, uint8_t);
+			void set_block(uint8_t, uint8_t, matrix);
+			void set(uint8_t, uint8_t, float);
+			void set(uint16_t, float);
+			matrix get_submatrix(uint8_t, uint8_t);
 
-    matrix operator*(float);
+			static matrix eye(uint8_t);
-    matrix operator*(matrix);
+			static matrix zeros(uint8_t, uint8_t);
-    matrix operator+(matrix);
+			static matrix full(uint8_t, uint8_t, float);
-    matrix operator-(matrix);
 
-};
+			float operator()(uint8_t, uint8_t);
+			float operator[](uint16_t);
+			uint16_t get_idx(uint8_t, uint8_t);
 
-} // namespace math
+			matrix T();
+			matrix flatten();
+			float minor(uint8_t, uint8_t);
 
+			matrix operator*(float);
+			matrix operator*(matrix);
+			matrix operator+(matrix);
+			matrix operator-(matrix);
+
+		};
+	} // 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
+
+