From 570685b0bbb4ec91b5451fe9d02e27e38d06408c Mon Sep 17 00:00:00 2001 From: Milo Priegnitz Date: Wed, 12 Jun 2024 17:23:11 +0200 Subject: [PATCH] Make correction step static --- include/sta/math/algorithms/kalmanFilter.hpp | 2 +- src/algorithms/kalmanFilter.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/sta/math/algorithms/kalmanFilter.hpp b/include/sta/math/algorithms/kalmanFilter.hpp index cbe65b6..1d76460 100644 --- a/include/sta/math/algorithms/kalmanFilter.hpp +++ b/include/sta/math/algorithms/kalmanFilter.hpp @@ -78,7 +78,7 @@ public: * @param R The measurement noise covariance matrix. * @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 diff --git a/src/algorithms/kalmanFilter.cpp b/src/algorithms/kalmanFilter.cpp index 01654db..3242822 100644 --- a/src/algorithms/kalmanFilter.cpp +++ b/src/algorithms/kalmanFilter.cpp @@ -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.show_serial(); // 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; }