本文整理汇总了Python中quaternion.Quaternion.normalize方法的典型用法代码示例。如果您正苦于以下问题：Python Quaternion.normalize方法的具体用法？Python Quaternion.normalize怎么用？Python Quaternion.normalize使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类quaternion.Quaternion的用法示例。
在下文中一共展示了Quaternion.normalize方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: fqa
# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import normalize [as 别名]
def fqa(msr):
ax = msr['accel.x']
ay = msr['accel.y']
az = msr['accel.z']
norm = math.sqrt(ax*ax + ay*ay + az*az)
# estimation of elevation quaternion (around y axes)
sin_theta_a = (ax / norm)
cos_theta_a = math.sqrt(1 - sin_theta_a ** 2)
sin_half_theta = math.copysign(1, sin_theta_a) * math.sqrt((1 - cos_theta_a) / 2)
cos_half_theta = math.sqrt((1 + cos_theta_a) / 2)
q_e = Quaternion(cos_half_theta, 0, sin_half_theta, 0)
# estimation of roll quaternion (around x axis)
sin_phi = (-ay / norm) / cos_theta_a
cos_phi = (-az / norm) / cos_theta_a
sin_half_phi = half_sin(sin_phi, cos_phi)
cos_half_phi = half_cos(sin_phi, cos_phi)
# TODO singularity avoidance!!!
q_r = Quaternion(cos_half_phi, sin_half_phi, 0, 0)
# estimation of azimuth quaternion (around z axis)
mx = msr['mag.x']
my = msr['mag.y']
mz = msr['mag.z']
norm = math.sqrt(mx*mx + my*my + mz*mz)
qm = Quaternion(0, mx / norm, my / norm, mz / norm)
qm_a = q_e.multiply(q_r).multiply(qm).multiply(q_r.inverse()).multiply(q_e.inverse())
Quaternion.normalize(qm_a)
cos_zeta = qm_a.c
sin_zeta = qm_a.b
sin_half_zeta = half_sin(sin_zeta, cos_zeta)
cos_half_zeta = half_cos(sin_zeta, cos_zeta)
q_a = Quaternion(cos_half_zeta, 0, 0, sin_half_zeta)
q_fqa = q_a.multiply(q_e).multiply(q_r)
return q_fqa示例2: update
# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import normalize [as 别名]
def update(self, msr):
t = msr['t']
dt = t - self.t
self.t = t
w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z'])
q_fqa = fqa(msr)
dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt)
dq_dynamic = w.multiply(self.q).scalar_multiply(0.5)
dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k))
self.q = self.q.add(dq.scalar_multiply(dt))
self.q = Quaternion.normalize(self.q)示例3: ToLeftQuaternionProductMatrix
# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import normalize [as 别名]
[x[1], -x[2], 0., x[0]],
[x[2], x[1], -x[0], 0.]])
def ToLeftQuaternionProductMatrix(x):
return np.array([
[0., -x[0], -x[1], -x[2]],
[x[0], 0., -x[2], x[1]],
[x[1], x[2], 0., -x[0]],
[x[2], -x[1], x[0], 0.]])
if __name__ == "__main__":
q = Quaternion(1.,0.,0.,0.)
print q.inverse()
print q.toAxisAngle()
q2 = Quaternion(1.,.01,0.,0.)
q2.normalize()
print q2
print q2.inverse()
print q2.inverse().dot(q2)
print q2.dot(q2.inverse())
print q.dot(q2)
print q.angleTo(q2)
raw_input()
q2 = Quaternion(1.,1.,0.,0.)
q2.fromAxisAngle(np.pi/2.0,np.array([0.,1.,1.]))
q3 = q.slerp(q2,0.5)
print q
print q3
print q2