ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-06-11 18:35:59 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Some attempts to make stuff faster

Browse Source
This commit is contained in:
dario 2024-07-17 09:14:43 +02:00
parent 6d8869a409
commit 71c29bfc58
3 changed files with 29 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -62,6 +62,8 @@ public:
*/
KalmanState predict(KalmanState state, matrix u);
KalmanState predict(KalmanState state);
/**
* @brief Corrects the state of the Kalman filter based on a measurement.
* @param state The current state and error convariance matrix.

27
src/algorithms/kalmanFilter.cpp
View File

@ -37,15 +37,32 @@ KalmanState KalmanFilter::predict(KalmanState state, matrix u)
return state;
}
KalmanState KalmanFilter::predict(KalmanState state)
{
state.x = F_ * state.x;
/*
state.x.set(0, 0, state.x[0] + F_.get_idx(0, 3) * state.x[3] + F_.get_idx(0, 6) * state.x[6]);
state.x.set(1, 0, state.x[1] + F_.get_idx(1, 4) * state.x[4] + F_.get_idx(1, 7) * state.x[7]);
state.x.set(2, 0, state.x[2] + F_.get_idx(2, 5) * state.x[5] + F_.get_idx(2, 8) * state.x[6]);
state.x.set(3, 0, state.x[3] + F_.get_idx(3, 6) * state.x[6]);
state.x.set(4, 0, state.x[4] + F_.get_idx(4, 7) * state.x[7]);
state.x.set(5, 0, state.x[5] + F_.get_idx(5, 8) * state.x[8]);
state.x.set(6, 0, state.x[6]);
state.x.set(7, 0, state.x[7]);
state.x.set(8, 0, state.x[8]);
*/
state.error = F_ * (state.error * F_.T()) + Q_;
return state;
}
KalmanState KalmanFilter::correct(KalmanState state, matrix z)
{
// Correct step implementation
// Calculate the Kalman gain
matrix K;
{
K = state.error * H_.T() * linalg::inv(H_ * state.error * H_.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 - H_ * state.x); //TODO check transpose
// Update the error covariance matrix

12
src/quaternion.cpp
View File

@ -8,6 +8,7 @@
#include <sta/math/quaternion.hpp>
#include <math.h>
#include <sta/debug/profile.hpp>
namespace sta
{
@ -32,6 +33,7 @@ namespace sta
oz *= (M_PI / 180.0f);
float norm = sqrt(ox*ox + oy*oy + oz*oz);
if (norm == 0)
{
return *this;
@ -42,14 +44,10 @@ namespace sta
float cosDt2 = cos(norm * dt2);
float sinDt2 = 1/norm * sin(norm * dt2);
// ox /= norm;
// oy /= norm;
// oz /= norm;
float qw = cosDt2*w + -sinDt2*ox*x + -sinDt2*oy*y + -sinDt2*oz*z;
float qx = sinDt2*ox*w + cosDt2*x + sinDt2*oz*y + -sinDt2*oy*z;
float qy = sinDt2*oy*w + -sinDt2*oz*x + cosDt2*y + sinDt2*ox*z;
float qz = sinDt2*oz*w + sinDt2*oy*x + -sinDt2*ox*y + cosDt2*z;
float qx = sinDt2*ox*w + cosDt2*x + sinDt2*oz*y + -sinDt2*oy*z;
float qy = sinDt2*oy*w + -sinDt2*oz*x + cosDt2*y + sinDt2*ox*z;
float qz = sinDt2*oz*w + sinDt2*oy*x + -sinDt2*ox*y + cosDt2*z;
return Quaternion(qw, qx, qy, qz).normalized();
}