From 97a87dc45336725123a774921f065f2226cf56a3 Mon Sep 17 00:00:00 2001
From: Gabriele <gabriele.nava@iit.it>
Date: Tue, 16 Jul 2024 19:02:41 +0200
Subject: [PATCH] fixed kf test

---
 tests/test_kalmanFilter.py | 38 +++++++++++++++++++-------------------
 1 file changed, 19 insertions(+), 19 deletions(-)

diff --git a/tests/test_kalmanFilter.py b/tests/test_kalmanFilter.py
index 7c2f2e2..4a13bae 100644
--- a/tests/test_kalmanFilter.py
+++ b/tests/test_kalmanFilter.py
@@ -7,35 +7,34 @@
 class TestKalmanFilter(unittest.TestCase):
 
     def test_KalmanFilter(self):
-        
         print('Run KF class test.')
 
         # initialization of state matrices
         A = np.eye(2)
         X = np.array([[0.1], [0.1]])
-        B = np.ones((X.shape[0], 1))
-        U = 0.5
+        B = np.ones((2, 1))  # 2 rows (same as X), 1 column
+        U = np.array([[0.5]])  # single control input as a column vector
 
         # process noise covariance matrix and initial state covariance
-        Q = np.eye(X.shape[0])
+        Q = np.eye(2)
         P = np.diag((0.01, 0.01))
 
         # measurement matrices (state X plus a random gaussian noise)
-        Y = np.array([X[0, 0] + 0.001, X[1, 0] + 0.001])
-        Y = Y.reshape(-1, 1)
+        Y = np.array([[X[0, 0] + 0.001], [X[1, 0] + 0.001]])  # 2x1 matrix
         C = np.eye(2)
 
         # measurement noise covariance
-        R = np.eye(Y.shape[0])
-
-        var = {}
-        var.update({'X': X})
-        var.update({'A': A})
-        var.update({'B': B})
-        var.update({'U': U})
-        var.update({'Q': Q})
-        var.update({'C': C})
-        var.update({'R': R})
+        R = np.eye(2)
+
+        var = {
+            'X': X,
+            'A': A,
+            'B': B,
+            'U': U,
+            'Q': Q,
+            'C': C,
+            'R': R
+        }
 
         kf = KalmanFilter()
         kf.setup(var)
@@ -43,11 +42,12 @@ def test_KalmanFilter(self):
         x_est, y_predict = kf.update(Y)
 
         # verify if the object of the class is correct
-        self.assertEqual(x_est[0], 0.3505)
-        self.assertEqual(x_est[1], 0.3505)
-        self.assertEqual(y_predict[0], 0.07026195923972386)
+        np.testing.assert_almost_equal(x_est[0], 0.1014, decimal=4)
+        np.testing.assert_almost_equal(x_est[1], 0.1014, decimal=4)
+        np.testing.assert_almost_equal(y_predict[0], 0.0001588, decimal=7)
 
 
 if __name__ == '__main__':
     suite = unittest.TestSuite()
     suite.addTest(TestKalmanFilter('test_KalmanFilter'))
+    unittest.TextTestRunner().run(suite)