ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-06-11 18:35:59 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Just some restructuring to make the mechanical engineering student Milo angry

Browse Source
This commit is contained in:
dario 2024-08-06 11:46:17 +02:00
parent 444c716080
commit 315abe5d6f
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
include/sta/math/algorithms/kalmanFilter.hpp
View File

@ -29,15 +29,6 @@ struct KalmanState
*/
class KalmanFilter
{
private:
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:
/**
* @brief Constructs a KalmanFilter object.
@ -62,6 +53,12 @@ public:
*/
KalmanState predict(KalmanState state, matrix u);
/**
* @brief Predicts the next state of the Kalman filter, based on the current state.
*
* @param state The current state and uncertainty convariance matrix.
* @return The predicted state of the Kalman filter.
*/
KalmanState predict(KalmanState state);
/**
@ -81,6 +78,14 @@ public:
* @return The corrected state of the Kalman filter.
*/
KalmanState correct(KalmanState state, matrix z, matrix H, matrix R);
private:
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.
};
} // namespace math

1
src/algorithms/kalmanFilter.cpp
View File

@ -65,6 +65,7 @@ KalmanState KalmanFilter::correct(KalmanState state, matrix z)
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 - H_ * state.x); //TODO check transpose
// Update the error covariance matrix
state.error = (identity_ - K * H_) * state.error;
return state;