# Path: imusim/algorithms/orientation.py
# class OrientationFilter(object):
# class GyroIntegrator(OrientationFilter):
# class BachmannCF(OrientationFilter):
# class OrientCF(OrientationFilter):
# class YunEKF(OrientationFilter):
# class DistLinAccelCF(OrientationFilter):
#     def __init__(self, initialTime, initialRotation):
#     def __call__(self, accel, mag, gyro, t):
#     def _update(self, accel, mag, gyro, dt, t):
#     def __init__(self, initialTime, initialRotation, **kwargs):
#     def _update(self, accel, mag, gyro, dt, t):
#     def __init__(self, initialTime, initialRotation, k, **kwargs):
#     def _update(self, accel, mag, gyro, dt, t):
#     def __init__(self, initialTime, initialRotation, k, aT, **kwargs):
#     def _update(self, accel, mag, gyro, dt, t):
#     def __init__(self, initialTime, initialRotation, initialRotationalVelocity,
#             initialCovariance, measurementCovariance, D, tau, **kwargs):
#     def _update(self, accel, mag, gyro, dt, t):
#     def dotx(self, xHat):
#     def Phi(self, xHat, dt):
#     def __init__(self, initialTime, initialRotation, k, joint, offset,
#             **kwargs):
#     def handleLinearAcceleration(self, jointAcceleration, dt):
#     def childAcceleration(self, o, dt):
#     def _update(self, accel, mag, gyro, dt, t):
#         K = self.P_minus * np.linalg.inv(self.P_minus + self.R)
#         P = (np.eye(7) - K) * self.P_minus
#     GRAVITY_VECTOR = vectors.vector(0, 0, STANDARD_GRAVITY)
# 
# Path: imusim/trajectories/rigid_body.py
# class Joint(Point):
#     """
#     A joint in a rigid body model, with its own rotating co-ordinate frame.
# 
#     @ivar children: List of child points that have this joint as parent.
#     """
# 
#     def __init__(self, parent, name=None, offset=None):
#         """
#         Construct a joint.
# 
#         @param parent: Parent L{Joint}, or None for the root joint.
#         @param name: Name of this joint.
#         @param offset: Offset of this joint in the co-ordinate frame of its
#             parent, as a 3x1 L{np.ndarray}. Default is zero offset.
#         """
#         Point.__init__(self, parent, name, offset)
#         self.children = []
# 
#     @property
#     def points(self):
#         """
#         Iterator for the points of the body model starting at this point.
# 
#         @see: L{preorderTraversal}
#         """
#         return self.preorderTraversal()
# 
#     @property
#     def pointNames(self):
#         """
#         List of point names in the tree rooted at this joint.
#         """
#         return [p.name for p in self.points]
# 
#     def getPoint(self, pointName):
#         """
#         Get a point by name from the point tree rooted at this point.
# 
#         @param pointName: The name of the point to return (string).
#         @return: The corresponding L{Point} object.
#         @raises KeyError: If the named point is not present in the subtree.
#         """
#         for p in self.points:
#             if p.name == pointName:
#                 return p
#         raise KeyError
# 
#     @property
#     def childJoints(self):
#         """
#         List of child joints that have this joint as parent.
#         """
#         return filter(lambda p: p.isJoint, self.children)
# 
#     @property
#     def hasChildJoints(self):
#         """
#         Whether this joint has any child joints.
#         """
#         return len(self.childJoints) != 0
# 
#     @property
#     def joints(self):
#         """
#         Iterator for the joints of the body model starting at this joint.
#         """
#         return self.preorderTraversal(condition=lambda p: p.isJoint)
# 
#     @property
#     def jointNames(self):
#         """
#         List of joint names in the body model tree rooted at this joint.
#         """
#         return [j.name for j in self.joints]
# 
#     def getJoint(self, jointName):
#         """
#         Get a joint by name from the subtree rooted at this joint.
# 
#         @param jointName: The name of the joint to return (string).
#         @return: The corresponding L{Joint} object.
#         @raises KeyError: If the named joint is not present in the subtree.
#         """
#         for p in self.joints:
#             if p.name == jointName:
#                 return p
#         raise KeyError
# 
#     @classmethod
#     def structureCopy(cls, ref, parent=None):
#         """
#         Create a deep copy of the body model tree starting from the given joint.
#         """
#         joint = cls(parent, ref.name, ref.positionOffset)
#         for child in ref.children:
#             if child.isJoint:
#                 cls.structureCopy(child, joint)
#             else:
#                 Point(joint, child.name, child.positionOffset)
#         return joint


import numpy as np
from imusim.testing.random_data import RandomRotationTrajectory
from imusim.testing.quaternions import assert_quaternions_correlated, assertMaximumErrorAngle
from imusim.testing.inspection import getImplementations
from imusim.algorithms import orientation
from imusim.maths.quaternions import Quaternion, QuaternionArray
from imusim.trajectories.rigid_body import Joint
from imusim.maths.vectors import vector
"""
Tests for orientation tracking algorithms.
"""
# Copyright (C) 2009-2011 University of Edinburgh
#
# This file is part of IMUSim.
#
# IMUSim is free software: you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# IMUSim is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with IMUSim.  If not, see <http://www.gnu.org/licenses/>.


dt = 0.01
GRAVITY = vector(0,0,1)
NORTH = vector(1,0,0)

