Add some documentation, Aff

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
-{
-	matrix error;
-	matrix x;
-};
-
+/**
+ * @class DynamicKalmanFilter
+ * @brief Represents a Kalman filter with dynamic time interval between steps.
+ */
 class DynamicKalmanFilter
 {
 private:
-	matrix A_;
-	matrix T_;
-	matrix B_;
-	matrix C_;
-	matrix Q_;
-	matrix R_;
-	uint8_t n_;
-	matrix identity_;
+	matrix A_; ///< The interval time independent part of state transition matrix.
+	matrix T_; ///< The time interval matrix.
+	matrix B_; ///< The control input matrix.
+	matrix H_; ///< The observation matrix.
+	matrix Q_; ///< The process noise covariance matrix.
+	matrix R_; ///< The measurement noise covariance matrix.
+	uint8_t n_; ///< The dimension of the state vector.
+	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 x;
+	matrix error; ///< The error covariance matrix.
+	matrix x;     ///< The state vector.
 };
 
+/**
+ * @class KalmanFilter
+ * @brief Represents a Kalman filter.
+ */
 class KalmanFilter
 {
 private:
-	matrix A_;
-	matrix B_;
-	matrix C_;
-	matrix Q_;
-	matrix R_;
-	uint8_t n_;
-	matrix identity_;
-
-
+	matrix F_;         ///< The state transition matrix.
+	matrix B_;         ///< The control input matrix.
+	matrix H_;         ///< The observation matrix.
+	matrix Q_;         ///< The process noise covariance matrix.
+	matrix R_;         ///< The measurement noise covariance matrix.
+	uint8_t n_;        ///< The dimension of the state vector.
+	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 stamath
+    namespace math
+    {
+        float fast_inv_sqrt(float);
+    } // namespace math
+} // namespace sta
 
 #endif /* INC_UTILS_HPP_ */