ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-06-12 19:05:58 +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 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: public:
/** /**
* @brief Constructs a KalmanFilter object. * @brief Constructs a KalmanFilter object.
@ -62,6 +53,12 @@ public:
*/ */
KalmanState predict(KalmanState state, matrix u); 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); KalmanState predict(KalmanState state);
/** /**
@ -81,6 +78,14 @@ public:
* @return The corrected state of the Kalman filter. * @return The corrected state of the Kalman filter.
*/ */
KalmanState correct(KalmanState state, matrix z, matrix H, matrix R); 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 } // 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_); matrix K = state.error * H_.T() * linalg::inv(H_ * state.error * H_.T() + R_);
// Update the state based on the measurement // Update the state based on the measurement
state.x = state.x + K * (z - H_ * state.x); //TODO check transpose state.x = state.x + K * (z - H_ * state.x); //TODO check transpose
// Update the error covariance matrix // Update the error covariance matrix
state.error = (identity_ - K * H_) * state.error; state.error = (identity_ - K * H_) * state.error;
return state; return state;