fixed bug in kalman

Author: pavelkomarov

pynumdiff/kalman_smooth/_kalman_smooth.py

@@ -36,9 +36,9 @@ def _kalman_forward_filter(xhat0, P0, y, A, C, Q, R, u=None, B=None):
         xhat_pre.append(xhat_) # the [n]th index holds _{n|n-1} info
         P_pre.append(P_)
 
-        xhat = xhat_ # handle missing data
-        P = P_
-        if not np.isnan(y[n]):
+        xhat = xhat_.copy()
+        P = P_.copy()
+        if not np.isnan(y[n]): # handle missing data
             K = P_ @ C.T @ np.linalg.inv(C @ P_ @ C.T + R)
             xhat += K @ (y[n] - C @ xhat_)
             P -= K @ C @ P_

pynumdiff/tests/test_diff_methods.py

@@ -138,22 +138,22 @@ def iterated_first_order(*args, **kwargs): return first_order(*args, **kwargs)
                  [(1, 0), (2, 2), (1, 0), (2, 2)],
                  [(1, 0), (3, 3), (1, 0), (3, 3)]],
     constant_velocity: [[(-25, -25), (-25, -25), (0, -1), (0, 0)],
-                        [(-5, -6), (-4, -4), (0, -1), (0, 0)],
-                        [(-1, -2), (0, 0), (0, 0), (1, 0)],
-                        [(0, -1), (1, 0), (0, -1), (1, 1)],
+                        [(-6, -6), (-5, -5), (0, -1), (0, 0)],
+                        [(-1, -2), (0, 0), (0, -1), (0, 0)],
+                        [(-1, -1), (1, 0), (0, -1), (1, 0)],
                         [(1, 1), (2, 2), (1, 1), (2, 2)],
                         [(1, 1), (3, 3), (1, 1), (3, 3)]],
-    constant_acceleration: [[(-25, -25), (-25, -25), (0, 0), (1, 0)],
-                            [(-5, -5), (-3, -3), (0, 0), (1, 0)],
-                            [(-4, -4), (-2, -2), (0, 0), (1, 0)],
-                            [(0, -1), (1, 0), (0, 0), (1, 0)],
-                            [(1, 1), (3, 2), (1, 1), (3, 2)],
+    constant_acceleration: [[(-25, -25), (-25, -25), (0, -1), (0, 0)],
+                            [(-5, -6), (-4, -5), (0, -1), (0, 0)],
+                            [(-5, -5), (-4, -4), (0, -1), (0, 0)],
+                            [(-1, -1), (0, 0), (0, -1), (0, 0)],
+                            [(1, 1), (2, 2), (1, 1), (2, 2)],
                             [(1, 1), (3, 3), (1, 1), (3, 3)]],
-    constant_jerk: [[(-25, -25), (-25, -25), (0, 0), (1, 0)],
-                    [(-5, -5), (-3, -3), (0, 0), (1, 0)],
-                    [(-4, -4), (-2, -2), (0, 0), (1, 0)],
-                    [(-1, -2), (1, 0), (0, 0), (1, 0)],
-                    [(1, 0), (2, 2), (1, 0), (2, 2)],
+    constant_jerk: [[(-25, -25), (-25, -25), (0, -1), (0, 0)],
+                    [(-5, -5), (-4, -5), (0, -1), (0, 0)],
+                    [(-4, -5), (-3, -4), (0, -1), (0, 0)],
+                    [(-1, -2), (0, 0), (0, -1), (0, 0)],
+                    [(1, 0), (2, 1), (1, 0), (2, 1)],
                     [(1, 1), (3, 3), (1, 1), (3, 3)]],
     velocity: [[(-25, -25), (-18, -19), (0, -1), (1, 0)],
                [(-12, -12), (-11, -12), (-1, -1), (-1, -2)],