diff --git a/filterpy/kalman/EKF.py b/filterpy/kalman/EKF.py index 87912653..b395aac8 100644 --- a/filterpy/kalman/EKF.py +++ b/filterpy/kalman/EKF.py @@ -119,6 +119,7 @@ def predict_update(self, z, HJacobian, Hx, args=(), hx_args=(), u=0): # update step S = dot3(H, P, H.T) + R K = dot3(P, H.T, linalg.inv (S)) + self._K = K self._x = x + dot(K, (z - Hx(x, *hx_args))) @@ -190,9 +191,11 @@ def update(self, z, HJacobian, Hx, R=None, args=(), hx_args=(), S = dot3(H, P, H.T) + R K = dot3(P, H.T, linalg.inv (S)) + self._K = K hx = Hx(x, *hx_args) y = residual(z, hx) + self._y = y self._x = x + dot(K, y) I_KH = self._I - dot(K, H)