def test_gyros_plot(self):
"""
Plot gyros versus time.
"""
gyros = np.array(map(self.fixture.gyros, self.time))
leg_a = ['wx', 'wy', 'wz']
plot_trinity(self.time, gyros, lgnd=leg_a)
def test_ned_acceleration(self):
self.skipTest('temp')
an = map(self.profile.an, self.time)
ae = map(self.profile.ae, self.time)
ad = map(self.profile.ad, self.time)
lgnd = ['an', 'ae', 'ad']
plot_trinity(self.time,np.array([an, ae, ad]).T, lgnd)
def test_gyro(self):
"""
Plot gyros angular velocity.
"""
self.skipTest('temp')
gyro = map(self.profile.gyros, self.time)
lgnd = ['wx', 'wy', 'wz']
print np.shape(gyro)
plot_trinity(self.time, gyro, lgnd)
def test_orientation(self):
"""
Plot Euler angles versus time.
"""
self.skipTest('temp')
gamma = map(self.profile.gamma, self.time)
theta = map(self.profile.theta, self.time)
psi = map(self.profile.psi, self.time)
lgnd = ['gamma', 'theta', 'psi']
plot_trinity(self.time,np.array([gamma, theta, psi]).T, lgnd)
def test_ned_velocity(self):
"""
Plot NED velocity versus time
"""
vn = map(self.profile.vn, self.time)
ve = map(self.profile.ve, self.time)
vd = map(self.profile.vd, self.time)
lgnd = ['vn', 've', 'vd']
plot_trinity(self.time, np.array([vn, ve, vd]).T, lgnd)
def test_run(self):
"""
Test calibration table movement.
"""
time = np.arange(0., 100., 1.)
a = zip(*map(self.rs.run, time))
#a = np.array([a])
leg_a = ['roll', 'pitch', 'yaw']
angle = [dcm2euler(dcm) for dcm in a[0]]
plot_trinity(time, angle, lgnd=leg_a)
leg_a = ['wx', 'wy', 'wz']
plot_trinity(time, a[1], lgnd=leg_a)
def test_accs_plot(self):
"""
Plot acceleration versus time.
"""
accs1 = np.array(map(self.fixture1.accs, self.time))
leg_a = ['ax-f1', 'ay', 'az']
plot_trinity(self.time, accs1, lgnd=leg_a)
accs2 = np.array(map(self.fixture2.accs, self.time))
leg_a = ['ax-f2', 'ay', 'az']
plot_trinity(self.time, accs2, lgnd=leg_a)
accs3 = np.array(map(self.fixture3.accs, self.time))
leg_a = ['ax-f3', 'ay', 'az']
plot_trinity(self.time, accs3, lgnd=leg_a)
def test_gyros_plot(self):
"""
Plot gyros versus time.
"""
gyros1 = np.array(map(self.fixture1.gyros, self.time))
leg_a = ['wx-f1', 'wy', 'wz']
plot_trinity(self.time, gyros1, lgnd=leg_a)
#
gyros2 = np.array(map(self.fixture2.gyros, self.time))
leg_a = ['wx-f2', 'wy', 'wz']
plot_trinity(self.time, gyros2, lgnd=leg_a)
gyros3 = np.array(map(self.fixture3.gyros, self.time))
leg_a = ['wx-f3', 'wy', 'wz']
plot_trinity(self.time, gyros3, lgnd=leg_a)
def setUp(self):
self.turn_time = 5
self.stady_time = 20
self.dt = 0.05
#Rotation set 1
self.rs1 = RotationSequence()
self.rs1.set_init_orientation(0., 0., 0. )
self.rs1.add(RotateIMU(self.stady_time, 0., 0., 0.))
self.rs1.add(RotateIMU(self.turn_time, np.pi, 0., 0.))
self.rs1.add(RotateIMU(self.stady_time, 0., 0., 0.))
self.rs1.add(RotateIMU(self.turn_time, np.pi, 0., 0.))
self.rs1.add(RotateIMU(self.stady_time, 0., 0., 0.))
self.rs1.add(RotateIMU(self.turn_time, 0., 0., np.pi))
self.rs1.add(RotateIMU(self.stady_time, 0., 0., 0.))
# self.rs1.add(RotateIMU(self.turn_time, 0., 0., np.pi))
# self.rs1.add(RotateIMU(15.*self.stady_time, 0., 0., 0.))
# self.rs1.add(RotateIMU(self.turn_time, 0., 0., np.pi))
# self.rs1.add(RotateIMU(15.*self.stady_time, 0., 0., 0.))
#Rotation set 2
self.rs2 = RotationSequence()
self.rs2.set_init_orientation( -np.pi/2., -np.pi/2, 0. )
self.rs2.add(RotateIMU(self.stady_time, 0., 0., 0.))
self.rs2.add(RotateIMU(self.turn_time, 0., np.pi, 0.))
self.rs2.add(RotateIMU(self.stady_time, 0., 0., 0.))
self.rs2.add(RotateIMU(self.turn_time, 0., np.pi, 0.))
self.rs2.add(RotateIMU(self.stady_time, 0., 0., 0.))
self.rs2.add(RotateIMU(self.turn_time, np.pi, 0., 0.))
self.rs2.add(RotateIMU(self.stady_time, 0., 0., 0.))
# self.rs2.add(RotateIMU(self.turn_time, np.pi, 0., 0.))
# self.rs2.add(RotateIMU(15.*self.stady_time, 0., 0., 0.))
# self.rs2.add(RotateIMU(self.turn_time, np.pi, 0., 0.))
# self.rs2.add(RotateIMU(15*self.stady_time, 0., 0., 0.))
#Rotation set 3
self.rs3 = RotationSequence()
self.rs3.set_init_orientation(np.pi/2, 0., np.pi/2)
self.rs3.add(RotateIMU(self.stady_time, 0., 0., 0.))
self.rs3.add(RotateIMU(self.turn_time, 0., 0., np.pi))
self.rs3.add(RotateIMU(self.stady_time, 0., 0., 0.))
self.rs3.add(RotateIMU(self.turn_time, 0., 0., np.pi))
self.rs3.add(RotateIMU(self.stady_time, 0., 0., 0.))
self.rs3.add(RotateIMU(self.turn_time, 0., np.pi, 0.))
self.rs3.add(RotateIMU(self.stady_time, 0., 0., 0.))
# self.rs3.add(RotateIMU(self.turn_time, 0., np.pi, 0.))
# self.rs3.add(RotateIMU(15*self.stady_time, 0., 0., 0.))
# self.rs3.add(RotateIMU(self.turn_time, 0., np.pi, 0.))
# self.rs3.add(RotateIMU(15*self.stady_time, 0., 0., 0.))
self.fixture1 = BasicCalibTraj(self.rs1)
self.fixture2 = BasicCalibTraj(self.rs2)
self.fixture3 = BasicCalibTraj(self.rs3)
self.time = np.arange(0., 150., self.dt)
to_rads = (np.pi/180.)/3600.
shape = len(self.time)
# 0.000001454
gyro_rand1 = (0.15e-05)*np.random.randn(shape,3)
gyro_rand2 = (0.15e-05)*np.random.randn(shape,3)
gyro_rand3 = (0.15e-05)*np.random.randn(shape,3)
acc_rand1 = 0.0005*np.random.randn(shape,3)
acc_rand2 = 0.0005*np.random.randn(shape,3)
acc_rand3 = 0.0005*np.random.randn(shape,3)
gyros1 = np.array(map(self.fixture1.gyros, self.time)) + gyro_rand1
gyros2 = np.array(map(self.fixture2.gyros, self.time)) + gyro_rand2
gyros3 = np.array(map(self.fixture3.gyros, self.time)) + gyro_rand3
accs1 = np.array(map(self.fixture1.accs, self.time)) + 0*acc_rand1
accs2 = np.array(map(self.fixture2.accs, self.time)) + 0*acc_rand2
accs3 = np.array(map(self.fixture3.accs, self.time)) + 0*acc_rand3
self.gyro_model = {'x': 3e-07, 'y':5e-07, 'z':7e-07,
'xx':0.0001, 'xy':0.0002, 'xz':0.0003,
'yx':0.0004, 'yy':0.0005, 'yz':0.0006,
'zx':0.0007, 'zy':0.0008, 'zz':0.0009,}
# acc parameters: bias and scalefactor, misalignment matrix
self.acc_model = {'x': 0.003, 'y':0.004, 'z':0.005,
'xx':0.001, 'xy':0.002, 'xz':0.000,
'yx':0.000, 'yy':0.003, 'yz':0.000,
'zx':0.004, 'zy':0.005, 'zz':0.006,}
gsfma = np.array([[self.gyro_model['xx'], self.gyro_model['xy'], self.gyro_model['xz']],
[self.gyro_model['yx'], self.gyro_model['yy'], self.gyro_model['yz']],
[self.gyro_model['zx'], self.gyro_model['zy'], self.gyro_model['zz']]])
gb = np.array([self.gyro_model['x'],
self.gyro_model['y'],
self.gyro_model['z']])
asfma = np.array([
[self.acc_model['xx'], self.acc_model['xy'], self.acc_model['xz']],
[self.acc_model['yx'], self.acc_model['yy'], self.acc_model['yz']],
[self.acc_model['zx'], self.acc_model['zy'], self.acc_model['zz']]])
ab = np.array([self.acc_model['x'],
self.acc_model['y'],
self.acc_model['z']])
# add scalefactor, misalignment and bias to accs and gyros
gyros1 = (np.dot(gyros1, gsfma.T + np.eye(3)) + gb)*self.dt
gyros2 = (np.dot(gyros2, gsfma.T + np.eye(3)) + gb)*self.dt
gyros3 = (np.dot(gyros3, gsfma.T + np.eye(3)) + gb)*self.dt
accs1 = (np.dot(accs1, asfma.T + np.eye(3)) + ab)
accs2 = (np.dot(accs2, asfma.T + np.eye(3)) + ab)
accs3 = (np.dot(accs3, asfma.T + np.eye(3)) + ab)
self.data_set1 = np.concatenate((gyros1, accs1), axis=1)
self.data_set2 = np.concatenate((gyros2, accs2), axis=1)
self.data_set3 = np.concatenate((gyros3, accs3), axis=1)
self.mc = DieselCalibrator()
self.mc.dt = self.dt
self.mc.time = self.time
self.mc.load_data(self.data_set1, self.data_set2, self.data_set3)
t0 = 20./self.dt -self.dt
t1 = 0./self.dt
t2 = (20. + 5.)/self.dt
t3 = (20. + 5. + 20. + 5.)/self.dt
t4 = (20. + 5. + 20. + 5. + 20. + 5.)/self.dt
# schedule of rotations
# t0: time shift
tbl = np.array([[t0, t1, t2, t3, t4],
[t0, t1, t2, t3, t4],
[t0, t1, t2, t3, t4]])
# dv1, dv2, dv3 = self.mc.calc_coefficient(tbl)
# self.mc.gyro_report()
dv1, dv2, dv3 = self.mc.calc_coefficient(tbl)
# dv1, dv2, dv3 = self.mc.calc_coefficient(tbl)
# dv1, dv2, dv3 = self.mc.calc_coefficient(tbl)
# dv1, dv2, dv3 = self.mc.calc_coefficient(tbl)
# dv1, dv2, dv3 = self.mc.calc_coefficient(tbl)
# dv1, dv2, dv3 = self.mc.calc_coefficient(tbl)
# dv1, dv2, dv3 = self.mc.calc_coefficient(tbl)
plot_trinity(self.time, dv1.T)
plot_trinity(self.time, dv2.T)
plot_trinity(self.time, dv3.T)
self.mc.gyro_report()
self.delta = 0.05
def test_accs(self):
self.skipTest('temp')
a = map(self.profile.accs, self.time)
lgnd = ['ax', 'ay', 'az']
plot_trinity(self.time, a, lgnd)