unscented.py 文件源码

def unscentedTransform(X, Wm, Wc, f):
    Y = None
    Ymean = None
    fdim = None
    N = np.shape(X)[1]
    for j in range(0,N):
        fImage = f(X[:,j])
        if Y is None:
            fdim = np.size(fImage)
            Y = np.zeros((fdim, np.shape(X)[1]))
            Ymean = np.zeros(fdim)
        Y[:,j] = fImage
        Ymean += Wm[j] * Y[:,j]
    Ycov = np.zeros((fdim, fdim))
    for j in range(0, N):
        meanAdjustedYj = Y[:,j] - Ymean
        Ycov += np.outer(Wc[j] * meanAdjustedYj, meanAdjustedYj)
    return Y, Ymean, Ycov