def quaternion_mean(self, sigma_points):
"""
@params: sigma_points
removes the mean from the quaternion comp of sigma points
"""
error_lim = 1e-2
q_curr = Quaternion()
q_curr.q = self.mu.reshape(4).astype('float64')
while True:
errors_axis_angle = []
for i in range(sigma_points.shape[1]):
qi = Quaternion()
qi.q = sigma_points[:, i]
ei = qi * q_curr.inv()
ei.normalize()
errors_axis_angle.append(ei.vec()) # of axis angle
e_bary = np.average(np.array(errors_axis_angle), axis=0)
e_bary_norm = np.linalg.norm(e_bary)
if e_bary_norm < error_lim:
return q_curr, np.array(errors_axis_angle)
# e_bary - axis angle
e_bary_quat = Quaternion()
e_bary_quat.from_axis_angle(e_bary)
q_curr.normalize()
q_curr = e_bary_quat * q_curr
def set_rotation(
self,
w: Optional[float] = None,
x: Optional[float] = None,
y: Optional[float] = None,
z: Optional[float] = None,
*,
quaternion: Optional[Quaternion] = None,
) -> None:
"""Set the rotation of the transform.
Arguments:
w: The w componet of the roation quaternion.
x: The x componet of the roation quaternion.
y: The y componet of the roation quaternion.
z: The z componet of the roation quaternion.
quaternion: Numpy quaternion representing the rotation.
Examples:
>>> transform = Transform3D([1, 1, 1], [1, 0, 0, 0])
>>> transform.set_rotation(0, 1, 0, 0)
>>> transform
Transform3D(
(translation): Vector3D(1, 1, 1),
(rotation): quaternion(0, 1, 0, 0)
)
"""
if quaternion:
self._rotation = Quaternion(quaternion)
return
self._rotation = Quaternion(w, x, y, z)
def calc_orientation(gyr, acc, samplerate, align_arr, beta, delta_t, times):
ref_qq = np.zeros([len(acc), 4])
ref_eu = np.zeros([len(acc), 3])
sampleperiod = 1 / samplerate
qq = Quaternion(align_arr)
madgwick = MadgwickAHRS(sampleperiod=sampleperiod,
quaternion=qq,
beta=beta)
count = 0
ll = len(gyr)
for i in range(0, len(gyr) - 1):
if (i % 300) == 0:
print(
str(i) + ' / ' + str(ll) + ' ' + str(round(i / ll * 100, 0)) +
'%')
while (count < times):
madgwick.update_imu(gyr[i], acc[i], delta_t[i])
count += 1
count = 0
# ref_qq[i]=madgwick.quaternion.elements
# ref_eu[i]=madgwick.quaternion.eu_angle
ref_qq[i, 0] = madgwick.quaternion._get_q()[0]
ref_qq[i, 1] = madgwick.quaternion._get_q()[1]
ref_qq[i, 2] = madgwick.quaternion._get_q()[2]
ref_qq[i, 3] = madgwick.quaternion._get_q()[3]
tempq = Quaternion(ref_qq[i])
ref_eu[i, :] = tempq.to_euler_angles_by_wiki()
ref_eu[i] *= 180 / np.pi
return ref_qq, ref_eu
def measurement_model(y_i, w_i_dash, delta_t, data, tuneR=10):
num = y_i.shape[0]
z_acc, z_gyro = np.zeros((num, 3)), np.zeros((num, 3))
z_gyro = y_i[:, 4:7]
g_quat = Quaternion(0, [0, 0, 9.8], "value")
R = np.identity(6) * tuneR
for i in range(y_i.shape[0]):
prev_q_wt = Quaternion(y_i[i][0], y_i[i][1:4], "value")
prev_q_wt_inverse = prev_q_wt.inverse()
next_q_wt = prev_q_wt_inverse.multiply(g_quat)
q_acc_body = next_q_wt.multiply(prev_q_wt)
z_acc[i] = q_acc_body.quaternion[1:4] #stores quaternion values
Z = np.hstack((z_acc, z_gyro))
cov_z = covariance(Z)
mean_z = np.average(Z, axis=0)
innov = data - mean_z
innov = innov.reshape((innov.shape[0], 1)) ##shape becomes 6*1
Pvv = cov_z + R
num = w_i_dash.shape[0]
Pxz = (1 / num) * (w_i_dash.T @ (Z - mean_z))
try:
kalman = Pxz @ np.linalg.inv(Pvv)
except (ValueError, ArithmeticError):
print("Error: trying to find inverse of singular matrix")
#6*1
update = kalman @ innov
#1*6
return update.T[0], kalman, Pvv
def predict(timestep, sigmas, q_t_t, w, n_dim):
n_sig = 2 * n_dim + 1
covar_weights = np.zeros(n_sig)
mean_weights = np.zeros(n_sig)
#print(sigmas)
covar_weights[0] = 2
for i in range(1, n_sig):
covar_weights[i] = 1. / (n_sig - 1)
mean_weights[i] = 1. / (n_sig - 1)
covar_weights = covar_weights.reshape(-1, 1)
q_tt_t = []
for i in range(sigmas.shape[0]):
a = Quaternion(0, 1 / 2. * sigmas[i]).exp()
b = Quaternion(0, 1 / 2. * w * timestep).exp()
q_trans = a.multiply(b)
q_tt_t.append(q_t_t.multiply(q_trans))
avg_q = average(q_tt_t, mean_weights)
sigmas_out = []
tmp = []
for i in range(len(q_tt_t)):
e = 2 * (avg_q.inv().multiply(q_tt_t[i])).log()
tmp.append(e.v)
tmp = np.asarray(tmp)
sigmas_out = np.transpose(tmp * covar_weights).dot(tmp)
return avg_q, sigmas_out, q_tt_t
def check(rfn, cfn, qfn, q):
""" compares the Quaternion function (qfn) with the equivilent
real (rfn) and complex (cfn) functions
"""
a = Quaternion(rfn(q.real))
b = qfn(Quaternion(q.real))
assert quaternion.isclose(a, b)
a = Quaternion(cfn(q.complex))
b = qfn(Quaternion(q.complex))
assert quaternion.isclose(a, b)
# For single value functions, we can create a z with same r and phi as the q
# Call the complex function, then create a Quaternion with f(r, phi) of the
# complex result merged in with the original axis.
#
q_polar = quaternion.polar(q)
z = cmath.rect(q_polar[0], q_polar[1])
fz = cfn(z)
fz_polar = cmath.polar(fz)
fi = quaternion.rect(fz_polar[0], fz_polar[1], q_polar[2])
fd = qfn(q)
assert quaternion.isclose(fi, fd)
# Test cut'n'paste errors referenceing imaginary components
# by rotating the axis,
#
a = qfn(qfoo(q))
b = qfoo(qfn(q))
assert quaternion.isclose(a, b)
a = qfn(qbar(q))
b = qbar(qfn(q))
assert quaternion.isclose(a, b)
def update(q_tt_t,
ave_q_tt_t,
Cov_tt_t,
z_acc,
g=Quaternion(0, [0, 0, 1]),
noise=np.eye(3) * 0.001):
n = len(q_tt_t)
#g = np.asarray(g)
tmp = []
for i in range(len(q_tt_t)):
e = 2 * ((ave_q_tt_t.inv()).multiply(q_tt_t[i])).log()
tmp.append(e.v)
e = np.asarray(tmp)
Z_tt = measure_model(q_tt_t, g)
Z_tt = [x.v for x in Z_tt]
Z_tt = np.asarray(Z_tt)
print(Z_tt, 'ztt')
weight_c = np.asarray([2.] + [1. / (n - 1)] * (n - 1)).reshape(-1, 1)
Ave_Z_tt = np.mean(Z_tt[:, :], axis=0)
print(Ave_Z_tt)
Pzz = np.transpose(
(Z_tt - Ave_Z_tt) * weight_c).dot(Z_tt - Ave_Z_tt) + noise
Pxz = np.transpose(e * weight_c).dot(-Z_tt + Ave_Z_tt)
K_tt = Pxz.dot(np.linalg.inv(Pzz))
q_tt_tt = ave_q_tt_t.multiply(
Quaternion(0,
K_tt.dot(z_acc - Ave_Z_tt) * (1 / 2)).exp())
Cov_tt_tt = Cov_tt_t - K_tt.dot(Pzz).dot(np.transpose(K_tt))
return q_tt_tt, Cov_tt_tt
def compute_mean(Y, prev_estimate):
mean = np.zeros(7)
err_vecs = np.zeros((3, Y.shape[1]))
q = Quaternion()
q.q = prev_estimate[:4]
qi = Quaternion()
Quat_part = Y[:4, :]
Omg_part = Y[4:, :]
e = Quaternion()
for _ in range(5):
for j in range(Y.shape[1]):
qi.q = Quat_part[:, j]
err_q = qi * q.inv()
err_vecs[:, j] = err_q.axis_angle()
err_mean = np.mean(err_vecs, axis=1)
e.from_axis_angle(err_mean)
q.q = (e * q).q
if np.linalg.norm(err_mean) < 0.001:
break
omg_mean = np.mean(Omg_part, axis=1)
mean[:4] = q.q
mean[4:] = omg_mean
return mean, err_vecs
def integrate(imu_data, train=True):
dt = [(imu_data['ts'][0, i + 1] - imu_data['ts'][0, i])
for i in range(imu_data['ts'].shape[1] - 1)]
angular_velocities = imu_data['vals'][3:, :]
quaternion_estimates = [Quaternion(1, [0, 0, 0])]
for t in range(angular_velocities.shape[1] - 1):
w = angular_velocities[:, t][[1, 2, 0]]
#quaternion_estimates.append(quaternion_estimates[-1].multiply(Quaternion(0, (0.5*w*dt[t])).exp().unit()))
quaternion_estimates.append((quaternion_estimates[t].multiply(
Quaternion(0, 0.5 * w * dt[t]).exp())).unit())
euler_estimates = []
for quaternion_estimate in quaternion_estimates:
euler_estimates.append(e3d.quat2euler(quaternion_estimate.to_numpy()))
vicon_data = read_dataset('trainset', 'vicon', '1')
vicon_euler = []
for i in range(vicon_data['rots'].shape[2]):
vicon_euler.append(e3d.mat2euler(vicon_data['rots'][:, :, i]))
if train:
for i in range(3):
pl.plot(imu_data['ts'][0], np.array(euler_estimates)[:, i])
pl.plot(vicon_data['ts'][0], np.array(vicon_euler)[:, i])
#pl.plot(vicon_data['ts'][0], np.vstack((np.array(euler_estimates)[:len(vicon_euler), i], np.array(vicon_euler)[:, i])).T)
pl.show()
else:
for i in range(3):
pl.plot(imu_data['ts'][0], np.array(euler_estimates)[:, i])
#pl.plot(vicon_data['ts'][0],np.array(vicon_euler)[:, i])
#pl.plot(vicon_data['ts'][0], np.vstack((np.array(euler_estimates)[:len(vicon_euler), i], np.array(vicon_euler)[:, i])).T)
pl.show()
def update_imu_new(self, gyroscope, accelerometer):
# Normalise accelerometer measurement
if norm(accelerometer) is 0:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
v = [2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]),
2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]),
self.q[0]**2 - self.q[1]**2 - self.q[2]**2 + self.q[3]**2]
error = np.cross(v, accelerometer.transpose())
self.IntError = self.IntError + error
# Apply feedback terms
ref = gyroscope - (self.beta*error).transpose() # Ref = Gyroscope - (obj.Kp*error + obj.Ki*obj.IntError)';
# Compute rate of change of quaternion
pDot = Quaternion.__mul__(Quaternion.__mul__(self.q, Quaternion([0, ref[0], ref[1], ref[2]])), 0.5) # Compute rate of change of quaternion
self.q = self.q + pDot * self.samplePeriod # Integrate rate of change of quaternion
self.q = Quaternion(self.q / norm(self.q)) # Normalise quaternion
# Store conjugate
self.quaternion = Quaternion.conj(self.q)
def getMeanQuaternion(q_cur, q_bar):
q = Quaternion(q_bar[0], q_bar[1:4]);
prev_e = 10
threshould = 0.0001;
ct = 0;
e_bar = np.array([0,0,0])
while(np.abs(LA.norm(e_bar) - prev_e) > threshould and ct < 50):
prev_e = LA.norm(e_bar);
E_i = np.zeros((3, 12));
for i in range(q_cur.shape[-1]):
er = getQtError(q_cur[0:4, i], q.inv());
E_i[:, i] = er.axis_angle();
e_bar = getBaryMean(E_i);
e_q = Quaternion();
e_q.from_axis_angle(e_bar)
q = e_q * q;
q.normalize();
ct += 1;
# print("err: " + str(LA.norm(e_bar)))
# print("ct: " + str(ct))
return q;
def update_imu(self, gyroscope, accelerometer):
"""
Perform one update step with data from a IMU sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
"""
q = self.quaternion
gyroscope = np.array(gyroscope, dtype=float).flatten()
accelerometer = np.array(accelerometer, dtype=float).flatten()
# Normalise accelerometer measurement
# print accelerometer
if norm(accelerometer) is 0:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
# Gradient descent algorithm corrective step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - accelerometer[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]])
step = j.T.dot(f)
step /= norm(step) # normalise step magnitude
# Compute rate of change of quaternion
qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1],
gyroscope[2])) * 0.5 - self.beta * step.T
# Integrate to yield quaternion
q += qdot * self.samplePeriod
self.quaternion = Quaternion(q / norm(q)) # normalise quaternion
def update_state_est(self, kalman_gain, innovation):
update_vec = np.matmul(kalman_gain, innovation)
update_vec_q = Quaternion()
update_vec_q.from_axis_angle(update_vec)
x_mu = Quaternion()
x_mu.q = self.mu
x_new_mu = x_mu * update_vec_q
self.mu = x_new_mu.q.reshape(4, 1)
def from_vector(cls, dq):
dual_quaternion_vector = None
if isinstance(dq, np.ndarray):
dual_quaternion_vector = dq.copy()
else:
dual_quaternion_vector = np.array(dq)
return cls(Quaternion(q=dual_quaternion_vector[0:4]),
Quaternion(q=dual_quaternion_vector[4:8]))
def update(self, gyroscope, accelerometer, magnetometer):
"""
Perform one update step with data from a AHRS sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data.
:param magnetometer: A three-element array containing the magnetometer data.
:return:
"""
q = self.quaternion
gyroscope = np.array(gyroscope, dtype=float).flatten()
accelerometer = np.array(accelerometer, dtype=float).flatten()
magnetometer = np.array(magnetometer, dtype=float).flatten()
# Normalise accelerometer measurement
if norm(accelerometer) is 0:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
# Normalise magnetometer measurement
if norm(magnetometer) is 0:
warnings.warn("magnetometer is zero")
return
magnetometer /= norm(magnetometer)
h = q * (Quaternion(0, magnetometer[0], magnetometer[1], magnetometer[2]) * q.conj())
b = np.array([0, norm(h[1:3]), 0, h[3]])
# Gradient descent algorithm corrective step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - accelerometer[1],
2 * (0.5 - q[1] ** 2 - q[2] ** 2) - accelerometer[2],
2 * b[1] * (0.5 - q[2] ** 2 - q[3] ** 2) + 2 * b[3] * (q[1] * q[3] - q[0] * q[2]) - magnetometer[0],
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] * (q[0] * q[1] + q[2] * q[3]) - magnetometer[1],
2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] * (0.5 - q[1] ** 2 - q[2] ** 2) - magnetometer[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]]
])
step = j.T.dot(f)
step /= norm(step) # normalise step magnitude
# Compute rate of change of quaternion
qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1], gyroscope[2])) * 0.5 - self.beta * step.T
# Integrate to yield quaternion
q += qdot * self.sample_period
self.quaternion = Quaternion(q / norm(q)) # normalise quaternion
def test_eq(self):
"""Test operator overloading method __eq__"""
# Create instances of class Quaternion to compare
q1 = Quaternion(0.9, -1, -0.4, 0.1)
q2 = Quaternion(0.9, -1, -0.4, 0.1)
q3 = Quaternion(0.9, -1.1, -0.4, 0.1)
# Assert expected behaviour
self.assertTrue(q1 == q2)
self.assertFalse(q1 == q3)
def setUp(self):
from quaternion import Quaternion, cross
from random import uniform
lo,hi = -100,100
a,b,c,d,e,f = (uniform(lo,hi) for i in range(6))
self.p = Quaternion(0,a,b,c)
self.q = Quaternion(0,d,e,f)
self.lhs = cross(self.p,self.q)
def rot_to_quat(w):
w = np.array(w)
norm = np.linalg.norm(w)
if (norm == 0):
return Quaternion(0, [0, 0, 0], "value")
axis = w / norm
q1 = Quaternion(norm, axis, "angle")
q1 = q1.unit_quaternion()
return q1
def __init__(self, data=None):
if data is not None:
data = struct.unpack('<hhhhhhhhhh', data)
self.accel = Vector(*[i / float(self.Scale.ACCELEROMETER) for i in data[4:7]])
self.gyro = Vector(*[i / float(self.Scale.GYROSCOPE) for i in data[7:]])
self.quat = Quaternion([i / float(self.Scale.ORIENTATION) for i in data[:4]])
else:
self.accel = Vector()
self.gyro = Vector()
self.quat = Quaternion()
def processA(X_p, prev, timeStamp): # Todo delta_t = (timeStamp - prev); for i in range(X_p.shape[-1]): q_ome = Quaternion(); q_ome.from_axis_angle(X_p[4:7, i] * delta_t); q_cur = Quaternion(X_p[0,i], X_p[1:4, i]); q_predict = q_ome * q_cur; q_predict.normalize(); X_p[0:4, i] = q_predict.q return X_p;
def __init__(self, connector):
self.connection = connector
self.arm = md.arm.UNKNOWN
self.pose = md.pose.REST
self.x_direction = md.x_direction.ELBOW
self.synced = False
self.startq = Quaternion(0, 0, 0, 1)
self.napq = Quaternion(0, 0, 0, 1)
self.imu = md.IMU()
self.emg = md.EMG()
def measurement_model(Y):
Z = np.zeros((6, 12))
g_quat = Quaternion(scalar=0, vec=[0, 0, 9.81])
quat = Quaternion()
for i in range(Y.shape[1]):
quat.q = Y[:4, i]
Z[:3, i] = (quat.inv() * (g_quat * quat)).q[1:]
Z[3:, i] = Y[4:, i]
return Z
def get_new_estimate(mean_minus, K, nu_k):
gain = np.dot(K, nu_k)
new_mean = np.zeros_like(mean_minus)
old = Quaternion()
old.q = mean_minus[:4]
update = Quaternion()
update.from_axis_angle(gain[:3])
new_mean[:4] = (update * old).q
new_mean[4:] = mean_minus[4:] + gain[3:]
return new_mean
def get_X(prev_state, W):
X = np.zeros((7, 12))
quat_prev = Quaternion()
quat_prev.q = prev_state[:4]
quat_noise = Quaternion()
for i in range(W.shape[1]):
quat_noise.from_axis_angle(W[:3, i])
X[:4, i] = (quat_prev * quat_noise).q
X[4:, i] = prev_state[4:] + W[3:, i]
return X
def process_model(X, delta_t):
Y = X
Quat_del = Quaternion()
quat_init = Quaternion()
for i in range(X.shape[1]):
omg = X[4:, i]
Quat_del.from_axis_angle(omg * delta_t)
quat_init.q = X[:4, i]
Y[:4, i] = (Quat_del * quat_init).q
return Y
def test_normalize(self):
"""Test method normalize"""
# Create instance of class Quaternion to normalize
q = Quaternion(-2.1, 0.0, 0.0, 0.0)
# Function call (to function to test)
q.normalize()
# Assert expected behaviour; the expected quaternion out and that
# the norm equals 1 after normalization.
self.assertTrue(q == Quaternion(-1.0, 0.0, 0.0, 0.0))
self.assertTrue(np.sqrt(q.w**2 + q.x**2 + q.y**2 + q.z**2) == 1)
def process_model_3(self, delta_t, omega, Xi):
Yi = Xi.copy() # 4x6
alpha_delta = omega * delta_t
for i in range(Xi.shape[1]):
qk_vec = Xi[:, i]
qk = Quaternion()
qk.q = qk_vec
q_delta = Quaternion()
q_delta.from_axis_angle(alpha_delta.reshape(3))
Yi[:, i] = (q_delta * qk).q
return Yi
def setUp(self):
from quaternion import Quaternion, exp, log
from random import uniform
self.exp = exp
self.log = log
lo,hi = -1,1
a,b,c,d,e,f,g,h = (uniform(lo,hi) for i in range(8))
self.p = Quaternion(a,b,c,d)
self.q = Quaternion(e,f,g,h)
def test_get_conjugate(self):
"""Test method get_conjugate"""
# Create instance of class Quaternion to get conjugate of
q_in = Quaternion(-2.1, 0.9, 1.4, -0.2)
# Function call (to function to test)
q = q_in.get_conjugate()
# Create oracle
q_oracle = Quaternion(-2.1, -0.9, -1.4, 0.2)
# Assert expected behaviour
self.assertTrue(q == q_oracle)
def rotate_quat(attitude, roll, pitch, yaw):
'''
Returns rotated quaternion
:param attitude: quaternion [w, x, y , z]
:param roll: rotation in rad
:param pitch: rotation in rad
:param yaw: rotation in rad
:returns: quaternion [w, x, y , z]
'''
quat = Quaternion(attitude)
rotation = Quaternion([roll, pitch, yaw])
res = rotation * quat
return res.q
