Extend correction step

.gitignore

@@ -0,0 +1,136 @@
+# Created by https://www.toptal.com/developers/gitignore/api/visualstudiocode,clion
+# Edit at https://www.toptal.com/developers/gitignore?templates=visualstudiocode,clion
+
+### CLion ###
+# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider
+# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839
+
+# User-specific stuff
+.idea/**/workspace.xml
+.idea/**/tasks.xml
+.idea/**/usage.statistics.xml
+.idea/**/dictionaries
+.idea/**/shelf
+
+# AWS User-specific
+.idea/**/aws.xml
+
+# Generated files
+.idea/**/contentModel.xml
+
+# Sensitive or high-churn files
+.idea/**/dataSources/
+.idea/**/dataSources.ids
+.idea/**/dataSources.local.xml
+.idea/**/sqlDataSources.xml
+.idea/**/dynamic.xml
+.idea/**/uiDesigner.xml
+.idea/**/dbnavigator.xml
+
+# Gradle
+.idea/**/gradle.xml
+.idea/**/libraries
+
+# Gradle and Maven with auto-import
+# When using Gradle or Maven with auto-import, you should exclude module files,
+# since they will be recreated, and may cause churn.  Uncomment if using
+# auto-import.
+# .idea/artifacts
+# .idea/compiler.xml
+# .idea/jarRepositories.xml
+# .idea/modules.xml
+# .idea/*.iml
+# .idea/modules
+# *.iml
+# *.ipr
+
+# CMake
+cmake-build-*/
+
+# Mongo Explorer plugin
+.idea/**/mongoSettings.xml
+
+# File-based project format
+*.iws
+
+# IntelliJ
+out/
+
+# mpeltonen/sbt-idea plugin
+.idea_modules/
+
+# JIRA plugin
+atlassian-ide-plugin.xml
+
+# Cursive Clojure plugin
+.idea/replstate.xml
+
+# SonarLint plugin
+.idea/sonarlint/
+
+# Crashlytics plugin (for Android Studio and IntelliJ)
+com_crashlytics_export_strings.xml
+crashlytics.properties
+crashlytics-build.properties
+fabric.properties
+
+# Editor-based Rest Client
+.idea/httpRequests
+
+# Android studio 3.1+ serialized cache file
+.idea/caches/build_file_checksums.ser
+
+### CLion Patch ###
+# Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721
+
+# *.iml
+# modules.xml
+# .idea/misc.xml
+# *.ipr
+
+# Sonarlint plugin
+# https://plugins.jetbrains.com/plugin/7973-sonarlint
+.idea/**/sonarlint/
+
+# SonarQube Plugin
+# https://plugins.jetbrains.com/plugin/7238-sonarqube-community-plugin
+.idea/**/sonarIssues.xml
+
+# Markdown Navigator plugin
+# https://plugins.jetbrains.com/plugin/7896-markdown-navigator-enhanced
+.idea/**/markdown-navigator.xml
+.idea/**/markdown-navigator-enh.xml
+.idea/**/markdown-navigator/
+
+# Cache file creation bug
+# See https://youtrack.jetbrains.com/issue/JBR-2257
+.idea/$CACHE_FILE$
+
+# CodeStream plugin
+# https://plugins.jetbrains.com/plugin/12206-codestream
+.idea/codestream.xml
+
+# Azure Toolkit for IntelliJ plugin
+# https://plugins.jetbrains.com/plugin/8053-azure-toolkit-for-intellij
+.idea/**/azureSettings.xml
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+
+### VisualStudioCode Patch ###
+# Ignore all local history of files
+.history
+.ionide
+
+# End of https://www.toptal.com/developers/gitignore/api/visualstudiocode,clion

README.md

@@ -2,14 +2,14 @@
 
 ## Description
 
