def test_consistency(self):
"""
Quaternions: Convert to and from quaternions and check consistency
"""
for i in range(100):
a_0 = quaternion.euler_fix_quadrant(2 * np.pi * np.random.rand(3))
n_0 = np.random.rand(3)
dn_0 = np.random.rand(3)
v_0 = np.random.rand(3)
# check that rotation matrix conversion is internally consistent
r_1 = quaternion.euler_to_rot_mat(a_0)
q_1 = quaternion.from_rot_mat(r_1)
r_2 = quaternion.to_rot_mat(q_1)
self.assertTrue(
np.isclose(r_1, r_2).all(),
f"Rotation matrices mismatch\n{r_1}\n{r_2}")
# check that transform is correct
v_0_t = quaternion.transform(v_0, q_1)
v_1 = quaternion.transform_inv(v_0_t, q_1)
v_2 = quaternion.transform(v_0_t, quaternion.conjugate(q_1))
self.assertTrue(
np.isclose(v_0, v_1).all(),
f"Transform mismatch\n{v_0}\n{v_1}")
self.assertTrue(
np.isclose(v_0, v_2).all(),
f"Conjugate transform mismatch\n{v_0}\n{v_2}")
w_b_1 = quaternion.rate_matrix_body(q_1)
o_b_1 = quaternion.from_angular_velocity(w_b_1, n_0)
n_1 = quaternion.to_angular_velocity(w_b_1, o_b_1)
self.assertTrue(
np.isclose(n_0, n_1).all(),
f"Angular velocity mismatch\n{n_0}\n{n_1}")
w_w_1 = quaternion.rate_matrix_world(q_1)
o_w_1 = quaternion.from_angular_velocity(w_w_1, n_0)
n_2 = quaternion.to_angular_velocity(w_w_1, o_w_1)
self.assertTrue(
np.isclose(n_0, n_2).all(),
f"Angular velocity mismatch\n{n_0}\n{n_2}")
l_b_1 = quaternion.from_angular_acceleration(w_b_1, dn_0)
dn_1 = quaternion.to_angular_acceleration(w_b_1, l_b_1)
self.assertTrue(
np.isclose(dn_0, dn_1).all(),
f"Angular acceleration mismatch\n{dn_0}\n{dn_1}")
l_w_1 = quaternion.from_angular_acceleration(w_w_1, dn_0)
dn_2 = quaternion.to_angular_acceleration(w_w_1, l_w_1)
self.assertTrue(
np.isclose(dn_0, dn_2).all(),
f"Angular acceleration mismatch\n{dn_0}\n{dn_2}")
def filter_translational(
sensor_state: Sensor,
filter_state: Filter,
u: np.ndarray,
dt: float,
):
# get working variables
s_est = filter_state.s
accel = sensor_state.accelerometer
x = s_est[:3]
v = s_est[7:10]
q = s_est[3:7]
# dead reckoning
g = quaternion.transform(accel, q) - np.array([0., 0., G])
# integrate
x = x + v * dt + 0.5 * dt**2 * g
v = v + g * dt
# estimate translational velocity based on xy accel
v_cd_xy_est = utils.signed_sqrt(-2 * m * accel[:2] / rho / cd_xy)
v_cd_est = quaternion.transform_inv(
np.array([v_cd_xy_est[0], v_cd_xy_est[1], 0.]), q)
v[:2] = v[:2] + (v_cd_est[:2] - v[:2]) * V_CD_GAIN
return x, v, g
def calc_translational_derivative(s: np.ndarray, u: np.ndarray):
x = s[:3] # translational position (world to body)
q = s[3:7] # quaternion (world to body)
v = s[7:10] # translational velocity (world)
f_motor = s[13:17] # motor forces
# convert v into body frame
v_body = quaternion.transform_inv(v, q)
# calculate contributions to translational force
f_gravity_w = m * np.array([0., 0., -G])
cd = np.array([cd_xy, cd_xy, cd_z])
f_drag_b = -0.5 * rho * np.multiply(cd, np.multiply(
v_body, np.abs(v_body)))
f_motor_b = np.array([0., 0., f_motor.sum()])
if x[2] < 0:
f_ground_w = np.array([0., 0., -k_ground * x[2] - d_ground * v[2]])
else:
f_ground_w = np.array([0., 0., 0.])
# calculate translational acceleration
g = (1. / m) * (f_gravity_w + f_ground_w +
quaternion.transform(f_drag_b + f_motor_b, q))
return g
def test_baseline(self):
"""
Quaternions: Compare transformations to baseline
"""
x = np.array([0., 1., 0.])
angles = np.array([
np.pi / 2., np.pi / 2., np.pi / 2., np.pi / 8., np.pi / 8.,
np.pi / 8.
])
i_axis = np.array([0, 1, 2, 2, 1, 0])
x_baseline = np.array([
[0., 0., 1.],
[1., 0., 0.],
[0., 1., 0.],
[-0.382683, 0.923880, 0.],
[-0.353553, 0.923880, 0.146447],
[-0.353553, 0.797511, 0.488852],
])
for i, a, i_axis in zip(range(0, len(angles)), angles, i_axis):
# rotate by angle a about axis i
q = np.concatenate((np.array([np.cos(a / 2.)]),
np.sin(a / 2.) * np.eye(3)[i_axis]))
x = quaternion.transform(x, q)
self.assertTrue(
np.isclose(x, x_baseline[i, :]).all(),
f"Transform vs baseline\n{x}\n{x_baseline[i, :]}")
def madgwick(sensor_state: Sensor, filter_state: Filter, dt: float):
# get working variables
q = filter_state.s[3:7]
gain = filter_state.r_madgwick_gain
g_accel = sensor_state.accelerometer
n_gyro = sensor_state.gyroscope
e_magnet = sensor_state.magnetometer
# find length of vectors
g_norm = utils.norm2(g_accel)
e_norm = utils.norm2(e_magnet)
if (g_norm > 0.01) and (e_norm > 0.01):
# normalize
g_accel = g_accel / g_norm
e_magnet = e_magnet / e_norm
h = quaternion.transform(e_magnet, q)
b = np.array([0., utils.norm2(h[0:2]), 0., h[2]])
# gradient descent step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - g_accel[0],
2 * (q[0] * q[1] + q[2] * q[3]) - g_accel[1],
2 * (0.5 - q[1]**2 - q[2]**2) - g_accel[2],
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2]) - e_magnet[0],
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3]) - e_magnet[1],
2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
(0.5 - q[1]**2 - q[2]**2) - e_magnet[2],
])
j = np.array([
[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0],
[
-2 * b[3] * q[2], 2 * b[3] * q[3],
-4 * b[1] * q[2] - 2 * b[3] * q[0],
-4 * b[1] * q[3] + 2 * b[3] * q[1]
],
[
-2 * b[1] * q[3] + 2 * b[3] * q[1],
2 * b[1] * q[2] + 2 * b[3] * q[0],
2 * b[1] * q[1] + 2 * b[3] * q[3],
-2 * b[1] * q[0] + 2 * b[3] * q[2]
],
[
2 * b[1] * q[2], 2 * b[1] * q[3] - 4 * b[3] * q[1],
2 * b[1] * q[0] - 4 * b[3] * q[2], 2 * b[1] * q[1]
],
])
# get step update from accelerometer and magnetometer
o_step = -1 * gain * utils.normalize(j.T @ f)
w_body = quaternion.rate_matrix_body(q)
n_step = quaternion.to_angular_velocity(w_body, o_step)
else:
n_step = np.zeros(3)
# integrate
n_body = n_gyro + n_step
q = quaternion.integrate(q, n_body, dt)
return q, n_body