def __init__(self, ReadFromArduino_instance, dt, offset_GYRO, verbose=0): self.dt = dt self.verbose = verbose self.ReadFromArduino_instance = ReadFromArduino_instance self.list_measurements = [] self.offset_GYRO = offset_GYRO # initial state: assume resting, Z downwards, still -------------------- # no rotation to Earth referential self.o = qt.normalise_quaternion( qt.Quaternion(1, 0.0001, 0.0001, 0.0001)) # reference vectors; can be used to print assumed orientation ---------- self.X_IMU_ref_IMU = qt.Vector(1, 0, 0) self.Y_IMU_ref_IMU = qt.Vector(0, 1, 0) self.Z_IMU_ref_IMU = qt.Vector(0, 0, 1) self.read_and_update_measurement() self.cycle = 0
import GWFrames import Quaternions # Read in the data files print("Reading waveforms from data files...") Ws = [None]*len(Files) for i,File in enumerate(Files): print("\tReading '{0}'".format(File)) Ws[i] = GWFrames.ReadFromNRAR(File) print("Finished!\n") W_0 = Ws[0] # Align to original Waveform print("Aligning all following waveforms to first...") xHat = Quaternions.Quaternion(0,1,0,0) # R0_mid = Quaternions.Squad(W_0.Frame(), W_0.T(), [tmid])[0] for i,W in enumerate(Ws[1:]): print("\tAligning {0} of {1}...".format(i+1, len(Ws[1:]))) GWFrames.AlignWaveforms(W_0, W, t1, t2) # # Rotate by pi if necessary # Ri_mid = Quaternions.Squad(W.Frame(), W.T(), [tmid])[0] # if((R0_mid*xHat*R0_mid.inverse()).dot(Ri_mid*xHat*Ri_mid.inverse())<0) : # W.RotateDecompositionBasis(Quaternions.exp(Quaternions.Quaternion(0,0,0,pi/2))) # # Align corotating frame of `W[1:]` to corotating frame of `W[0]` # W.AlignTimeAndFrame(W_0, t1, t2); Diff = W_0.Compare(W) plot(Diff.T(), Quaternions.angle(Diff.Frame())) print("Finished!\n")
def Vf_from_quat(q): qvf = q * Quaternions.Quaternion(0,0,0,1) * q.conjugate() return np.array([qvf[1],qvf[2],qvf[3]])