ukf.py 文件源码

def update(self, z):
        sigmas_f, sigmas_h = self.sigmas_f, self.sigmas_h

        for i in range(self.M.num_sigmas):
            sigmas_h[i] = self.h(sigmas_f[i])

        zp, Pz = self.unscented_transform(sigmas_h, self.M.Wm, self.M.Wc, self._R)

        Pxz = np.zeros((self._n, self._k))
        for i in range(self.M.num_sigmas):
            Pxz += self.M.Wc[i] * np.outer(sigmas_f[i] - self.xp, sigmas_h[i] - zp)

        K = Pxz.dot(inv(Pz)) # Kalman gain

        self._x = self.xp + K.dot(z-zp)
        self._P = self.Pp - K.dot(Pz).dot(K.T)