def integrate(dt, phase, orientation):
""" Integrate motion to produce new orientation """
phase += 2*pi*dt*Omegaf
omega = Omega*cos(phase)
delta = (np.eye(3), rotations.hat_map(dt*omega))
orientation = SO3.multiply(orientation, SO3.expmap(delta))
return phase, omega, orientation
def accelerometer(orientation):
""" Produce gravity vector from orientation matrix """
acceleration = SO3.multiply(SO3.inverse(orientation), Gravity)
return acceleration
