def testUnscentedTransform_linearFunction():
    xBar = np.array([[1]])
    Sigma = np.array([[0.1]])

    def transformFunction(x):
        return 2 * x

    ut = transforms.UnscentedTransform(transformFunction)
    mean, var = ut(xBar, Sigma)

    assert_almost_equal(mean, 2 * xBar)
    assert_almost_equal(var, 2 * Sigma * 2)
def testUnscentedTransform_quadratic():
    xBar = np.array([[2]])
    Sigma = np.array([[0.25]])

    def transformFunction(x):
        return x**2

    ut = transforms.UnscentedTransform(transformFunction)
    mean, var = ut(xBar, Sigma)

    random.seed(RANDOM_SEED)
    x = random.normal(loc=xBar, scale=np.sqrt(Sigma), size=1000)
    X = transformFunction(x)
    checkMeanAndCovariance(mean, var, X)
def checkUnscentedTransform_polarToCartesian(params):
    meanR, meanTheta, varR, varTheta = params

    def transformFunction(params):
        r, theta = params
        return np.vstack(transforms.polarToCartesian(r, theta))

    # calculate results using unscented transform
    ut = transforms.UnscentedTransform(transformFunction)
    mean, cov = ut(np.vstack((meanR, meanTheta)), np.diag((varR, varTheta)))

    # calculate results using monte carlo sim
    random.seed(RANDOM_SEED)
    r = random.normal(loc=meanR, scale=np.sqrt(varR), size=1000)
    theta = random.normal(loc=meanTheta, scale=np.sqrt(varTheta), size=1000)
    cartesianCoords = np.hstack(transformFunction(p) for p in zip(r, theta))
    checkMeanAndCovariance(mean, cov, cartesianCoords)
def testUnscentedTransform_rotation():
    xBar = np.array([[20], [0]])
    Sigma = np.array([[3, 0], [0, 2]])

    r = 1 / np.sqrt(2)
    T = np.array([[r, -r], [r, r]])

    def transformFunction(x):
        x, y = x
        return np.dot(T, np.vstack((x, y)))

    ut = transforms.UnscentedTransform(transformFunction)
    mean, cov = ut(xBar, Sigma)

    random.seed(RANDOM_SEED)
    x = random.normal(loc=xBar[0], scale=np.sqrt(Sigma[0, 0]), size=1000)
    y = random.normal(loc=xBar[1], scale=np.sqrt(Sigma[1, 1]), size=1000)
    transformedPoints = np.hstack(transformFunction(p) for p in zip(x, y))
    checkMeanAndCovariance(mean, cov, transformedPoints)