-The Performant Embedded Algebra Kit (PEAK) is a lightweight and easy-to-use library for performing various algebraic operations.
+The Performant Embedded Algebra Kit (PEAK) is a lightweight, easy-to-use library for performing various algebraic operations. This library is built on Lukas Freiheits's matrix structures and linear algebra code, originally developed for developed for Aquila Maris' [caput firmware](https://git.intern.spaceteamaachen.de/AQUILA_MARIS/caput_firmware/src/branch/main/caput). 
 
 ## Features
 
 - Matrix operations: Perform matrix addition, subtraction, multiplication, and inversion.
 - Kalman Filter implementations
     - basic Kalman Filter
-    - Dynamic Kalman Filter, with variable delta t
+    - Dynamic Kalman Filter, with variable delta t (WIP)
 
 ## Usage
 
@@ -70,6 +70,8 @@ sta::math::matrix m_res = m_i * m_a;
 
 ```
 
+### Kalman Filters
+Every Kalman filter object requires a measurement matrix *H* and a measurement covariance matrix *R*. These matrices will be used when the `correct` method is called with only the current state and a measurement value. If you are using multiple sensors, you can also pass new *H* and *R* matrices directly, which will then be used instead. This also allows you to handle changes in measurement uncertainty over time.
 ## Limitations
 
 As of now, only matrices with no more than 256 elements are supported.

include/sta/math/algorithms/dynamicKalmanFilter.hpp

@@ -22,10 +22,11 @@ class DynamicKalmanFilter
 {
 private:
 	matrix A_; ///< The interval time independent part of state transition matrix.
-	matrix T_; ///< The time interval matrix.
+	matrix TA_; ///< The time interval matrix for the state transition matrix.
 	matrix B_; ///< The control input matrix.
 	matrix H_; ///< The observation matrix.
-	matrix Q_; ///< The process noise covariance matrix.
+	matrix Q_; ///< The interval time independent part of the process noise covariance matrix.
+	matrix TQ_; ///< The time interval matrix for 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.
@@ -37,13 +38,14 @@ public:
 	 * @brief Constructs a DynamicKalmanFilter object. The time interval will be dynamic. The state transition matrix will be build from A, T and the time interval dt during the prediction step.
 	 * Where F(i,j) = A(i,j) + dt^T(i,j).
 	 * @param A The interval time independent part of state transition matrix.
-	 * @param T The time interval matrix.
+	 * @param TA The time interval matrix for the state transition matrix.
 	 * @param B The control input matrix.
 	 * @param H The observation matrix.
 	 * @param Q The process noise covariance matrix.
+	 * @param TQ The time interval matrix for the process noise covariance matrix.
 	 * @param R The measurement noise covariance matrix.
 	 */
-	DynamicKalmanFilter(matrix A, matrix T, matrix B, matrix H, matrix Q, matrix R);
+	DynamicKalmanFilter(matrix A, matrix TA, matrix B, matrix H, matrix Q, matrix TQ, matrix R);
 
 	/**
 	 * @brief Destroys the DynamicKalmanFilter object.

include/sta/math/algorithms/kalmanFilter.hpp

@@ -69,6 +69,16 @@ public:
 	 * @return The corrected state of the Kalman filter.
 	 */
 	KalmanState correct(KalmanState state, matrix z);
+
+	/**
+	 * @brief Corrects the state of the Kalman filter based on a measurement. The observation matrix and measurement noise covariance matrix are passed as arguments.
+	 * @param state The current state and error convariance matrix.
+	 * @param z The observed measurement.
+	 * @param H The observation matrix.
+	 * @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);
 };
     
 } // namespace math

src/algorithms/dynamicKalmanFilter.cpp

@@ -10,17 +10,20 @@ namespace sta
 namespace math
 {
 
-DynamicKalmanFilter::DynamicKalmanFilter(matrix A, matrix T, matrix B, matrix H, matrix Q, matrix R) : A_{A},T_{T}, B_{B}, H_{H}, Q_{Q}, R_{R}, n_{A.get_cols()}
+DynamicKalmanFilter::DynamicKalmanFilter(matrix A, matrix TA, matrix B, matrix H, matrix Q, matrix TQ, matrix R) : A_{A},TA_{TA}, B_{B}, H_{H}, Q_{Q}, TQ_{TQ}, R_{R}, n_{A.get_cols()}
 {
 	STA_ASSERT_MSG(A.get_rows() == B.get_rows(), "#rows mismatch: A, B!");
 	STA_ASSERT_MSG(A.get_cols() == H.get_cols(), "#cols mismatch: A, H!");
-    STA_ASSERT_MSG(A.get_rows() == T.get_rows(), "#rows mismatch: A, T!");
+    STA_ASSERT_MSG(A.get_rows() == TA.get_rows(), "#rows mismatch: A, TA!");
-	STA_ASSERT_MSG(A.get_cols() == T.get_cols(), "#cols mismatch: A, T!");
+	STA_ASSERT_MSG(A.get_cols() == TA.get_cols(), "#cols mismatch: A, TA!");
 	STA_ASSERT_MSG(A.get_rows() == Q.get_rows(), "#rows mismatch: A, Q!");
 	STA_ASSERT_MSG(Q.get_cols() == Q.get_rows(), "Q not square!");
     STA_ASSERT_MSG(A.get_cols() == A.get_rows(), "A not square!");
 	STA_ASSERT_MSG(H.get_rows() == R.get_rows(), "#rows mismatch: H, R");
 	STA_ASSERT_MSG(R.get_rows() == R.get_cols(), "#R not square");
+    STA_ASSERT_MSG(Q.get_rows() == TQ.get_rows(), "#rows mismatch: Q, TQ!");
+	STA_ASSERT_MSG(Q.get_cols() == TQ.get_cols(), "#cols mismatch: Q, TQ!");
+
     identity_ = matrix::eye(n_);
 }
 
@@ -32,14 +35,19 @@ DynamicKalmanFilter::~DynamicKalmanFilter()
 KalmanState DynamicKalmanFilter::predict(float dt, KalmanState state, matrix u)
 {
     //Build the state transition matrix
-	matrix F = matrix::zeros(9, 9);
+	matrix F = matrix::zeros(A_.get_rows(), A_.get_rows());
-	for(int i =0; i  < n_*n_;i++){
+	for(int i =0; i  < A_.get_rows()* A_.get_rows();i++){
-		F.set(i, A_[i] * std::pow(dt, T_[i]));
+		F.set(i, A_[i] * std::pow(dt, TA_[i]));
+	}
+
+    matrix Q = matrix::zeros(Q_.get_rows(), Q_.get_cols());
+	for(int i =0; i  < Q_.get_rows()* Q_.get_cols();i++){
+		Q.set(i, Q_[i] * std::pow(dt, TQ_[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_;
+    state.error = F * state.error * F.T() + Q;
     return state;
 }
 

src/algorithms/kalmanFilter.cpp

@@ -50,6 +50,20 @@ KalmanState KalmanFilter::correct(KalmanState state, matrix z)
     return state;
 }
 
+KalmanState KalmanFilter::correct(KalmanState state, matrix z, matrix H, matrix R)
+{
+    // Correct step implementation
+    // Calculate the Kalman gain
+    matrix K = state.error * H.T() * linalg::inv(H * state.error * H.T() + R);
+    K.show_serial();
+    // Update the state based on the measurement
+    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;
+    return state;
+}
+
 } // namespace math
 
 } // namespace sta