Add some documentation, Aff

README.md

@@ -1,15 +1,75 @@
 # Performant Embedded Algebra Kit (PEAK)
 
-
-> ⚠️ **Warning:** WORK IN PROGRESS and UNTESTED
-
-
 ## Description
 
-The Performant Embeddded Algebra Kit (PEAK) is a lightweight and easy-to-use library for performing various algebraic operations.
+The Performant Embedded Algebra Kit (PEAK) is a lightweight and easy-to-use library for performing various algebraic operations.
 
 ## Features
 
 - Matrix operations: Perform matrix addition, subtraction, multiplication, and inversion.
-- Kalman Filter implementation
+- Kalman Filter implementations
+    - basic Kalman Filter
+    - Dynamic Kalman Filter, with variable delta t
 
+## Usage
+
+### Basics
+
+The following example demonstrates the creation of matrices and basic operations on them.
+
+```cpp
+#include <sta/math/linalg/matrix.hpp>
+#include <sta/math/linalg/linalg.hpp>
+
+// Create a 3x2 matrix with every entry initialized to 0:
+sta::math::matrix m_0 = sta::math::matrix::zeros(3,2);
+
+// Set specific entries in the matrix:
+m_0.set(0, 1, 4); // set the entry in the first column and second column to 0
+m_0.set(1, 0, 5);
+m_0.set(2, 2, 99);
+
+// Display the matrix:
+m_0.show_serial();
+
+// Output:
+// Matrix shape: (3 x 2)
+// | 0.000000 | 4.000000 |
+// | 5.000000 | 0.000000 |
+// | 0.000000 | 0.000000 |
+
+// Create a 3x2 matrix with every entry initialized to 10:
+sta::math::matrix m_10 = sta::math::matrix::full(3,2,10); 
+
+// Matrices of the same shape can be added together using the + operator:
+sta::math::matrix m_a = m_0 + m_10;
+
+// m_a:
+// | 10.000000 | 14.000000 |
+// | 15.000000 | 10.000000 |
+// | 10.000000 | 10.000000 |
+
+// Create an identity matrix of size 3 and multiply it by 5:
+sta::math::matrix m_i = sta::math::matrix::eye(3) * 5;
+
+// Set a specific entry in the identity matrix:
+m_i.set(0, 2, 0.5);
+
+// m_i:
+// | 5.000000 | 0.000000 | 0.500000 |
+// | 0.000000 | 5.000000 | 0.000000 |
+// | 0.000000 | 0.000000 | 5.000000 |
+
+// Matrices of the appropriate shape can be multiplied together using the * operator:
+sta::math::matrix m_res = m_i * m_a;
+
+// m_res:
+// | 55.000000 | 75.000000 |
+// | 75.000000 | 50.000000 |
+// | 50.000000 | 50.000000 |
+
+```
+
+## Limitations
+
+As of now, only matrices with no more than 256 elements are supported.

include/sta/math/algorithms/dynamicKalmanFilter.hpp

@@ -1,39 +1,77 @@
 #ifndef DYNAMIC_KALMAN_FILTER_HPP
 #define DYNAMIC_KALMAN_FILTER_HPP
 #include <sta/math/linalg/matrix.hpp>
+#include <sta/math/algorithms/kalmanFilter.hpp>
+
+/**
+ * @file dynamicKalmanFilter.hpp
+ * @brief Contains the definition of the DynamicKalmanFilter class.
+ */
+
+namespace sta
+{
 
 namespace math
 {
 
-struct KalmanState
+/**
-{
+ * @class DynamicKalmanFilter
-	matrix error;
+ * @brief Represents a Kalman filter with dynamic time interval between steps.
-	matrix x;
+ */
-};
-
 class DynamicKalmanFilter
 {
 private:
-	matrix A_;
+	matrix A_; ///< The interval time independent part of state transition matrix.
-	matrix T_;
+	matrix T_; ///< The time interval matrix.
-	matrix B_;
+	matrix B_; ///< The control input matrix.
-	matrix C_;
+	matrix H_; ///< The observation matrix.
-	matrix Q_;
+	matrix Q_; ///< The process noise covariance matrix.
-	matrix R_;
+	matrix R_; ///< The measurement noise covariance matrix.
-	uint8_t n_;
+	uint8_t n_; ///< The dimension of the state vector.
-	matrix identity_;
+	matrix identity_; ///< The identity matrix with size of the state vector.
 
 
 
 public:
-	DynamicKalmanFilter(matrix, matrix, matrix, matrix, matrix, matrix);
+	/**
+	 * @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).
+	 * @param A The interval time independent part of state transition matrix.
+	 * @param T The time interval matrix.
+	 * @param B The control input matrix.
+	 * @param H The observation matrix.
+	 * @param Q The process noise covariance matrix.
+	 * @param R The measurement noise covariance matrix.
+	 */
+	DynamicKalmanFilter(matrix A, matrix T, matrix B, matrix H, matrix Q, matrix R);
+
+	/**
+	 * @brief Destroys the DynamicKalmanFilter object.
+	 */
 	~DynamicKalmanFilter();
-	KalmanState predict(float, KalmanState, matrix);
+
-	KalmanState correct(KalmanState, matrix);
+	/**
+	 * @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.
+	 * Where F(i,j) = A(i,j) + dt^T(i,j).
+	 * @param dt The time interval between the current and the next step.
+	 * @param state The current state and error convariance matrix.
+	 * @param u The control input.
+	 * @return The predicted state of the Kalman filter.
+	 */
+	KalmanState predict(float dt , KalmanState state, matrix u);
+
+	/**
+	 * @brief Corrects the state of the Kalman filter based on a measurement.
+	 * @param state The current state and error convariance matrix.
+	 * @param z The observed measurement.
+	 * @return The corrected state of the Kalman filter.
+	 */
+	KalmanState correct(KalmanState state, matrix z);
 };
     
+} // namespace math
 
