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