From 315abe5d6fb4a75887cb764f15797f9fe5c5d07b Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Tue, 6 Aug 2024 11:46:17 +0200
Subject: [PATCH] Just some restructuring to make the mechanical engineering
 student Milo angry

---
 include/sta/math/algorithms/kalmanFilter.hpp | 23 ++++++++++++--------
 src/algorithms/kalmanFilter.cpp              |  1 +
 2 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/include/sta/math/algorithms/kalmanFilter.hpp b/include/sta/math/algorithms/kalmanFilter.hpp
index b42f853..558904a 100644
--- a/include/sta/math/algorithms/kalmanFilter.hpp
+++ b/include/sta/math/algorithms/kalmanFilter.hpp
@@ -29,15 +29,6 @@ struct KalmanState
  */
 class KalmanFilter
 {
-private:
-	matrix F_;         ///< The state transition matrix.
-	matrix B_;         ///< The control input matrix.
-	matrix H_;         ///< The observation matrix.
-	matrix Q_;         ///< The process noise covariance matrix.
-	matrix R_;         ///< The measurement noise covariance matrix.
-	uint8_t n_;        ///< The dimension of the state vector.
-	matrix identity_;  ///< The identity matrix with size of the state vector.
-
 public:
 	/**
 	 * @brief Constructs a KalmanFilter object.
@@ -62,6 +53,12 @@ public:
 	 */
 	KalmanState predict(KalmanState state, matrix u);
 
+	/**
+	 * @brief Predicts the next state of the Kalman filter, based on the current state.
+	 * 
+	 * @param state The current state and uncertainty convariance matrix.
+	 * @return The predicted state of the Kalman filter. 
+	 */
 	KalmanState predict(KalmanState state);
 
 	/**
@@ -81,6 +78,14 @@ public:
 	 * @return The corrected state of the Kalman filter.
 	 */
 	KalmanState correct(KalmanState state, matrix z, matrix H, matrix R);
+private:
+	matrix F_;         ///< The state transition matrix.
+	matrix B_;         ///< The control input matrix.
+	matrix H_;         ///< The observation matrix.
+	matrix Q_;         ///< The process noise covariance matrix.
+	matrix R_;         ///< The measurement noise covariance matrix.
+	uint8_t n_;        ///< The dimension of the state vector.
+	matrix identity_;  ///< The identity matrix with size of the state vector.
 };
     
 } // namespace math
diff --git a/src/algorithms/kalmanFilter.cpp b/src/algorithms/kalmanFilter.cpp
index 59269d5..8b3821b 100644
--- a/src/algorithms/kalmanFilter.cpp
+++ b/src/algorithms/kalmanFilter.cpp
@@ -65,6 +65,7 @@ KalmanState KalmanFilter::correct(KalmanState state, matrix z)
     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
     state.error = (identity_ - K * H_) * state.error;
     return state;