diff --git a/filterpy/kalman/CubatureKalmanFilter.py b/filterpy/kalman/CubatureKalmanFilter.py index 74018763..d36eb911 100644 --- a/filterpy/kalman/CubatureKalmanFilter.py +++ b/filterpy/kalman/CubatureKalmanFilter.py @@ -318,9 +318,9 @@ def predict(self, dt=None, fx_args=()): # evaluate cubature points for k in range(self._num_sigmas): - self.sigmas_f[k] = self.fx(sigmas[k], dt, *fx_args) + sigmas_f[k] = self.fx(sigmas[k], dt, *fx_args) - self.x, self.P = ckf_transform(self.sigmas_f, self.Q) + self.x, self.P = ckf_transform(sigmas_f, self.Q) # save prior self.x_prior = self.x.copy() @@ -359,6 +359,8 @@ def update(self, z, R=None, hx_args=()): elif isscalar(R): R = eye(self.dim_z) * R + self.sigmas_f = spherical_radial_sigmas(self.x, self.P) + for k in range(self._num_sigmas): self.sigmas_h[k] = self.hx(self.sigmas_f[k], *hx_args)