ukf.py 文件源码

def sigma_points(self, X, P):
        sigmas = np.zeros((2 * self.n + 1, self.n))
        U = cholesky((self.n + self.l) * P)

        sigmas[0] = X
        for k in range (self.n):
            sigmas[k + 1]   = X + U[k]
            sigmas[self.n + k + 1] = X - U[k]

        return sigmas