def checkDoubleIntegral(method, function, integral, doubleIntegral):
estimated_integral = np.empty_like(x)
integrator = integrators.DoubleIntegrator(doubleIntegral(0), integral(0),
method)
for i, s in enumerate(x[1:]):
estimated_integral[i + 1] = integrator(function(s), dt)
assert_vectors_correlated(estimated_integral[1:], doubleIntegral(x[1:]))
def checkDoubleIntegral(method, function, integral, doubleIntegral):
estimated_integral = np.empty_like(x)
integrator = integrators.DoubleIntegrator(doubleIntegral(0),integral(0),
method)
for i,s in enumerate(x[1:]):
estimated_integral[i+1] = integrator(function(s), dt)
assert_vectors_correlated(estimated_integral[1:], doubleIntegral(x[1:]))
def checkTrajectory(T, truePositions, trueRotations):
"""
Check the outputs of a trajectory model agree with truth values.
@param T: Trajectory to check.
@param truePositions: L{TimeSeries} of true position values.
@param trueRotations: L{TimeSeries} of true rotation values.
"""
# Get time indices at which position comparisons valid
t = truePositions.timestamps
validity = (t >= T.startTime) & (t <= T.endTime)
t = t[validity]
dt = np.gradient(t)
p = truePositions.values[:, validity]
# Check position
assert_vectors_correlated(T.position(t), p)
# Check velocity
v = np.array(map(np.gradient, p)) / dt
assert_vectors_correlated(T.velocity(t[2:-2]), v[:, 2:-2])
# Check acceleration
a = np.array(map(np.gradient, v)) / dt
assert_vectors_correlated(T.acceleration(t[4:-4]), a[:, 4:-4])
# Get time indices at which rotation comparisons valid
t = trueRotations.timestamps
validity = (t >= T.startTime) & (t <= T.endTime)
t = t[validity]
r = trueRotations.values[validity]
# Check rotation
assertQuaternionAlmostEqual(T.rotation(t), r, tol=0.05)
# Check angular velocity
r, lastR = r[1:], r[:-1]
t, dt = t[1:], np.diff(t)
diffOmega = (2 * (r - lastR) * lastR.conjugate).array.T[1:] / dt
trajOmega = T.rotationalVelocity(t - dt / 2)
assert_vectors_correlated(trajOmega[:, 2:-2], diffOmega[:, 2:-2])
# Check angular acceleration
diffAlpha = np.array(map(np.gradient, diffOmega)) / dt
trajAlpha = T.rotationalAcceleration(t - dt / 2)
assert_vectors_correlated(trajAlpha[:, 4:-4], diffAlpha[:, 4:-4])
def checkTrajectory(T, truePositions, trueRotations):
"""
Check the outputs of a trajectory model agree with truth values.
@param T: Trajectory to check.
@param truePositions: L{TimeSeries} of true position values.
@param trueRotations: L{TimeSeries} of true rotation values.
"""
# Get time indices at which position comparisons valid
t = truePositions.timestamps
validity = (t >= T.startTime) & (t <= T.endTime)
t = t[validity]
dt = np.gradient(t)
p = truePositions.values[:,validity]
# Check position
assert_vectors_correlated(T.position(t), p)
# Check velocity
v = np.array(map(np.gradient, p)) / dt
assert_vectors_correlated(T.velocity(t[2:-2]), v[:,2:-2])
# Check acceleration
a = np.array(map(np.gradient, v)) / dt
assert_vectors_correlated(T.acceleration(t[4:-4]), a[:,4:-4])
# Get time indices at which rotation comparisons valid
t = trueRotations.timestamps
validity = (t >= T.startTime) & (t <= T.endTime)
t = t[validity]
r = trueRotations.values[validity]
# Check rotation
assertQuaternionAlmostEqual(T.rotation(t), r, tol=0.05)
# Check angular velocity
r, lastR = r[1:], r[:-1]
t, dt = t[1:], np.diff(t)
diffOmega = (2 * (r - lastR) * lastR.conjugate).array.T[1:] / dt
trajOmega = T.rotationalVelocity(t - dt/2)
assert_vectors_correlated(trajOmega[:,2:-2], diffOmega[:,2:-2])
# Check angular acceleration
diffAlpha = np.array(map(np.gradient, diffOmega)) / dt
trajAlpha = T.rotationalAcceleration(t - dt/2)
assert_vectors_correlated(trajAlpha[:,4:-4], diffAlpha[:,4:-4])
def checkIntegral(method, function, integral):
estimated_integral = np.empty_like(x)
integrator = method(integral(0))
for i, s in enumerate(x[1:]):
estimated_integral[i + 1] = integrator(function(s), dt)
assert_vectors_correlated(estimated_integral[1:], integral(x[1:]))
def checkIntegral(method, function, integral):
estimated_integral = np.empty_like(x)
integrator = method(integral(0))
for i,s in enumerate(x[1:]):
estimated_integral[i+1] = integrator(function(s), dt)
assert_vectors_correlated(estimated_integral[1:], integral(x[1:]))
