ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-09-29 06:37:33 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Add: conviniece functions, Fix: Asserts and debug prints

Browse Source
This commit is contained in:
Milo Priegnitz
2024-06-02 20:31:43 +02:00
parent 7c8388ebb9
commit af40569b9d
3 changed files with 20 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/algorithms/kalmanFilter.cpp
View File

@@ -9,11 +9,11 @@ namespace math
KalmanFilter::KalmanFilter(matrix A, matrix B, matrix C, matrix Q, matrix R) : A_{A}, B_{B}, C_{C}, Q_{Q}, R_{R}, n_{A.get_cols()}
{
STA_ASSERT_MSG(A.get_rows() == B.get_rows(), "#rows mismatch: A, B!");
STA_ASSERT_MSG(A.get_rows() == C.get_rows(), "#rows mismatch: A, C!");
STA_ASSERT_MSG(A.get_cols() == C.get_cols(), "#cols mismatch: A, C!");
STA_ASSERT_MSG(A.get_rows() == Q.get_rows(), "#rows mismatch: A, Q!");
STA_ASSERT_MSG(A.get_cols() == Q.get_rows(), "Q not square!");
STA_ASSERT_MSG(C.get_rows() == R.get_rows(), "#rows mismatch: C, R");
STA_ASSERT_MSG(R.get_cols() == R.get_rows(), "R not square!");
STA_ASSERT_MSG(R.get_rows() == R.get_cols(), "#R not square");
identity_ = matrix::eye(n_);
}
@@ -37,8 +37,10 @@ KalmanState KalmanFilter::correct(KalmanState state, matrix z)
// Correct step implementation
// Calculate the Kalman gain
matrix K = state.error * C_.T() * linalg::inv(C_ * state.error * C_.T() + R_);
K.show_serial();
// Update the state based on the measurement
state.x = state.x + K * (z - C_ * state.x); //TODO check transpose
state.x.show_serial();
// Update the error covariance matrix
state.error = (identity_ - K * C_) * state.error;
return state;