diff --git a/filterpy/kalman/EKF.py b/filterpy/kalman/EKF.py index 4e32b43..9fb23d2 100644 --- a/filterpy/kalman/EKF.py +++ b/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) diff --git a/filterpy/kalman/tests/test_ekf_predict_update_state.py b/filterpy/kalman/tests/test_ekf_predict_update_state.py new file mode 100644 index 0000000..682def8 --- /dev/null +++ b/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