ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-06-10 18:16:00 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/sta/math/linalg/matrix.hpp
View File

@ -1,7 +1,6 @@
#ifndef INC_MATRIX_HPP_
#define INC_MATRIX_HPP_
#include <cstdint>
namespace math
{
@ -39,6 +38,7 @@ struct matrix
static matrix eye(uint8_t);
static matrix zeros(uint8_t, uint8_t);
static matrix full(uint8_t, uint8_t, float);
float operator()(uint8_t, uint8_t);
float operator[](uint16_t);
@ -57,4 +57,5 @@ struct matrix
}
#endif /* INC_MATRIX_HPP_ */

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;

17
src/linalg/matrix.cpp
View File

@ -1,10 +1,10 @@
#include <sta/math/linalg/matrix.hpp>
#include <sta/math/linalg/linalg.hpp>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <sta/debug/debug.hpp>
#include <sta/debug/assert.hpp>
namespace math {
matrix::matrix() {
@ -302,9 +302,19 @@ matrix matrix::zeros(uint8_t r, uint8_t c) {
}
float matrix::operator()(uint8_t r, uint8_t c) {
matrix matrix::full(uint8_t r, uint8_t c, float v) {
return datafield[get_idx(r, c)];
matrix output(r, c);
for (uint16_t i = 0; i < r * c; i++) {
output.datafield[i] = v;
}
return output;
}
float matrix::operator()(uint8_t r, uint8_t c) {
float i = datafield[get_idx(r, c)];
return i;
}
@ -415,6 +425,7 @@ void matrix::show_serial() {
}
void matrix::show_shape() {
STA_DEBUG_PRINTF("Matrix shape: (%d x %d)", get_rows(), get_cols());