From e3d45a11c3cee9136476ebc6b038b54eed623746 Mon Sep 17 00:00:00 2001 From: dario Date: Tue, 18 Jun 2024 01:05:08 +0200 Subject: [PATCH] Added attitude determination --- .../math/algorithms/attitude/integrate.hpp | 37 ++++++ .../math/algorithms/dynamicKalmanFilter.hpp | 110 +++++++++--------- include/sta/math/linalg/matrix.hpp | 86 +++++++------- include/sta/math/quaternion.hpp | 35 ++++++ src/algorithms/attitude/integrate.cpp | 75 ++++++++++++ src/quaternion.cpp | 57 +++++++++ 6 files changed, 301 insertions(+), 99 deletions(-) create mode 100644 include/sta/math/algorithms/attitude/integrate.hpp create mode 100644 include/sta/math/quaternion.hpp create mode 100644 src/algorithms/attitude/integrate.cpp create mode 100644 src/quaternion.cpp diff --git a/include/sta/math/algorithms/attitude/integrate.hpp b/include/sta/math/algorithms/attitude/integrate.hpp new file mode 100644 index 0000000..5d44585 --- /dev/null +++ b/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 +#include + + +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 diff --git a/include/sta/math/algorithms/dynamicKalmanFilter.hpp b/include/sta/math/algorithms/dynamicKalmanFilter.hpp index e09dd23..2599d7c 100644 --- a/include/sta/math/algorithms/dynamicKalmanFilter.hpp +++ b/include/sta/math/algorithms/dynamicKalmanFilter.hpp @@ -11,68 +11,68 @@ namespace sta { -namespace math -{ + namespace math + { -/** - * @class DynamicKalmanFilter - * @brief Represents a Kalman filter with dynamic time interval between steps. - */ -class DynamicKalmanFilter -{ -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. - matrix H_; ///< The observation 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 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. + /** + * @class DynamicKalmanFilter + * @brief Represents a Kalman filter with dynamic time interval between steps. + */ + class DynamicKalmanFilter + { + 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. + matrix H_; ///< The observation 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 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: - /** - * @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 TA The time interval matrix for the state transition matrix. - * @param B The control input matrix. - * @param H The observation matrix. - * @param Q The process noise covariance matrix. - * @param TQ The time interval matrix for the process 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); + 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). + * @param A The interval time independent part of state transition matrix. + * @param TA The time interval matrix for the state transition matrix. + * @param B The control input matrix. + * @param H The observation matrix. + * @param Q The process noise covariance matrix. + * @param TQ The time interval matrix for the process 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); - /** - * @brief Destroys the DynamicKalmanFilter object. - */ - ~DynamicKalmanFilter(); + /** + * @brief Destroys the DynamicKalmanFilter object. + */ + ~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. - * 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 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 + /** + * @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 diff --git a/include/sta/math/linalg/matrix.hpp b/include/sta/math/linalg/matrix.hpp index 1b7ce2a..6b98a23 100644 --- a/include/sta/math/linalg/matrix.hpp +++ b/include/sta/math/linalg/matrix.hpp @@ -5,62 +5,60 @@ namespace sta { -namespace math -{ - -struct matrix -{ + namespace math + { - float * datafield = nullptr; - uint8_t * shape = nullptr; + struct matrix + { - matrix(); - matrix(const matrix&); - matrix(uint8_t, uint8_t); - matrix(uint8_t, uint8_t, float*); - ~matrix(); + float * datafield = nullptr; + uint8_t * shape = nullptr; - bool is_valid(); + matrix(); + matrix(const matrix&); + matrix(uint8_t, uint8_t); + matrix(uint8_t, uint8_t, float*); + ~matrix(); - uint16_t get_size(); - uint8_t get_rows(); - uint8_t get_cols(); + bool is_valid(); - matrix clone(); - void show_serial(); - void show_shape(); + uint16_t get_size(); + uint8_t get_rows(); + uint8_t get_cols(); - matrix& operator=(matrix); - void reshape(uint8_t, uint8_t); - - 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); + matrix clone(); + void show_serial(); + void show_shape(); - static matrix eye(uint8_t); - static matrix zeros(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& operator=(matrix); + void reshape(uint8_t, uint8_t); - matrix T(); - matrix flatten(); - float minor(uint8_t, uint8_t); + 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); - matrix operator*(float); - matrix operator*(matrix); - matrix operator+(matrix); - matrix operator-(matrix); + static matrix eye(uint8_t); + static matrix zeros(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); -} // 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_ */ diff --git a/include/sta/math/quaternion.hpp b/include/sta/math/quaternion.hpp new file mode 100644 index 0000000..f937ff4 --- /dev/null +++ b/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 */ diff --git a/src/algorithms/attitude/integrate.cpp b/src/algorithms/attitude/integrate.cpp new file mode 100644 index 0000000..d98fd6b --- /dev/null +++ b/src/algorithms/attitude/integrate.cpp @@ -0,0 +1,75 @@ +/* + * integrate.cpp + * + * Created on: Jun 17, 2024 + * Author: Dario + */ + +#include +#include + +#include + +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 + + + diff --git a/src/quaternion.cpp b/src/quaternion.cpp new file mode 100644 index 0000000..cdee3b4 --- /dev/null +++ b/src/quaternion.cpp @@ -0,0 +1,57 @@ +/* + * quaternion.cpp + * + * Created on: Jun 17, 2024 + * Author: Dario + */ + +#include + +#include + +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 + +