def integrate_der(q: np.ndarray, n_body: np.ndarray, dt: float):
"""
Return the derivative of the integrated quaternion q as a function of previous time step q
and rotational velocity
:param q: quaternion
:param n_body: rotational velocity vector
:param dt: time step
:return: q_der_q and q_der_n
"""
q = utils.normalize(q)
n_body_norm = utils.norm2(n_body)
w_world = rate_matrix_world(q)
q_hat = np.concatenate((
np.array([np.cos(0.5 * n_body_norm * dt)]),
np.sin(0.5 * n_body_norm * dt) * utils.normalize(n_body),
))
q_hat_mat = matrix(q_hat)
# derivative of q with respect to previous time step q
q_der_q = q_hat_mat @ (np.eye(4) - q.reshape(-1,1) @ q.reshape(1,-1))
# derivative of q with respect to rotational velocity
q_der_n = dt * 0.5 * w_world.T
return q_der_q, q_der_n
def to_axis_angle(q: np.ndarray):
"""
Calculate axis rotation by angle from quaternion
:param q: quaternion
:return: axis
:return: angle
"""
q = utils.normalize(q)
theta = 2 * np.arccos(q[0])
axis = utils.normalize(q[1:])
return axis, theta
def conjugate(q: np.ndarray):
"""
Calculate quaternion conjugate
:param q: quaternion
:return: quaternion conjugate
"""
return utils.normalize(np.multiply(q, np.array([1, -1, -1, -1])))
def test_normalize(self):
"""
Utilities: Vector normalize
"""
for i in range(10):
x = 10. * np.random.rand(3)
x_normalized = utils.normalize(x)
x_norm = utils.norm2(x_normalized)
if x_norm > 0.001:
self.assertAlmostEqual(x_norm, 1., 6)
def from_axis_angle(axis: np.ndarray, theta: float):
"""
Calculate quaternion from rotation by angle about axis
:param axis: axis
:param theta: angle
:return: quaternion
"""
return utils.normalize(np.concatenate((np.array([np.cos(theta / 2.)]),
np.sin(theta / 2.) * axis)))
def integrate(q: np.ndarray, n_body: np.ndarray, dt: float):
"""
Calculate integrate single time step unit quaternion
:param q: quaternion
:param n_body: rotational velocity in body frame
:param dt: time step
:return: quaternion at t+1
"""
q = utils.normalize(q)
n_body_norm = utils.norm2(n_body)
q_hat = np.concatenate((
np.array([np.cos(0.5 * n_body_norm * dt)]),
np.sin(0.5 * n_body_norm * dt) * utils.normalize(n_body),
))
q = product(q_hat, q)
return q
def transform_inv(v: np.ndarray, q: np.ndarray):
"""
Calculate quaternion conjugate
:param v: vector
:param q: quaternion
:return: vector inverse-transformed by quaternion
"""
q = utils.normalize(q)
q_conj = conjugate(q)
return transform(v, q_conj, q)
def to_rot_mat(q: np.ndarray):
"""
Calculate rotation matrix from quaternion
:param q: quaternion
:return: rotation matrix
"""
q = utils.normalize(q)
return np.array([
[q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2, 2*(q[1]*q[2] + q[0]*q[3]), 2*(q[1]*q[3] - q[0]*q[2])],
[2*(q[2]*q[1] - q[0]*q[3]), q[0]**2 + q[2]**2 - q[1]**2 - q[3]**2, 2*(q[2]*q[3] + q[0]*q[1])],
[2*(q[3]*q[1] + q[0]*q[2]), 2*(q[3]*q[2] - q[0]*q[1]), q[0]**2 + q[3]**2 - q[1]**2 - q[2]**2],
])
def transform(v: np.ndarray, q: np.ndarray, q_conj: np.ndarray = None):
"""
Calculate quaternion conjugate
:param v: vector
:param q: quaternion
:param q_conj: optional conjugate quaternion
:return: vector transformed by quaternion
"""
q = utils.normalize(q)
if q_conj is None:
q_conj = conjugate(q)
v_pad = np.concatenate((np.array([0.]), v))
return product(q, product(v_pad, q_conj))[1:]
def rot_mat_der(q: np.ndarray, i_q: int = 0, b_inverse: bool = False):
"""
Return the derivative of a rotation matrix wrt given element
:param q: quaternion
:param i_q: derivative with respect to element i_q
:param b_inverse: boolean true to return derivative of inverse rotation matrix
:return: r_der_qi derivative of rotation matrix wrt i_q
"""
q = utils.normalize(q)
u_dv_dt = -2 * q[i_q] * to_rot_mat(q)
if i_q == 0:
v_du_dt = 2 * np.array([
[q[0], q[3], -q[2]],
[-q[3], q[0], q[1]],
[q[2], -q[1], q[0]],
])
elif i_q == 1:
v_du_dt = 2 * np.array([
[q[1], q[2], q[3]],
[q[2], -q[1], q[0]],
[q[3], -q[0], -q[1]],
])
elif i_q == 2:
v_du_dt = 2 * np.array([
[-q[2], q[1], -q[0]],
[q[1], q[2], q[3]],
[q[0], q[3], -q[2]],
])
elif i_q == 3:
v_du_dt = 2 * np.array([
[-q[3], q[0], q[1]],
[-q[0], -q[3], q[2]],
[q[1], q[2], q[3]],
])
else:
return np.nan * np.ones((3, 3))
if b_inverse:
return np.ascontiguousarray(np.add(u_dv_dt, v_du_dt).T)
else:
return np.add(u_dv_dt, v_du_dt)
def find_valid_maxima(image_cube, footprint, exclusion, low_threshold):
'''
Returns a 3D array that is true everywhere that image_cube
has a local maxima except in regions marked for exclusion.
'''
# peak_local_max expects a normalized image (values between 0 and 1)
normalized_image_cube = normalize(image_cube)
maxima = peak_local_max(normalized_image_cube,
indices=False,
min_distance=1,
threshold_rel=0,
threshold_abs=0,
exclude_border=True,
footprint=footprint)
return maxima & (~exclusion) & (image_cube >= low_threshold)
if __name__ == '__main__':
t = np.arange(0, T_MAX, DT)
# target trajectory
tgt = np.zeros((len(t), 4))
tgt[:, 0] = 0. * np.ones_like(t)
tgt[:, 1] = 0. * np.ones_like(t)
tgt[:, 2] = 5. * np.ones_like(t)
psi = 1. * np.pi / 4.
if MODE == "Pitch":
axis = utils.normalize(np.array([0., 1., 0.]))
elif MODE == "Roll":
axis = utils.normalize(np.array([1., 0., 0.]))
elif MODE == "Yaw":
axis = utils.normalize(np.array([0., 0., 1.]))
else:
raise Exception("Unrecognised mode")
# initial conditions
s0 = np.zeros(17)
s0[drone.State.x.value] = 0.
s0[drone.State.y.value] = 0.
s0[drone.State.z.value] = 5.
s0[drone.State.q_0.value] = np.cos(psi / 2.)
s0[drone.State.q_i.value] = np.sin(psi / 2.) * axis[0]
s0[drone.State.q_j.value] = np.sin(psi / 2.) * axis[1]
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
