# 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
#
# 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):
        vo = vectorObservationMethod(inclinationAngle=inclination)
    else:
        vo = vectorObservationMethod()
    q = Quaternion.fromEuler(eulerAngles)
    inclination = math.radians(inclination)
    m = np.array([[math.cos(inclination)],[0],
        [math.sin(inclination)]])
    g = np.array([[0,0,1]],dtype=float).T
    g = q.rotateFrame(g)
    m = q.rotateFrame(m)
    assertQuaternionAlmostEqual(q,vo(g,m))

def testVectorObservationMethods():
    for method in getImplementations(vector_observation,

# 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

#
# 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):
        vo = vectorObservationMethod(inclinationAngle=inclination)
    else:
        vo = vectorObservationMethod()
    q = Quaternion.fromEuler(eulerAngles)
    inclination = math.radians(inclination)
    m = np.array([[math.cos(inclination)],[0],
        [math.sin(inclination)]])
    g = np.array([[0,0,1]],dtype=float).T
    g = q.rotateFrame(g)
    m = q.rotateFrame(m)
    assertQuaternionAlmostEqual(q,vo(g,m))

def testVectorObservationMethods():
    for method in getImplementations(vector_observation,

            vector_observation.VectorObservation):