def planar_ctrl_law(e_p_2d, e_v_2d):
e_p = np.array([e_p_2d[0], e_p_2d[1], 0.])
e_v = np.array([e_v_2d[0], e_v_2d[1], 0.])
norm_e_p = utils.norm2(e_p)
norm_e_v = utils.norm2(e_v)
if norm_e_p > utils.EPS:
# calculate axis to rotate about
axis_p = utils.cross(np.array([0., 0., 1.]), e_p / norm_e_p)
# calculate angle target
theta_p = utils.smooth_symmetric_clip(K_P * norm_e_p, a_clip=THETA_MAX)
q_p = quaternion.from_axis_angle(axis_p, theta_p)
if (norm_e_p < utils.EPS) and norm_e_v > utils.EPS:
# damping terms
axis_v = utils.cross(np.array([0., 0., 1.]), e_v / norm_e_v)
theta_v = utils.smooth_symmetric_clip(K_V * norm_e_v, a_clip=THETA_MAX)
q_v = quaternion.from_axis_angle(axis_v, theta_v)
else:
q_v = np.array([1., 0., 0., 0.])
# get resultant quaternion
q_xy = quaternion.product(q_p, q_v)
axis_tot, theta_tot = quaternion.to_axis_angle(q_xy)
# saturate angle to limit
theta_clip = utils.smooth_symmetric_clip(theta_tot, a_clip=THETA_MAX)
q_xy = quaternion.from_axis_angle(axis_tot, theta_clip)
else:
q_xy = np.array([1., 0., 0., 0.])
return q_xy
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 test_norm2(self):
"""
Utilities: Vector two norm
"""
for i in range(10):
x = 10. * np.random.rand(3)
x_norm = utils.norm2(x)
self.assertAlmostEqual(np.multiply(x, x).sum(), x_norm**2, 4)
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 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 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