Add: Dynamic KF and remove timesteps from normal KF

include/sta/math/algorithms/dynamicKalmanFilter.hpp

@@ -0,0 +1,39 @@
+#ifndef DYNAMIC_KALMAN_FILTER_HPP
+#define DYNAMIC_KALMAN_FILTER_HPP
+#include <sta/math/linalg/matrix.hpp>
+
+namespace math
+{
+
+struct KalmanState
+{
+	matrix error;
+	matrix x;
+};
+
+class DynamicKalmanFilter
+{
+private:
+	matrix A_;
+	matrix T_;
+	matrix B_;
+	matrix C_;
+	matrix Q_;
+	matrix R_;
+	uint8_t n_;
+	matrix identity_;
+
+
+
+public:
+	DynamicKalmanFilter(matrix, matrix, matrix, matrix, matrix, matrix);
+	~DynamicKalmanFilter();
+	KalmanState predict(float, KalmanState, matrix);
+	KalmanState correct(KalmanState, matrix);
+};
+    
+
+    
+}
+
+#endif // DYNAMIC_KALMAN_FILTER_HPP

include/sta/math/algorithms/kalmanFilter.hpp

@@ -27,8 +27,8 @@ private:
 public:
 	KalmanFilter(matrix, matrix, matrix, matrix, matrix);
 	~KalmanFilter();
-	KalmanState predict(float, KalmanState, matrix);
+	KalmanState predict(KalmanState, matrix);
-	KalmanState correct(float,KalmanState, matrix);
+	KalmanState correct(KalmanState, matrix);
 };
     
 

src/algorithms/dynamicKalmanFilter.cpp

@@ -0,0 +1,52 @@
+#include <sta/math/algorithms/dynamicKalmanFilter.hpp>
+#include <sta/math/linalg/linalg.hpp>
+#include <sta/debug/debug.hpp>
+#include <sta/debug/assert.hpp>
+#include <cmath>
+
+namespace math
+{
+
+DynamicKalmanFilter::DynamicKalmanFilter(matrix A, matrix T, matrix B, matrix C, matrix Q, matrix R) : A_{A},T_{T}, B_{B}, C_{C}, Q_{Q}, R_{R}, n_{A.get_cols()}
+{
+    STA_ASSERT_MSG(A.get_rows() == T.get_rows() && A.get_cols() == T.get_cols(), "#rows mismatch: A, B!");
+	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_);
+}
+
+DynamicKalmanFilter::~DynamicKalmanFilter()
+{
+    // Destructor implementation
+}
+
+KalmanState DynamicKalmanFilter::predict(float dt, KalmanState state, matrix u)
+{
+    //Build the state transition matrix
+	matrix F = matrix::zeros(9, 9);
+	for(int i =0; i  < n_*n_;i++){
+		F.set(i, A_[i] * std::pow(dt, T_[i]));
+	}
+    // Update the state based on the system dynamics
+    state.x = F * state.x + F * u;
+    // Update the error covariance matrix
+    state.error = F * state.error * F.T() + Q_;
+    return state;
+}
+
+KalmanState DynamicKalmanFilter::correct(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;
+}
+}

src/algorithms/kalmanFilter.cpp

@@ -22,7 +22,7 @@ KalmanFilter::~KalmanFilter()
     // Destructor implementation
 }
 
-KalmanState KalmanFilter::predict(float dt, KalmanState state, matrix u)
+KalmanState KalmanFilter::predict(KalmanState state, matrix u)
 {
     // Predict step implementation
     // Update the state based on the system dynamics
@@ -32,7 +32,7 @@ KalmanState KalmanFilter::predict(float dt, KalmanState state, matrix u)
     return state;
 }
 
-KalmanState KalmanFilter::correct(float dt, KalmanState state, matrix z)
+KalmanState KalmanFilter::correct(KalmanState state, matrix z)
 {
     // Correct step implementation
     // Calculate the Kalman gain