# Path: imusim/algorithms/vector_observation.py
# class VectorObservation(object):
# class TRIAD(VectorObservation):
# class GramSchmidt(VectorObservation):
# class FQA(VectorObservation):
# class LeastSquaresOptimalVectorObservation(VectorObservation):
# class DavenportQ(LeastSquaresOptimalVectorObservation):
# class QUEST(LeastSquaresOptimalVectorObservation):
#     def __call__(self, *measurements):
#     def _process(self, *measurements):
#     def _process(self, g, m):
#     def _process(self, g, m):
#     def __init__(self):
#     def cosHalfAngle(self, cosAngle):
#     def sinHalfAngle(self, cosAngle, sinAngle):
#     def _process(self, g, m):
#     def __init__(self, refs=None, weights=None, inclinationAngle=None):
#     def _process(self, *meas):
#     def _apply(self, refs, meas):
#     def _process(self, *meas):
#         B = sum([a * np.dot(b, r.T)
#             for a,b,r in zip(self.weights, meas, self.refs)])
#         S = B + B.T
#         K = np.empty((4,4))
#         K[0:3,0:3] = S-sigma*np.identity(3)
#         K[0:3,3] = z[:,0]
#         K[3,0:3] = z[:,0]
#         K[3,3] = sigma
#         B = sum([a * np.dot(m, r.T) for a,m,r in zip(self.weights, meas, refs)])
#         S = B + B.T
#         Z = sum([a * vectors.cross(m, r) for a,m,r in zip(self.weights, meas, refs)])
#         X = np.dot(alpha*np.identity(3) + beta*S + np.dot(S,S), Z)
#         EPS = 1e-8


import numpy as np
import itertools
import math
from imusim.algorithms import vector_observation
from imusim.maths.quaternions import Quaternion
from imusim.testing.quaternions import assertQuaternionAlmostEqual
from imusim.testing.inspection import getImplementations
"""
Tests for vector observation 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/>.



angles = [45*p for p in xrange(8)]

def checkVectorObservation(vectorObservationMethod, eulerAngles, inclination):
    if issubclass(vectorObservationMethod,

# Path: imusim/algorithms/vector_observation.py
# class VectorObservation(object):
# class TRIAD(VectorObservation):
# class GramSchmidt(VectorObservation):
# class FQA(VectorObservation):
# class LeastSquaresOptimalVectorObservation(VectorObservation):
# class DavenportQ(LeastSquaresOptimalVectorObservation):
# class QUEST(LeastSquaresOptimalVectorObservation):
#     def __call__(self, *measurements):
#     def _process(self, *measurements):
#     def _process(self, g, m):
#     def _process(self, g, m):
#     def __init__(self):
#     def cosHalfAngle(self, cosAngle):
#     def sinHalfAngle(self, cosAngle, sinAngle):
#     def _process(self, g, m):
#     def __init__(self, refs=None, weights=None, inclinationAngle=None):
#     def _process(self, *meas):
#     def _apply(self, refs, meas):
#     def _process(self, *meas):
#         B = sum([a * np.dot(b, r.T)
#             for a,b,r in zip(self.weights, meas, self.refs)])
#         S = B + B.T
#         K = np.empty((4,4))
#         K[0:3,0:3] = S-sigma*np.identity(3)
#         K[0:3,3] = z[:,0]
#         K[3,0:3] = z[:,0]
#         K[3,3] = sigma
#         B = sum([a * np.dot(m, r.T) for a,m,r in zip(self.weights, meas, refs)])
#         S = B + B.T
#         Z = sum([a * vectors.cross(m, r) for a,m,r in zip(self.weights, meas, refs)])
#         X = np.dot(alpha*np.identity(3) + beta*S + np.dot(S,S), Z)
#         EPS = 1e-8


# Path: imusim/tests/algorithms/vector_observation_test.py
import numpy as np
import itertools
import math
from imusim.algorithms import vector_observation
from imusim.maths.quaternions import Quaternion
from imusim.testing.quaternions import assertQuaternionAlmostEqual
from imusim.testing.inspection import getImplementations

"""
Tests for vector observation 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/>.



angles = [45*p for p in xrange(8)]

def checkVectorObservation(vectorObservationMethod, eulerAngles, inclination):
    if issubclass(vectorObservationMethod,

            vector_observation.LeastSquaresOptimalVectorObservation):