fix: add inverse for 2x2 matrix

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];

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) {

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;