diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..eff2e6c --- /dev/null +++ b/.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 \ No newline at end of file diff --git a/README.md b/README.md index e56e520..9b61508 100644 --- a/README.md +++ b/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. \ No newline at end of file diff --git a/include/sta/math/algorithms/dynamicKalmanFilter.hpp b/include/sta/math/algorithms/dynamicKalmanFilter.hpp index cd6e9fc..e09dd23 100644 --- a/include/sta/math/algorithms/dynamicKalmanFilter.hpp +++ b/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. diff --git a/include/sta/math/algorithms/kalmanFilter.hpp b/include/sta/math/algorithms/kalmanFilter.hpp index 7f192e0..cbe65b6 100644 --- a/include/sta/math/algorithms/kalmanFilter.hpp +++ b/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 diff --git a/src/algorithms/dynamicKalmanFilter.cpp b/src/algorithms/dynamicKalmanFilter.cpp index cab7174..8689868 100644 --- a/src/algorithms/dynamicKalmanFilter.cpp +++ b/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_cols() == T.get_cols(), "#cols mismatch: A, T!"); + STA_ASSERT_MSG(A.get_rows() == TA.get_rows(), "#rows mismatch: A, TA!"); + 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); - for(int i =0; i < n_*n_;i++){ - F.set(i, A_[i] * std::pow(dt, T_[i])); + matrix F = matrix::zeros(A_.get_rows(), A_.get_rows()); + for(int i =0; i < A_.get_rows()* A_.get_rows();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; } diff --git a/src/algorithms/kalmanFilter.cpp b/src/algorithms/kalmanFilter.cpp index b65c6d0..01654db 100644 --- a/src/algorithms/kalmanFilter.cpp +++ b/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