def imu2body(dF, pos=[0, 0, 0]):
acc = dF.to_numpy()[:,3:]
gyr = dF.to_numpy()[:,:3]
acc_c = rate2acc(gyr, pos)
grv = np.array([[0],[0],[-9.81]])
q0=ahrs.Quaternion(acc2q(acc[0]))
imu = ahrs.filters.Complementary(acc=acc, gyr=gyr, frequency=fs, q0=q0, gain=0.001)
theta = ahrs.QuaternionArray(imu.Q)
th = ahrs.QuaternionArray(imu.Q).to_angles()
acc_cc = np.zeros_like(acc)
for ii in range(len(acc)):
acc_cc[ii,:] = acc[ii,:] + ahrs.Quaternion(imu.Q[ii]).rotate(grv).T
a = acc_cc
v = FDI(a)
d = FDI(v)
om = gyr
def setUp(self) -> None:
# Generate random attitudes
num_samples = 500
angular_velocities = random_angvel(num_samples=num_samples,
span=(-np.pi, np.pi))
self.R = ahrs.QuaternionArray(
ahrs.filters.AngularRate(angular_velocities).Q).to_DCM()
# Rotated reference vectors + noise
self.noise_sigma = 1e-5
self.Rg = np.array([R @ REFERENCE_GRAVITY_VECTOR
for R in self.R]) + np.random.standard_normal(
(num_samples, 3)) * self.noise_sigma
self.Rm = np.array([R @ REFERENCE_MAGNETIC_VECTOR
for R in self.R]) + np.random.standard_normal(
(num_samples, 3)) * self.noise_sigma
def setUp(self) -> None:
# Create random attitudes
num_samples = 1000
self.Qts = ahrs.QuaternionArray(
np.random.random((num_samples, 4)) - 0.5)
self.rotations = self.Qts.to_DCM()
# Add noise to reference vectors and rotate them by the random attitudes
noises = np.random.randn(2 * num_samples, 3) * 1e-3
self.Rg = np.array([
R.T @ (np.array([0.0, 0.0, GRAVITY]) + noises[i])
for i, R in enumerate(self.rotations)
])
self.Rm = np.array([
R.T @ (REFERENCE_MAGNETIC_VECTOR + noises[i + num_samples])
for i, R in enumerate(self.rotations)
])
self.decimal_precision = 7e-2
def setUp(self) -> None:
# Create random attitudes
a_ref = np.array([0.0, 0.0, -NORMAL_GRAVITY])
m_ref = REFERENCE_MAGNETIC_VECTOR
num_samples = 1000
angular_velocities = random_angvel(num_samples=num_samples,
span=(-np.pi, np.pi))
self.Qts = ahrs.QuaternionArray(
ahrs.filters.AngularRate(angular_velocities).Q)
rotations = self.Qts.to_DCM()
# Add noise to reference vectors and rotate them by the random attitudes
self.noise_sigma = 1e-5
self.Rg = np.array([R.T @ a_ref
for R in rotations]) + np.random.standard_normal(
(num_samples, 3)) * self.noise_sigma
self.Rm = np.array([R.T @ m_ref
for R in rotations]) + np.random.standard_normal(
(num_samples, 3)) * self.noise_sigma
def setUp(self) -> None:
# Create random attitudes
num_samples = 1000
angular_velocities = random_angvel(num_samples=num_samples,
span=(-np.pi, np.pi))
self.Qts = ahrs.QuaternionArray(
ahrs.filters.AngularRate(angular_velocities).Q)
self.rotations = self.Qts.to_DCM()
# Add noise to reference vectors and rotate them by the random attitudes
noise_sigma = 1e-5
self.decimal_precision = noise_sigma * 10.0
self.Rg = np.array(
[R.T @ REFERENCE_GRAVITY_VECTOR
for R in self.rotations]) + np.random.standard_normal(
(num_samples, 3)) * noise_sigma
self.Rm = np.array(
[R.T @ REFERENCE_MAGNETIC_VECTOR
for R in self.rotations]) + np.random.standard_normal(
(num_samples, 3)) * noise_sigma
def imu2body(df, t, fs, pos=[0, 0, 0]):
gyr = df[:, 0:3]
acc = df[:, 3:]
grv = np.array([[0], [0], [-9.81]])
alpha = FDD(gyr)
accc = acc + np.cross(gyr, np.cross(gyr, pos)) + np.cross(alpha, pos)
q0 = ahrs.Quaternion(ahrs.common.orientation.acc2q(accc[0]))
imu = ahrs.filters.Complementary(acc=accc,
gyr=gyr,
frequency=fs,
q0=q0,
gain=0.001)
theta = ahrs.QuaternionArray(imu.Q).to_angles()
acccc = np.zeros_like(accc)
for ii in range(len(acc)):
acccc[ii, :] = accc[ii, :] + ahrs.Quaternion(imu.Q[ii]).rotate(grv).T
v = FDI(acccc)
d = FDI(v)
ah = {}
ah['Dx'] = d[:, 0]
ah['Dy'] = d[:, 1]
ah['Dz'] = d[:, 2]
ah['Vx'] = v[:, 0]
ah['Vy'] = v[:, 1]
ah['Vz'] = v[:, 2]
ah['Ax'] = acccc[:, 0]
ah['Ay'] = acccc[:, 1]
ah['Az'] = acccc[:, 2]
ah['thx'] = theta[:, 0]
ah['thy'] = theta[:, 1]
ah['thz'] = theta[:, 2]
ah['omx'] = gyr[:, 0]
ah['omy'] = gyr[:, 1]
ah['omz'] = gyr[:, 2]
ah['alx'] = alpha[:, 0]
ah['aly'] = alpha[:, 1]
ah['alz'] = alpha[:, 2]
dataFrame = pd.DataFrame(ah, t)
return dataFrame
def test_quaternion_average(self):
# Create 10 continuous quaternions representing rotations around the
# Z-axis between 0 and 90 degrees and average them. The mean quaternion
# must be, naturally, equal to the 45 degree quaternion.
Q = ahrs.QuaternionArray([ahrs.Quaternion(rpy=np.array([0.0, 0.0, x])*ahrs.DEG2RAD) for x in range(0, 100, 10)])
np.testing.assert_almost_equal(Q.average(), ahrs.Quaternion(rpy=np.array([0.0, 0.0, 45.0])*ahrs.DEG2RAD), decimal=self.decimal_precision)
def setUp(self) -> None:
self.Q0 = ahrs.QuaternionArray()
self.Q1 = ahrs.QuaternionArray(np.identity(4))
self.decimal_precision = 15
