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

Make correction step static

Browse Source
This commit is contained in:
Milo Priegnitz 2024-06-12 17:23:11 +02:00
parent 9b5b01908c
commit 570685b0bb
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -78,7 +78,7 @@ public:
* @param R The measurement noise covariance matrix. * @param R The measurement noise covariance matrix.
* @return The corrected state of the Kalman filter. * @return The corrected state of the Kalman filter.
*/ */
KalmanState correct(KalmanState state, matrix z, matrix H, matrix R); static KalmanState correct(KalmanState state, matrix z, matrix H, matrix R);
}; };
} // namespace math } // namespace math

2
src/algorithms/kalmanFilter.cpp
View File

@ -60,7 +60,7 @@ KalmanState KalmanFilter::correct(KalmanState state, matrix z, matrix H, matrix
state.x = state.x + K * (z - H * state.x); //TODO check transpose state.x = state.x + K * (z - H * state.x); //TODO check transpose
state.x.show_serial(); state.x.show_serial();
// Update the error covariance matrix // Update the error covariance matrix
state.error = (identity_ - K * H) * state.error; state.error = (matrix::eye(state.x.get_rows()) - K * H) * state.error;
return state; return state;
} }