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:]))