def run_furgale():
bezier_order = 4
bezier_params = np.random.rand(bezier_order, 3)
bezier_params *= 10
bez = lambda t: bezier(bezier_params, t)
bezderiv = lambda t: np.squeeze(numdifftools.Jacobian(lambda tt: bez(tt))(t))
t0 = 1.23
r0 = cayley(bez(t0))
w0 = angular_velocity_from_cayley_deriv(bez(t0), bezderiv(t0))
print 'Params:'
print bezier_params
print 'Rotation'
print r0
print 'Numeric right:', angular_velocity_right(lambda t: cayley(bez(t)), t0)
print 'Analytic global:', w0
print 'Numeric left:', angular_velocity_left(lambda t: cayley(bez(t)), t0)
print 'Analytic local:', np.dot(r0, w0)
def predict_gyro(bezier_params, gyro_bias, time):
s = zero_offset_bezier(bezier_params, time)
s_deriv = zero_offset_bezier_deriv(bezier_params, time)
orient = cayley(s)
angular_velocity = angular_velocity_from_cayley_deriv(s, s_deriv)
return np.dot(orient, angular_velocity) + gyro_bias