-    
+} // namespace sta
-}
 
 #endif // DYNAMIC_KALMAN_FILTER_HPP

include/sta/math/algorithms/kalmanFilter.hpp

@@ -2,37 +2,76 @@
 #define KALMAN_FILTER_HPP
 #include <sta/math/linalg/matrix.hpp>
 
+/**
+ * @file kalmanFilter.hpp
+ * @brief Contains the definition of the KalmanFilter class and related structures.
+ */
+
+namespace sta
+{
+
 namespace math
 {
 
+/**
+ * @struct KalmanState
+ * @brief Represents the state of the Kalman filter.
+ */
 struct KalmanState
 {
-	matrix error;
+	matrix error; ///< The error covariance matrix.
-	matrix x;
+	matrix x;     ///< The state vector.
 };
 
+/**
+ * @class KalmanFilter
+ * @brief Represents a Kalman filter.
+ */
 class KalmanFilter
 {
 private:
-	matrix A_;
+	matrix F_;         ///< The state transition matrix.
-	matrix B_;
+	matrix B_;         ///< The control input matrix.
-	matrix C_;
+	matrix H_;         ///< The observation matrix.
-	matrix Q_;
+	matrix Q_;         ///< The process noise covariance matrix.
-	matrix R_;
+	matrix R_;         ///< The measurement noise covariance matrix.
-	uint8_t n_;
+	uint8_t n_;        ///< The dimension of the state vector.
-	matrix identity_;
+	matrix identity_;  ///< The identity matrix with size of the state vector.
-
-
 
 public:
-	KalmanFilter(matrix, matrix, matrix, matrix, matrix);
+	/**
+	 * @brief Constructs a KalmanFilter object.
+	 * @param F The state transition matrix.
+	 * @param B The control input matrix.
+	 * @param H The observation matrix.
+	 * @param Q The process noise covariance matrix.
+	 * @param R The measurement noise covariance matrix.
+	 */
+	KalmanFilter(matrix F, matrix B, matrix H, matrix Q, matrix R);
+
+	/**
+	 * @brief Destroys the KalmanFilter object.
+	 */
 	~KalmanFilter();
-	KalmanState predict(KalmanState, matrix);
+
-	KalmanState correct(KalmanState, matrix);
+	/**
+	 * @brief Predicts the next state of the Kalman filter, based on the current state and a control input.
+	 * @param state The current state and uncertainty convariance matrix.
+	 * @param u The control input.
+	 * @return The predicted state of the Kalman filter.
+	 */
+	KalmanState predict(KalmanState state, matrix u);
+
+	/**
+	 * @brief Corrects the state of the Kalman filter based on a measurement.
+	 * @param state The current state and error convariance matrix.
+	 * @param z The observed measurement.
+	 * @return The corrected state of the Kalman filter.
+	 */
+	KalmanState correct(KalmanState state, matrix z);
 };
     
+} // namespace math
 
-    
+} // namespace sta
-}
-
 #endif // KALMAN_FILTER_HPP

include/sta/math/linalg/linalg.hpp

@@ -2,6 +2,9 @@
 #define INC_LINALG_HPP_
 #include <sta/math/linalg/matrix.hpp>
 
+
+namespace sta 
+{
 namespace math 
 {
 namespace linalg 
@@ -27,6 +30,8 @@ matrix inv_schur_dec(matrix);
 matrix _inv_char_poly_3x3(matrix);
 matrix _inv_char_poly_2x2(matrix);
 
-}
+} // namespace linalg
-}
+} // namespace math
+} // namespace sta
+
 #endif /* INC_LINALG_HPP_ */

include/sta/math/linalg/matrix.hpp

