Add matrix and kf code

src/algorithms/kalmanFilter.cpp

@@ -0,0 +1,46 @@
+#include <sta/math/algorithms/kalmanFilter.hpp>
+#include <sta/math/linalg/linalg.hpp>
+#include <sta/debug/debug.hpp>
+#include <sta/debug/assert.hpp>
+
+namespace math
+{
+
+KalmanFilter::KalmanFilter(matrix A, matrix B, matrix C, matrix Q, matrix R) : A_{A}, B_{B}, C_{C}, Q_{Q}, R_{R}, n_{A.get_cols()}
+{
+	STA_ASSERT_MSG(A.get_rows() == B.get_rows(), "#rows mismatch: A, B!");
+	STA_ASSERT_MSG(A.get_rows() == C.get_rows(), "#rows mismatch: A, C!");
+	STA_ASSERT_MSG(A.get_rows() == Q.get_rows(), "#rows mismatch: A, Q!");
+	STA_ASSERT_MSG(A.get_cols() == Q.get_rows(), "Q not square!");
+	STA_ASSERT_MSG(C.get_rows() == R.get_rows(), "#rows mismatch: C, R");
+	STA_ASSERT_MSG(R.get_cols() == R.get_rows(), "R not square!");
+    identity_ = matrix::eye(n_);
+}
+
+KalmanFilter::~KalmanFilter()
+{
+    // Destructor implementation
+}
+
+KalmanState KalmanFilter::predict(float dt, KalmanState state, matrix u)
+{
+    // Predict step implementation
+    // Update the state based on the system dynamics
+    state.x = A_ * state.x + B_ * u;
+    // Update the error covariance matrix
+    state.error = A_ * state.error * A_.T() + Q_;
+    return state;
+}
+
+KalmanState KalmanFilter::correct(float dt, KalmanState state, matrix z)
+{
+    // Correct step implementation
+    // Calculate the Kalman gain
+    matrix K = state.error * C_.T() * linalg::inv(C_ * state.error * C_.T() + R_);
+    // Update the state based on the measurement
+    state.x = state.x + K * (z - C_ * state.x); //TODO check transpose
+    // Update the error covariance matrix
+    state.error = (identity_ - K * C_) * state.error;
+    return state;
+}
+}