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)