@@ -1,6 +1,10 @@
 #ifndef INC_MATRIX_HPP_
 #define INC_MATRIX_HPP_
 #include <cstdint>
+
+namespace sta
+{
+
 namespace math
 {
     
@@ -55,7 +59,8 @@ struct matrix
 
 };
 
+} // namespace math
 
-}
+} // namespace sta
 
 #endif /* INC_MATRIX_HPP_ */

include/sta/math/utils.hpp

@@ -1,10 +1,12 @@
 #ifndef INC_UTILS_HPP_
 #define INC_UTILS_HPP_
-namespace math
+
+namespace sta
 {
-    float fast_inv_sqrt(float);
+    namespace math
-        
+    {
-    
+        float fast_inv_sqrt(float);
-} // namespace stamath
+    } // namespace math
+} // namespace sta
 
 #endif /* INC_UTILS_HPP_ */

src/algorithms/dynamicKalmanFilter.cpp

@@ -4,18 +4,23 @@
 #include <sta/debug/assert.hpp>
 #include <cmath>
 
+namespace sta
+{
+
 namespace math
 {
 
-DynamicKalmanFilter::DynamicKalmanFilter(matrix A, matrix T, matrix B, matrix C, matrix Q, matrix R) : A_{A},T_{T}, B_{B}, C_{C}, Q_{Q}, R_{R}, n_{A.get_cols()}
+DynamicKalmanFilter::DynamicKalmanFilter(matrix A, matrix T, matrix B, matrix H, matrix Q, matrix R) : A_{A},T_{T}, B_{B}, H_{H}, Q_{Q}, R_{R}, n_{A.get_cols()}
 {
-    STA_ASSERT_MSG(A.get_rows() == T.get_rows() && A.get_cols() == T.get_cols(), "#rows mismatch: A, B!");
 	STA_ASSERT_MSG(A.get_rows() == B.get_rows(), "#rows mismatch: A, B!");
-	STA_ASSERT_MSG(A.get_rows() == C.get_rows(), "#rows mismatch: A, C!");
+	STA_ASSERT_MSG(A.get_cols() == H.get_cols(), "#cols mismatch: A, H!");
+    STA_ASSERT_MSG(A.get_rows() == T.get_rows(), "#rows mismatch: A, T!");
+	STA_ASSERT_MSG(A.get_cols() == T.get_cols(), "#cols mismatch: A, T!");
 	STA_ASSERT_MSG(A.get_rows() == Q.get_rows(), "#rows mismatch: A, Q!");
-	STA_ASSERT_MSG(A.get_cols() == Q.get_rows(), "Q not square!");
+	STA_ASSERT_MSG(Q.get_cols() == Q.get_rows(), "Q not square!");
-	STA_ASSERT_MSG(C.get_rows() == R.get_rows(), "#rows mismatch: C, R");
+    STA_ASSERT_MSG(A.get_cols() == A.get_rows(), "A not square!");
-	STA_ASSERT_MSG(R.get_cols() == R.get_rows(), "R not square!");
+	STA_ASSERT_MSG(H.get_rows() == R.get_rows(), "#rows mismatch: H, R");
+	STA_ASSERT_MSG(R.get_rows() == R.get_cols(), "#R not square");
     identity_ = matrix::eye(n_);
 }
 
@@ -42,11 +47,14 @@ KalmanState DynamicKalmanFilter::correct(KalmanState state, matrix z)
 {
     // Correct step implementation
     // Calculate the Kalman gain
-    matrix K = state.error * C_.T() * linalg::inv(C_ * state.error * C_.T() + R_);
+    matrix K = state.error * H_.T() * linalg::inv(H_ * state.error * H_.T() + R_);
     // Update the state based on the measurement
-    state.x = state.x + K * (z - C_ * state.x); //TODO check transpose
+    state.x = state.x + K * (z - H_ * state.x); //TODO check transpose
     // Update the error covariance matrix
-    state.error = (identity_ - K * C_) * state.error;
+    state.error = (identity_ - K * H_) * state.error;
     return state;
 }
-}
+
+} // namespace math
+
+} // namespace sta

src/algorithms/kalmanFilter.cpp

@@ -3,16 +3,20 @@
 #include <sta/debug/debug.hpp>
 #include <sta/debug/assert.hpp>
 
