Better logging and added BHI160 sensor

spatz/models/kalman.py

@@ -6,6 +6,9 @@ from numpy.typing import ArrayLike
 
 def inv(val):
     if np.isscalar(val):
+        if val == 0:
+            return 0
+
         return 1 / val
     
     if len(val) == 1:
@@ -76,6 +79,8 @@ class KalmanFilter:
         H = self.__H
         R = self.__R
 
+        n = len(x)
+
         if hasattr(self.__H, '__call__'):
             H = self.__H(dt)
 
@@ -86,9 +91,9 @@ class KalmanFilter:
         K = err @ H.T @ inv(H @ err @ H.T + R)
 
         # Compute the corrected state.
-        x = x + K @ (z - H @ x)
+        x = x + (K @ (z - H @ x)).T
 
         # Compute the error after correction.
-        err = (np.identity('TODO') - K @ H) @ err
+        err = (np.identity(n) - K @ H) @ err
 
-        return x, err
+        return np.squeeze(np.asarray(x)), err