ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-12-15 17:48:03 +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
Download Patch File Download Diff File Expand all files Collapse all files

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