+namespace sta
+{
+
 namespace math
 {
 
-KalmanFilter::KalmanFilter(matrix A, matrix B, matrix C, matrix Q, matrix R) : A_{A}, B_{B}, C_{C}, Q_{Q}, R_{R}, n_{A.get_cols()}
+KalmanFilter::KalmanFilter(matrix F, matrix B, matrix H, matrix Q, matrix R) : F_{F}, B_{B}, H_{H}, Q_{Q}, R_{R}, n_{F.get_cols()}
 {
-	STA_ASSERT_MSG(A.get_rows() == B.get_rows(), "#rows mismatch: A, B!");
+	STA_ASSERT_MSG(F.get_rows() == B.get_rows(), "#rows mismatch: F, B!");
-	STA_ASSERT_MSG(A.get_cols() == C.get_cols(), "#cols mismatch: A, C!");
+	STA_ASSERT_MSG(F.get_cols() == H.get_cols(), "#cols mismatch: F, H!");
-	STA_ASSERT_MSG(A.get_rows() == Q.get_rows(), "#rows mismatch: A, Q!");
+	STA_ASSERT_MSG(F.get_rows() == Q.get_rows(), "#rows mismatch: F, Q!");
-	STA_ASSERT_MSG(A.get_cols() == Q.get_rows(), "Q not square!");
+	STA_ASSERT_MSG(Q.get_cols() == Q.get_rows(), "Q not square!");
-	STA_ASSERT_MSG(C.get_rows() == R.get_rows(), "#rows mismatch: C, R");
+    STA_ASSERT_MSG(F.get_cols() == F.get_rows(), "F not square!");
+	STA_ASSERT_MSG(H.get_rows() == R.get_rows(), "#rows mismatch: H, R");
 	STA_ASSERT_MSG(R.get_rows() == R.get_cols(), "#R not square");
     identity_ = matrix::eye(n_);
 }
@@ -26,9 +30,9 @@ KalmanState KalmanFilter::predict(KalmanState state, matrix u)
 {
     // Predict step implementation
     // Update the state based on the system dynamics
-    state.x = A_ * state.x + B_ * u;
+    state.x = F_ * state.x + B_ * u;
     // Update the error covariance matrix
-    state.error = A_ * state.error * A_.T() + Q_;
+    state.error = F_ * state.error * F_.T() + Q_;
     return state;
 }
 
@@ -36,13 +40,16 @@ KalmanState KalmanFilter::correct(KalmanState state, matrix z)
 {
     // Correct step implementation
     // Calculate the Kalman gain
-    matrix K = state.error * C_.T() * linalg::inv(C_ * state.error * C_.T() + R_);
+    matrix K = state.error * H_.T() * linalg::inv(H_ * state.error * H_.T() + R_);
     K.show_serial();
     // Update the state based on the measurement
-    state.x = state.x + K * (z - C_ * state.x); //TODO check transpose
+    state.x = state.x + K * (z - H_ * state.x); //TODO check transpose
     state.x.show_serial();
     // Update the error covariance matrix
-    state.error = (identity_ - K * C_) * state.error;
+    state.error = (identity_ - K * H_) * state.error;
     return state;
 }
-}
+
+} // namespace math
+
+} // namespace sta

src/linalg/linalg.cpp

@@ -5,6 +5,8 @@
 #include <sta/debug/debug.hpp>
 #include <sta/debug/assert.hpp>
 
+namespace sta{
+
 namespace math {
 
 namespace linalg {
@@ -346,7 +348,8 @@ matrix _inv_char_poly_2x2(matrix m) {
 }
 
 
-}
+} // namespace linalg
 
+} // namespace math
 
-}
+} // namespace sta

src/linalg/matrix.cpp

@@ -5,6 +5,8 @@
 #include <iostream>
 #include <sta/debug/debug.hpp>
 #include <sta/debug/assert.hpp>
+namespace sta
+{
 namespace math {
 
 matrix::matrix() {
@@ -432,4 +434,6 @@ void matrix::show_shape() {
 
 }
 
-}
+}// namespace math
+
+}//namespace sta

src/utils.cpp

@@ -1,26 +1,31 @@
 #include <sta/math/utils.hpp>
 #include <cstdint>
 
+namespace sta
+{
+
 namespace math
 {
 
         
-    float fast_inv_sqrt(float v) {
+float fast_inv_sqrt(float v) {
 
-        long i;
+    long i;
-        float x2, y;
+    float x2, y;
-        const float threehalfs = 1.5f;
+    const float threehalfs = 1.5f;
 
-        y = v;
+    y = v;
-        x2 = y * 0.5f;
+    x2 = y * 0.5f;
-        i = * (long*)&y;
+    i = * (long*)&y;
-        i = 0x5f3759df - (i >> 1);
+    i = 0x5f3759df - (i >> 1);
-        y = *(float *) &i;
+    y = *(float *) &i;
-        y = y * (threehalfs - (x2 * y * y));
+    y = y * (threehalfs - (x2 * y * y));
-        //y = y * (threehalfs - (x2 * y * y));
+    //y = y * (threehalfs - (x2 * y * y));
 
-        return y;
+    return y;
 
-    }
+}
     
-} // namespace stamath
+} // namespace math
+
+} // namespace sta