ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-08-06 13:27:34 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Added attitude determination

Browse Source
This commit is contained in:
dario
2024-06-18 01:05:08 +02:00
parent 570685b0bb
commit dec530a7df
6 changed files with 301 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
include/sta/math/algorithms/attitude/integrate.hpp Normal file
View 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

110
include/sta/math/algorithms/dynamicKalmanFilter.hpp
View File

@@ -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