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

Extend correction step

Browse Source
This commit is contained in:
Milo Priegnitz
2024-06-12 15:03:38 +02:00
parent 5cf23f802e
commit c5ea7dc596
6 changed files with 185 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -22,10 +22,11 @@ class DynamicKalmanFilter
{
private:
matrix A_; ///< The interval time independent part of state transition matrix.
matrix T_; ///< The time interval 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 process noise covariance 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.
@@ -37,13 +38,14 @@ 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 T The time interval 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 T, matrix B, matrix H, matrix Q, matrix R);
DynamicKalmanFilter(matrix A, matrix TA, matrix B, matrix H, matrix Q, matrix TQ, matrix R);
/**
* @brief Destroys the DynamicKalmanFilter object.

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

@@ -69,6 +69,16 @@ public:
* @return The corrected state of the Kalman filter.
*/
KalmanState correct(KalmanState state, matrix z);
/**
* @brief Corrects the state of the Kalman filter based on a measurement. The observation matrix and measurement noise covariance matrix are passed as arguments.
* @param state The current state and error convariance matrix.
* @param z The observed measurement.
* @param H The observation matrix.
* @param R The measurement noise covariance matrix.
* @return The corrected state of the Kalman filter.
*/
KalmanState correct(KalmanState state, matrix z, matrix H, matrix R);
};
} // namespace math