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 some documentation, Aff

Browse Source
This commit is contained in:
Milo Priegnitz
2024-06-02 22:15:33 +02:00
parent af40569b9d
commit e9b2017744
11 changed files with 265 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/algorithms/dynamicKalmanFilter.cpp
View File

@@ -4,18 +4,23 @@
#include <sta/debug/assert.hpp>
#include <cmath>
namespace sta
{
namespace math
{
DynamicKalmanFilter::DynamicKalmanFilter(matrix A, matrix T, matrix B, matrix C, matrix Q, matrix R) : A_{A},T_{T}, B_{B}, C_{C}, Q_{Q}, R_{R}, n_{A.get_cols()}
DynamicKalmanFilter::DynamicKalmanFilter(matrix A, matrix T, matrix B, matrix H, matrix Q, matrix R) : A_{A},T_{T}, B_{B}, H_{H}, Q_{Q}, R_{R}, n_{A.get_cols()}
{
STA_ASSERT_MSG(A.get_rows() == T.get_rows() && A.get_cols() == T.get_cols(), "#rows mismatch: A, B!");
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() == H.get_cols(), "#cols mismatch: A, H!");
STA_ASSERT_MSG(A.get_rows() == T.get_rows(), "#rows mismatch: A, T!");
STA_ASSERT_MSG(A.get_cols() == T.get_cols(), "#cols mismatch: A, T!");
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(Q.get_cols() == Q.get_rows(), "Q not square!");
STA_ASSERT_MSG(A.get_cols() == A.get_rows(), "A not square!");
STA_ASSERT_MSG(H.get_rows() == R.get_rows(), "#rows mismatch: H, R");
STA_ASSERT_MSG(R.get_rows() == R.get_cols(), "#R not square");
identity_ = matrix::eye(n_);
}
@@ -42,11 +47,14 @@ KalmanState DynamicKalmanFilter::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_);
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 - C_ * state.x); //TODO check transpose
state.x = state.x + K * (z - H_ * state.x); //TODO check transpose
// Update the error covariance matrix
state.error = (identity_ - K * C_) * state.error;
state.error = (identity_ - K * H_) * state.error;
return state;
}
}
} // namespace math
} // namespace sta

31
src/algorithms/kalmanFilter.cpp
View File

@@ -3,16 +3,20 @@
#include <sta/debug/debug.hpp>
#include <sta/debug/assert.hpp>
namespace sta
{
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()}
KalmanFilter::KalmanFilter(matrix F, matrix B, matrix H, matrix Q, matrix R) : F_{F}, B_{B}, H_{H}, Q_{Q}, R_{R}, n_{F.get_cols()}
{
STA_ASSERT_MSG(A.get_rows() == B.get_rows(), "#rows mismatch: A, B!");
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(F.get_rows() == B.get_rows(), "#rows mismatch: F, B!");
STA_ASSERT_MSG(F.get_cols() == H.get_cols(), "#cols mismatch: F, H!");
STA_ASSERT_MSG(F.get_rows() == Q.get_rows(), "#rows mismatch: F, Q!");
STA_ASSERT_MSG(Q.get_cols() == Q.get_rows(), "Q not square!");
STA_ASSERT_MSG(F.get_cols() == F.get_rows(), "F not square!");
STA_ASSERT_MSG(H.get_rows() == R.get_rows(), "#rows mismatch: H, R");
STA_ASSERT_MSG(R.get_rows() == R.get_cols(), "#R not square");
identity_ = matrix::eye(n_);
}
@@ -26,9 +30,9 @@ KalmanState KalmanFilter::predict(KalmanState state, matrix u)
{
// Predict step implementation
// Update the state based on the system dynamics
state.x = A_ * state.x + B_ * u;
state.x = F_ * state.x + B_ * u;
// Update the error covariance matrix
state.error = A_ * state.error * A_.T() + Q_;
state.error = F_ * state.error * F_.T() + Q_;
return state;
}
@@ -36,13 +40,16 @@ 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_);
matrix K = state.error * H_.T() * linalg::inv(H_ * state.error * H_.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 = state.x + K * (z - H_ * state.x); //TODO check transpose
state.x.show_serial();
// Update the error covariance matrix
state.error = (identity_ - K * C_) * state.error;
state.error = (identity_ - K * H_) * state.error;
return state;
}
}
} // namespace math
} // namespace sta