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