# Path: sensbiotk/transforms3d/quaternions.py
# MAX_FLOAT = np.maximum_sctype(np.float)
# FLOAT_EPS = np.finfo(np.float).eps
#     X = x*s
#     Y = y*s
#     Z = z*s
#     K = np.array([
#         [Qxx - Qyy - Qzz, 0,               0,               0              ],
#         [Qyx + Qxy,       Qyy - Qxx - Qzz, 0,               0              ],
#         [Qzx + Qxz,       Qzy + Qyz,       Qzz - Qxx - Qyy, 0              ],
#         [Qyz - Qzy,       Qzx - Qxz,       Qxy - Qyx,       Qxx + Qyy + Qzz]]
#         ) / 3.0
#     c = math.cos(theta); s = math.sin(theta); C = 1-c
# def fillpositive(xyz, w2_thresh=None):
# def quat2mat(q):
# def mat2quat(M):
# def mult(q1, q2):
# def conjugate(q):
# def norm(q):
# def norm2(q):
# def normalize(q):
# def isunit(q):
# def inverse(q):
# def eye():
# def rotate_vector(v, q):
# def nearly_equivalent(q1, q2, rtol=1e-5, atol=1e-8):
# def angle_axis2quat(theta, vector, is_normalized=False):
# def angle_axis2mat(theta, vector, is_normalized=False):
# def quat2angle_axis(quat, identity_thresh=None):


import numpy as np
from sensbiotk.transforms3d import quaternions as nq
        self._C = [0.0, 0.0, 1.0, 0.0]
        self._D = [0.0, 1.0, 0.0, 0.0]

        return

    def init_observer(self, z):
        """ Martin Salaun init observer
        """
        # Quaternions construction
        ya = [0, z[0], z[1], z[2]]
        yb = [0, z[3], z[4], z[5]]
        yc = nq.mult(ya, yb)
        # Normalization
        ya = nq.normalize(ya)
#        yb = nq.normalize(yb)
        yc[0] = 0
        yc = nq.normalize(yc)

        if ya[3] == 1:
            self.q = 1
            self.qinv = 1
        else:
            self.qinv = [-ya[2], 1 - ya[3], 0, ya[1]]
            self.qinv = nq.normalize(self.qinv)
            self.q = nq.conjugate(self.qinv)

        yc = nq.mult(nq.mult(self.q, yc), self.qinv)

        if yc[2] != 1:
            self.qinv = nq.mult(self.qinv, [-yc[1], 0, yc[3], 1 - yc[2]])

# Path: sensbiotk/transforms3d/quaternions.py
# MAX_FLOAT = np.maximum_sctype(np.float)
# FLOAT_EPS = np.finfo(np.float).eps
#     X = x*s
#     Y = y*s
#     Z = z*s
#     K = np.array([
#         [Qxx - Qyy - Qzz, 0,               0,               0              ],
#         [Qyx + Qxy,       Qyy - Qxx - Qzz, 0,               0              ],
#         [Qzx + Qxz,       Qzy + Qyz,       Qzz - Qxx - Qyy, 0              ],
#         [Qyz - Qzy,       Qzx - Qxz,       Qxy - Qyx,       Qxx + Qyy + Qzz]]
#         ) / 3.0
#     c = math.cos(theta); s = math.sin(theta); C = 1-c
# def fillpositive(xyz, w2_thresh=None):
# def quat2mat(q):
# def mat2quat(M):
# def mult(q1, q2):
# def conjugate(q):
# def norm(q):
# def norm2(q):
# def normalize(q):
# def isunit(q):
# def inverse(q):
# def eye():
# def rotate_vector(v, q):
# def nearly_equivalent(q1, q2, rtol=1e-5, atol=1e-8):
# def angle_axis2quat(theta, vector, is_normalized=False):
# def angle_axis2mat(theta, vector, is_normalized=False):
# def quat2angle_axis(quat, identity_thresh=None):


# Path: sensbiotk/algorithms/martin_ahrs.py
import numpy as np
from sensbiotk.transforms3d import quaternions as nq

        self._C = [0.0, 0.0, 1.0, 0.0]
        self._D = [0.0, 1.0, 0.0, 0.0]

        return

    def init_observer(self, z):
        """ Martin Salaun init observer
        """
        # Quaternions construction
        ya = [0, z[0], z[1], z[2]]
        yb = [0, z[3], z[4], z[5]]
        yc = nq.mult(ya, yb)
        # Normalization
        ya = nq.normalize(ya)
#        yb = nq.normalize(yb)
        yc[0] = 0
        yc = nq.normalize(yc)

        if ya[3] == 1:
            self.q = 1
            self.qinv = 1
        else:
            self.qinv = [-ya[2], 1 - ya[3], 0, ya[1]]
            self.qinv = nq.normalize(self.qinv)
            self.q = nq.conjugate(self.qinv)

        yc = nq.mult(nq.mult(self.q, yc), self.qinv)

        if yc[2] != 1:
            self.qinv = nq.mult(self.qinv, [-yc[1], 0, yc[3], 1 - yc[2]])

            self.qinv = nq.normalize(self.qinv)