filterParameters = {

# Path: imusim/algorithms/orientation.py
# class OrientationFilter(object):
# class GyroIntegrator(OrientationFilter):
# class BachmannCF(OrientationFilter):
# class OrientCF(OrientationFilter):
# class YunEKF(OrientationFilter):
# class DistLinAccelCF(OrientationFilter):
#     def __init__(self, initialTime, initialRotation):
#     def __call__(self, accel, mag, gyro, t):
#     def _update(self, accel, mag, gyro, dt, t):
#     def __init__(self, initialTime, initialRotation, **kwargs):
#     def _update(self, accel, mag, gyro, dt, t):
#     def __init__(self, initialTime, initialRotation, k, **kwargs):
#     def _update(self, accel, mag, gyro, dt, t):
#     def __init__(self, initialTime, initialRotation, k, aT, **kwargs):
#     def _update(self, accel, mag, gyro, dt, t):
#     def __init__(self, initialTime, initialRotation, initialRotationalVelocity,
#             initialCovariance, measurementCovariance, D, tau, **kwargs):
#     def _update(self, accel, mag, gyro, dt, t):
#     def dotx(self, xHat):
#     def Phi(self, xHat, dt):
#     def __init__(self, initialTime, initialRotation, k, joint, offset,
#             **kwargs):
#     def handleLinearAcceleration(self, jointAcceleration, dt):
#     def childAcceleration(self, o, dt):
#     def _update(self, accel, mag, gyro, dt, t):
#         K = self.P_minus * np.linalg.inv(self.P_minus + self.R)
#         P = (np.eye(7) - K) * self.P_minus
#     GRAVITY_VECTOR = vectors.vector(0, 0, STANDARD_GRAVITY)
# 
# Path: imusim/trajectories/rigid_body.py
# class Joint(Point):
#     """
#     A joint in a rigid body model, with its own rotating co-ordinate frame.
# 
#     @ivar children: List of child points that have this joint as parent.
#     """
# 
#     def __init__(self, parent, name=None, offset=None):
#         """
#         Construct a joint.
# 
#         @param parent: Parent L{Joint}, or None for the root joint.
#         @param name: Name of this joint.
#         @param offset: Offset of this joint in the co-ordinate frame of its
#             parent, as a 3x1 L{np.ndarray}. Default is zero offset.
#         """
#         Point.__init__(self, parent, name, offset)
#         self.children = []
# 
#     @property
#     def points(self):
#         """
#         Iterator for the points of the body model starting at this point.
# 
#         @see: L{preorderTraversal}
#         """
#         return self.preorderTraversal()
# 
#     @property
#     def pointNames(self):
#         """
#         List of point names in the tree rooted at this joint.
#         """
#         return [p.name for p in self.points]
# 
#     def getPoint(self, pointName):
#         """
#         Get a point by name from the point tree rooted at this point.
# 
#         @param pointName: The name of the point to return (string).
#         @return: The corresponding L{Point} object.
#         @raises KeyError: If the named point is not present in the subtree.
#         """
#         for p in self.points:
#             if p.name == pointName:
#                 return p
#         raise KeyError
# 
#     @property
#     def childJoints(self):
#         """
#         List of child joints that have this joint as parent.
#         """
#         return filter(lambda p: p.isJoint, self.children)
# 
#     @property
#     def hasChildJoints(self):
#         """
#         Whether this joint has any child joints.
#         """
#         return len(self.childJoints) != 0
# 
#     @property
#     def joints(self):
#         """
#         Iterator for the joints of the body model starting at this joint.
#         """
#         return self.preorderTraversal(condition=lambda p: p.isJoint)
# 
#     @property
#     def jointNames(self):
#         """
#         List of joint names in the body model tree rooted at this joint.
#         """
#         return [j.name for j in self.joints]
# 
#     def getJoint(self, jointName):
#         """
#         Get a joint by name from the subtree rooted at this joint.
# 
#         @param jointName: The name of the joint to return (string).
#         @return: The corresponding L{Joint} object.
#         @raises KeyError: If the named joint is not present in the subtree.
#         """
#         for p in self.joints:
#             if p.name == jointName:
#                 return p
#         raise KeyError
# 
#     @classmethod
#     def structureCopy(cls, ref, parent=None):
#         """
#         Create a deep copy of the body model tree starting from the given joint.
#         """
#         joint = cls(parent, ref.name, ref.positionOffset)
#         for child in ref.children:
#             if child.isJoint:
#                 cls.structureCopy(child, joint)
#             else:
#                 Point(joint, child.name, child.positionOffset)
#         return joint


# Path: imusim/tests/algorithms/orientation_test.py
import numpy as np
from imusim.testing.random_data import RandomRotationTrajectory
from imusim.testing.quaternions import assert_quaternions_correlated, assertMaximumErrorAngle
from imusim.testing.inspection import getImplementations
from imusim.algorithms import orientation
from imusim.maths.quaternions import Quaternion, QuaternionArray
from imusim.trajectories.rigid_body import Joint
from imusim.maths.vectors import vector

"""
Tests for orientation tracking algorithms.
"""
# Copyright (C) 2009-2011 University of Edinburgh
#
# This file is part of IMUSim.
#
# IMUSim is free software: you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# IMUSim is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with IMUSim.  If not, see <http://www.gnu.org/licenses/>.


dt = 0.01
GRAVITY = vector(0,0,1)
NORTH = vector(1,0,0)

filterParameters = {

    orientation.OrientCF : {