diff --git a/filterpy/kalman/UKF.py b/filterpy/kalman/UKF.py index a43bbb1..2a28b4f 100644 --- a/filterpy/kalman/UKF.py +++ b/filterpy/kalman/UKF.py @@ -515,22 +515,22 @@ def rts_smoother(self, Xs, Ps, Qs=None, dt=None): Pb = 0 x = Xs[k] for i in range(num_sigmas): - y = sigmas_f[i] - x + y = self.residual_x(sigmas_f[i], x) Pb += self.Wc[i] * outer(y, y) Pb += Qs[k] # compute cross variance Pxb = 0 for i in range(num_sigmas): - z = sigmas[i] - Xs[k] - y = sigmas_f[i] - xb + z = self.residual_x(sigmas[i], Xs[k]) + y = self.residual_x(sigmas_f[i], xb) Pxb += self.Wc[i] * outer(z, y) # compute gain K = dot(Pxb, inv(Pb)) # update the smoothed estimates - xs[k] += dot (K, xs[k+1] - xb) + xs[k] += dot (K, self.residual_x(xs[k+1], xb)) ps[k] += dot3(K, ps[k+1] - Pb, K.T) Ks[k] = K