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

7
src/linalg/linalg.cpp
View File

@@ -5,6 +5,8 @@
#include <sta/debug/debug.hpp>
#include <sta/debug/assert.hpp>
namespace sta{
namespace math {
namespace linalg {
@@ -346,7 +348,8 @@ matrix _inv_char_poly_2x2(matrix m) {
}
}
} // namespace linalg
} // namespace math
}
} // namespace sta

6
src/linalg/matrix.cpp
View File

@@ -5,6 +5,8 @@
#include <iostream>
#include <sta/debug/debug.hpp>
#include <sta/debug/assert.hpp>
namespace sta
{
namespace math {
matrix::matrix() {
@@ -432,4 +434,6 @@ void matrix::show_shape() {
}
}
}// namespace math
}//namespace sta

33
src/utils.cpp
View File

@@ -1,26 +1,31 @@
#include <sta/math/utils.hpp>
#include <cstdint>
namespace sta
{
namespace math
{
float fast_inv_sqrt(float v) {
float fast_inv_sqrt(float v) {
long i;
float x2, y;
const float threehalfs = 1.5f;
long i;
float x2, y;
const float threehalfs = 1.5f;
y = v;
x2 = y * 0.5f;
i = * (long*)&y;
i = 0x5f3759df - (i >> 1);
y = *(float *) &i;
y = y * (threehalfs - (x2 * y * y));
//y = y * (threehalfs - (x2 * y * y));
y = v;
x2 = y * 0.5f;
i = * (long*)&y;
i = 0x5f3759df - (i >> 1);
y = *(float *) &i;
y = y * (threehalfs - (x2 * y * y));
//y = y * (threehalfs - (x2 * y * y));
return y;
return y;
}
}
} // namespace stamath
} // namespace math
} // namespace sta