From e9b20177448ef160ad4e359b8ac17ffda07f7ab4 Mon Sep 17 00:00:00 2001 From: Milo Priegnitz Date: Sun, 2 Jun 2024 22:15:33 +0200 Subject: [PATCH] Add some documentation, Aff --- README.md | 72 ++++++++++++++++-- .../math/algorithms/dynamicKalmanFilter.hpp | 76 ++++++++++++++----- include/sta/math/algorithms/kalmanFilter.hpp | 73 +++++++++++++----- include/sta/math/linalg/linalg.hpp | 9 ++- include/sta/math/linalg/matrix.hpp | 7 +- include/sta/math/utils.hpp | 12 +-- src/algorithms/dynamicKalmanFilter.cpp | 28 ++++--- src/algorithms/kalmanFilter.cpp | 31 +++++--- src/linalg/linalg.cpp | 7 +- src/linalg/matrix.cpp | 6 +- src/utils.cpp | 33 ++++---- 11 files changed, 265 insertions(+), 89 deletions(-) diff --git a/README.md b/README.md index 6e79522..e60fb90 100644 --- a/README.md +++ b/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 +#include + +// 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. \ No newline at end of file diff --git a/include/sta/math/algorithms/dynamicKalmanFilter.hpp b/include/sta/math/algorithms/dynamicKalmanFilter.hpp index da691b1..cd6e9fc 100644 --- a/include/sta/math/algorithms/dynamicKalmanFilter.hpp +++ b/include/sta/math/algorithms/dynamicKalmanFilter.hpp @@ -1,39 +1,77 @@ #ifndef DYNAMIC_KALMAN_FILTER_HPP #define DYNAMIC_KALMAN_FILTER_HPP #include +#include + +/** + * @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 diff --git a/include/sta/math/algorithms/kalmanFilter.hpp b/include/sta/math/algorithms/kalmanFilter.hpp index f6de3ee..7f192e0 100644 --- a/include/sta/math/algorithms/kalmanFilter.hpp +++ b/include/sta/math/algorithms/kalmanFilter.hpp @@ -2,37 +2,76 @@ #define KALMAN_FILTER_HPP #include +/** + * @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 diff --git a/include/sta/math/linalg/linalg.hpp b/include/sta/math/linalg/linalg.hpp index 6494b32..0fd97a8 100644 --- a/include/sta/math/linalg/linalg.hpp +++ b/include/sta/math/linalg/linalg.hpp @@ -2,6 +2,9 @@ #define INC_LINALG_HPP_ #include + +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_ */ diff --git a/include/sta/math/linalg/matrix.hpp b/include/sta/math/linalg/matrix.hpp index cf4b3af..1b7ce2a 100644 --- a/include/sta/math/linalg/matrix.hpp +++ b/include/sta/math/linalg/matrix.hpp @@ -1,6 +1,10 @@ #ifndef INC_MATRIX_HPP_ #define INC_MATRIX_HPP_ #include + +namespace sta +{ + namespace math { @@ -55,7 +59,8 @@ struct matrix }; +} // namespace math -} +} // namespace sta #endif /* INC_MATRIX_HPP_ */ diff --git a/include/sta/math/utils.hpp b/include/sta/math/utils.hpp index 468cb43..17efdd5 100644 --- a/include/sta/math/utils.hpp +++ b/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_ */ \ No newline at end of file diff --git a/src/algorithms/dynamicKalmanFilter.cpp b/src/algorithms/dynamicKalmanFilter.cpp index 2ef55f0..cab7174 100644 --- a/src/algorithms/dynamicKalmanFilter.cpp +++ b/src/algorithms/dynamicKalmanFilter.cpp @@ -4,18 +4,23 @@ #include #include +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(C.get_rows() == R.get_rows(), "#rows mismatch: C, R"); - STA_ASSERT_MSG(R.get_cols() == R.get_rows(), "R not square!"); + STA_ASSERT_MSG(Q.get_cols() == Q.get_rows(), "Q not square!"); + STA_ASSERT_MSG(A.get_cols() == A.get_rows(), "A 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 diff --git a/src/algorithms/kalmanFilter.cpp b/src/algorithms/kalmanFilter.cpp index 8bd8b62..b65c6d0 100644 --- a/src/algorithms/kalmanFilter.cpp +++ b/src/algorithms/kalmanFilter.cpp @@ -3,16 +3,20 @@ #include #include +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(A.get_cols() == C.get_cols(), "#cols mismatch: A, C!"); - 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(C.get_rows() == R.get_rows(), "#rows mismatch: C, R"); + STA_ASSERT_MSG(F.get_rows() == B.get_rows(), "#rows mismatch: F, B!"); + STA_ASSERT_MSG(F.get_cols() == H.get_cols(), "#cols mismatch: F, H!"); + STA_ASSERT_MSG(F.get_rows() == Q.get_rows(), "#rows mismatch: F, Q!"); + STA_ASSERT_MSG(Q.get_cols() == Q.get_rows(), "Q not square!"); + 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 diff --git a/src/linalg/linalg.cpp b/src/linalg/linalg.cpp index 0ea52c3..7d69ee7 100644 --- a/src/linalg/linalg.cpp +++ b/src/linalg/linalg.cpp @@ -5,6 +5,8 @@ #include #include +namespace sta{ + namespace math { namespace linalg { @@ -346,7 +348,8 @@ matrix _inv_char_poly_2x2(matrix m) { } -} +} // namespace linalg +} // namespace math -} +} // namespace sta diff --git a/src/linalg/matrix.cpp b/src/linalg/matrix.cpp index 811d901..ab3ae88 100644 --- a/src/linalg/matrix.cpp +++ b/src/linalg/matrix.cpp @@ -5,6 +5,8 @@ #include #include #include +namespace sta +{ namespace math { matrix::matrix() { @@ -432,4 +434,6 @@ void matrix::show_shape() { } -} +}// namespace math + +}//namespace sta diff --git a/src/utils.cpp b/src/utils.cpp index 2172fc4..dba37c2 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -1,26 +1,31 @@ #include #include +namespace sta +{ + namespace math { - float fast_inv_sqrt(float v) { +float fast_inv_sqrt(float v) { - long i; - float x2, y; - const float threehalfs = 1.5f; + long i; + float x2, y; + const float threehalfs = 1.5f; - y = v; - x2 = y * 0.5f; - i = * (long*)&y; - i = 0x5f3759df - (i >> 1); - y = *(float *) &i; - y = y * (threehalfs - (x2 * y * y)); - //y = y * (threehalfs - (x2 * y * y)); + y = v; + x2 = y * 0.5f; + i = * (long*)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *) &i; + y = y * (threehalfs - (x2 * y * y)); + //y = y * (threehalfs - (x2 * y * y)); - return y; + return y; - } +} -} // namespace stamath +} // namespace math + +} // namespace sta