From 30899c47f00bc1b1c6e0ffd22d094a1910cb8ae9 Mon Sep 17 00:00:00 2001
From: marfanr <mohammadarfan45@gmail.com>
Date: Mon, 16 Dec 2024 22:51:46 +0700
Subject: [PATCH] fix: add inverse for 2x2 matrix

---
 include/keisan/matrix/matrix.hpp      |  3 ++-
 include/keisan/matrix/matrix.impl.hpp | 26 +++++++++++++++++++++++++-
 src/kalman.cpp                        |  4 ++--
 3 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/include/keisan/matrix/matrix.hpp b/include/keisan/matrix/matrix.hpp
index 60fb4ad..b851fc8 100644
--- a/include/keisan/matrix/matrix.hpp
+++ b/include/keisan/matrix/matrix.hpp
@@ -78,9 +78,10 @@ class Matrix
   const double * operator[](size_t pos) const;
 
   bool inverse();
+  bool inverse2();
 
   Matrix<N, M> transpose() const;
-  Matrix<M, N> round(double & tolerance = 1e-9) const;
+  Matrix<M, N> round(const double & tolerance) const;
 
 private:
   double data[M * N];
diff --git a/include/keisan/matrix/matrix.impl.hpp b/include/keisan/matrix/matrix.impl.hpp
index e420f8f..ffab330 100644
--- a/include/keisan/matrix/matrix.impl.hpp
+++ b/include/keisan/matrix/matrix.impl.hpp
@@ -306,6 +306,30 @@ const double * Matrix<M, N>::operator[](size_t pos) const
   return data + (pos * N);
 }
 
+template<size_t M, size_t N>
+bool Matrix<M, N>::inverse2()
+{
+  static_assert(
+    M == 2 && N == 2,
+    "Inverse matrix operation only available for 2 by 2 matrix.");
+
+  auto inverse = Matrix<M, N>::zero();
+  auto source = *this;
+
+  inverse[0][0] = source[1][1];
+  inverse[0][1] = -source[0][1];
+  inverse[1][0] = -source[1][0];
+  inverse[1][1] = source[0][0];
+  
+  double determinant = source[0][0] * inverse[0][0] + source[0][1] * inverse[1][0];
+  if (determinant == 0) {
+    return false;
+  }
+
+  (*this) = inverse * (1.0 / determinant);
+  return true;
+}
+
 template<size_t M, size_t N>
 bool Matrix<M, N>::inverse()
 {
@@ -459,7 +483,7 @@ Matrix<N, M> Matrix<M, N>::transpose() const
 }
 
 template<size_t M, size_t N>
-Matrix<M, N> Matrix<M, N>::round(double & tolerance = 1e-9) const {
+Matrix<M, N> Matrix<M, N>::round(const double & tolerance) const {
   Matrix<M, N> matrix;
   for (size_t i = 0; i < M; ++i) {
     for (size_t j = 0; j < N; ++j) {
diff --git a/src/kalman.cpp b/src/kalman.cpp
index 3310be6..15d39c1 100644
--- a/src/kalman.cpp
+++ b/src/kalman.cpp
@@ -52,8 +52,8 @@ Matrix<4, 1> Kalman::predict() {
 }
  
 Matrix<4, 1> Kalman::update(Matrix<2, 1> measurement) {
-  Matrix<4, 2> K = (P*(H.transpose()))*(H*P*H.transpose() + R).inverse();
-  Xk = (Xk + K*(measurement - H*Xk)).round();
+  Matrix<4, 2> K = (P*(H.transpose()))*(H*P*H.transpose() + R).inverse2();
+  Xk = (Xk + K*(measurement - H*Xk)).round(1e-9);
   Matrix<4, 4> I = Matrix<4, 4>::identity();
   P = (I - K*H)*P;