Preserve predicted EKF priors and support missing measurements

filterpy/kalman/EKF.py

@@ -222,11 +222,19 @@ def predict_update(self, z, HJacobian, Hx, args=(), hx_args=(), u=0):
         x = dot(F, x) + dot(B, u)
         P = dot(F, P).dot(F.T) + Q
 
-        H = HJacobian(x, *args)
-
         # save prior
-        self.x_prior = np.copy(self.x)
-        self.P_prior = np.copy(self.P)
+        self.x_prior = np.copy(x)
+        self.P_prior = np.copy(P)
+
+        if z is None:
+            self.x = x
+            self.P = P
+            self.z = reshape_z(np.array([None]*self.dim_z), self.dim_z, self.x.ndim)
+            self.x_post = self.x.copy()
+            self.P_post = self.P.copy()
+            return
+
+        H = HJacobian(x, *args)
 
         # update step
         PHT = dot(P, H.T)

filterpy/kalman/tests/test_ekf_predict_update_state.py

@@ -0,0 +1,39 @@
+from copy import deepcopy
+import numpy as np
+import pytest
+from filterpy.kalman import ExtendedKalmanFilter
+
+
+def make_filter():
+    f = ExtendedKalmanFilter(2, 1)
+    f.x = np.array([[1.0], [2.0]])
+    f.P = np.array([[2.0, 0.3], [0.3, 1.0]])
+    f.F = np.array([[1.0, 0.5], [0.0, 1.0]])
+    f.Q = np.eye(2) * 0.2
+    f.H = np.array([[1.0, 0.0]])
+    f.B = np.array([[0.2], [0.4]])
+    return f
+
+
+@pytest.mark.parametrize("measurement", [None, np.array([[2.5]])])
+def test_combined_step_matches_separate_predict_and_update(measurement):
+    combined = make_filter()
+    separate = deepcopy(combined)
+    jacobian = lambda x: np.array([[1.0, 0.0]])
+    observation = lambda x: x[:1]
+    separate.predict(u=np.array([[0.7]]))
+    separate.update(measurement, jacobian, observation)
+    combined.predict_update(measurement, jacobian, observation, u=np.array([[0.7]]))
+    for name in ["x", "P", "x_prior", "P_prior", "x_post", "P_post"]:
+        np.testing.assert_allclose(getattr(combined, name), getattr(separate, name))
+
+
+def test_missing_measurement_does_not_call_observation_functions():
+    f = make_filter()
+
+    def unused(x):
+        raise AssertionError("observation evaluated for missing measurement")
+
+    f.predict_update(None, unused, unused)
+    np.testing.assert_allclose(f.x, [[2.0], [2.0]])
+    assert f.z[0, 0] is None