mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git
synced 2025-06-11 18:35:59 +00:00
Added attitude determination
This commit is contained in:
dario
parent
570685b0bb
commit
dec530a7df
37
include/sta/math/algorithms/attitude/integrate.hpp
Normal file
37
include/sta/math/algorithms/attitude/integrate.hpp
Normal file
@ -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
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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_ */
|
||||
|
||||
35
include/sta/math/quaternion.hpp
Normal file
35
include/sta/math/quaternion.hpp
Normal file
@ -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 */
|
||||
75
src/algorithms/attitude/integrate.cpp
Normal file
75
src/algorithms/attitude/integrate.cpp
Normal file
@ -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
|
||||
|
||||
|
||||
|
||||
57
src/quaternion.cpp
Normal file
57
src/quaternion.cpp
Normal file
@ -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
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user