def fuse(self, mag_normal, acc):
acc_len = quaternion.vector_len(acc)
if acc_len < 0.95 or acc_len > 1.05:
self.quality = 0.0
return
acc_normal = quaternion.normal(acc)
mag_rot_q = quaternion.from_two_vector(self.mag_vector, mag_normal)
gravity_vector = self.neg_v(acc_normal)
origin_gravity = quaternion.rotate_vector([0.0, 0.0, -1.0], mag_rot_q)
v1 = quaternion.cross(mag_normal, gravity_vector)
v2 = quaternion.cross(mag_normal, origin_gravity)
v1 = quaternion.normal(v1)
v2 = quaternion.normal(v2)
ang_rot_dot = quaternion.dot(v1, v2)
ang_rot = math.acos(ang_rot_dot) * 180.0 / math.pi
v_axis = quaternion.cross(v2, v1)
v_axis = quaternion.normal(v_axis)
ang_axis_dot = quaternion.dot(v_axis, mag_normal)
if ang_axis_dot <= -1.0:
ang_axis_dot = -0.99999
if ang_axis_dot >= 1.0:
ang_axis_dot = 0.99999
ang_axis = math.acos(ang_axis_dot) * 180.0 / math.pi
rot_axis = mag_normal
if ang_axis > 90.0:
rot_axis = self.neg_v(mag_normal)
rot_q = quaternion.from_axis_angle(rot_axis, ang_rot)
rot_gravity = quaternion.rotate_vector(origin_gravity, rot_q)
dot = quaternion.dot(rot_gravity, gravity_vector)
ang = math.acos(dot) * 180.0 / math.pi
quality = ang / 10.0
if quality > 1.0:
quality = 1.0
self.quality = quality
#print '%.2f %.2f %.2f %.2f' % (ang2, ang_axis, ang_rot, ang)
q = quaternion.multiply(rot_q, mag_rot_q)
self.q = self.neg_q(q)
def fuse(self, mag, gyro_q):
dot = quaternion.dot(mag, self.mag_vector)
ang = math.acos(dot)
ang = ang * 180.0 / math.pi
rotated_mag = quaternion.rotate_vector(self.mag_vector, gyro_q)
dot2 = quaternion.dot(rotated_mag, self.mag_vector)
ang2 = math.acos(dot2)
ang2 = ang2 * 180.0 / math.pi
print '%.2f %.2f' % (ang, ang2)
def test_dot():
b = Quaternion(+17.16, -1.32, -1.48, -2.80)
for a in qlist:
u = quaternion.dot(a, a)
v = a.quadrance()
assert abs(u - v) < 1.0e-15
u = a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z
v = quaternion.dot(a, b)
assert abs(u - v) < 1.0e-15
u = quaternion.dot(b, a)
v = quaternion.dot(a, b)
assert abs(u - v) < 1.0e-15
def setUp(self):
from quaternion import Quaternion, dot
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 = dot(self.p,self.q)
def calc_error(x_axis, y_axis, z_axis):
e1 = quaternion.dot(x_axis, y_axis)
e1 = abs(e1)
e2 = quaternion.dot(y_axis, z_axis)
e2 = abs(e2)
e3 = quaternion.dot(x_axis, z_axis)
e3 = abs(e3)
z = quaternion.cross(x_axis, y_axis)
e4 = quaternion.cross(z, z_axis)
e4 = quaternion.vector_len(e4)
dot4 = quaternion.dot(z, z_axis)
if dot4 < 0.3:
e4 += 2.0
e = e1 + e2 + e3 + e4
return e
def fuse(self, mag, gyro_q):
self.take_snap(mag, gyro_q)
self.count += 1
self.increace_q(gyro_q)
self.filter_q()
if self.need_calc == False:
return
q_mag = quaternion.from_two_vector(mag, self.mag_vector)
gyro_diff_neg = self.neg_q(self.gyro_diff)
es = []
for i in range(360):
q_rot = quaternion.from_axis_angle(self.mag_vector, i)
q_mag_rot = quaternion.multiply(q_rot, q_mag)
q_last = quaternion.multiply(gyro_diff_neg, q_mag_rot)
x_axis = quaternion.rotate_vector([1.0, 0.0, 0.0], q_last)
y_axis = quaternion.rotate_vector([0.0, 1.0, 0.0], q_last)
z_axis = quaternion.rotate_vector([0.0, 0.0, 1.0], q_last)
x_v = quaternion.dot(x_axis, self.mag_vector)
y_v = quaternion.dot(y_axis, self.mag_vector)
z_v = quaternion.dot(z_axis, self.mag_vector)
e = 0.0
e += abs(x_v - self.snap_mag[0])
e += abs(y_v - self.snap_mag[1])
e += abs(z_v - self.snap_mag[2])
es.append(e)
min_e = min(es)
min_index = es.index(min_e)
average = sum(es) / len(es)
self.es_list.append(es)
if len(self.es_list) > 30:
#f = open('es_list', 'wb')
#pickle.dump(self.es_list, f)
print 'dumped'
axis, angle = quaternion.get_axis_angle(self.gyro_diff)
axis = quaternion.normal(axis)
dot = quaternion.dot(axis, self.mag_vector)
q_rot = quaternion.from_axis_angle(self.mag_vector, min_index)
q_mag_rot = quaternion.multiply(q_rot, q_mag)
if min_e < 0.03 and dot < 0.9:
print 'updated'
self.snap_q = gyro_q
self.calced_q = q_mag_rot
else:
print 'not acurate'
self.need_calc = False
self.snap_mag = None