def testTrajectories(self): position = vectors.vector(0,0,0) # Accelerometer test trajectories for angles in [ (0,-90,0),(0,90,0), # X down, X up (pitch -/+90 deg) (0,0,90),(0,0,-90), # Y down, Y up (roll +/-90 deg) (0,0,0),(0,0,180)]: # Z down, Z up (roll 0/180 deg) rotation = Quaternion().setFromEuler(angles) yield StaticTrajectory(rotation=rotation) # Magnetometer test trajectories field = self.environment.magneticField(position, 0) N = field / vectors.norm(field) E = vectors.vector(-N[1,0], N[0,0], 0) E /= vectors.norm(E) D = vectors.cross(N, E) for vectorset in [ (N,E,D), (-N,E,-D), # X north, X south (E,-N,D), (-E,N,D), # Y north, Y south (D,E,-N), (-D,E,N)]: # Z north, Z south rotation = Quaternion().setFromVectors(*vectorset) yield StaticTrajectory(rotation=rotation) # Gyro test trajectories for axis in range(3): omega = vectors.vector(0,0,0) omega[axis] = self.rotationalVelocity yield StaticContinuousRotationTrajectory(omega) yield StaticContinuousRotationTrajectory(-omega)
def testTrajectories(self): position = vectors.vector(0, 0, 0) # Accelerometer test trajectories for angles in [ (0, -90, 0), (0, 90, 0), # X down, X up (pitch -/+90 deg) (0, 0, 90), (0, 0, -90), # Y down, Y up (roll +/-90 deg) (0, 0, 0), (0, 0, 180) ]: # Z down, Z up (roll 0/180 deg) rotation = Quaternion().setFromEuler(angles) yield StaticTrajectory(rotation=rotation) # Magnetometer test trajectories field = self.environment.magneticField(position, 0) N = field / vectors.norm(field) E = vectors.vector(-N[1, 0], N[0, 0], 0) E /= vectors.norm(E) D = vectors.cross(N, E) for vectorset in [ (N, E, D), (-N, E, -D), # X north, X south (E, -N, D), (-E, N, D), # Y north, Y south (D, E, -N), (-D, E, N) ]: # Z north, Z south rotation = Quaternion().setFromVectors(*vectorset) yield StaticTrajectory(rotation=rotation) # Gyro test trajectories for axis in range(3): omega = vectors.vector(0, 0, 0) omega[axis] = self.rotationalVelocity yield StaticContinuousRotationTrajectory(omega) yield StaticContinuousRotationTrajectory(-omega)
def testBVHInput(): data = r"""HIERARCHY ROOT root { OFFSET 0.0 0.0 0.0 CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation JOINT j1 { OFFSET 10.0 0.0 0.0 CHANNELS 3 Zrotation Xrotation Yrotation End Site { OFFSET 0.0 10.0 0.0 } } } MOTION Frames: 1 Frame Time: 0.1 0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0 """ testFile = tempfile.NamedTemporaryFile(mode='w+t', encoding='utf-8') with testFile: testFile.write(data) testFile.flush() model = loadBVHFile(testFile.name) assert len(list(model.points)) == 3 assert model.name == 'root' assert len(model.channels) == 6 assert len(model.children) == 1 assert_almost_equal(model.position(0), vector(0, 0, 0)) assertQuaternionAlmostEqual(model.rotation(0), convertCGtoNED(Quaternion(1, 0, 0, 0))) j1 = model.getJoint('j1') assert j1.parent is model assert len(j1.channels) == 3 assert len(j1.children) == 1 assert_almost_equal(j1.positionOffset, vector(10, 0, 0)) assert_almost_equal(j1.position(0), convertCGtoNED(vector(10, 0, 0))) assertQuaternionAlmostEqual( j1.rotation(0), convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy'))) j1end = model.getPoint('j1_end') assert j1end.parent is j1 assert len(j1end.channels) == 0 assert not j1end.isJoint assert_almost_equal(j1end.positionOffset, vector(0, 10, 0)) assert_almost_equal(j1end.position(0), vector(0, 0, 0))
def testBVHInput(): data = r"""HIERARCHY ROOT root { OFFSET 0.0 0.0 0.0 CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation JOINT j1 { OFFSET 10.0 0.0 0.0 CHANNELS 3 Zrotation Xrotation Yrotation End Site { OFFSET 0.0 10.0 0.0 } } } MOTION Frames: 1 Frame Time: 0.1 0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0 """ testFile = tempfile.NamedTemporaryFile() with testFile: testFile.write(data) testFile.flush() model = loadBVHFile(testFile.name) assert len(list(model.points)) == 3 assert model.name == 'root' assert len(model.channels) == 6 assert len(model.children) == 1 assert_almost_equal(model.position(0),vector(0,0,0)) assertQuaternionAlmostEqual(model.rotation(0), convertCGtoNED(Quaternion(1,0,0,0))) j1 = model.getJoint('j1') assert j1.parent is model assert len(j1.channels) == 3 assert len(j1.children) == 1 assert_almost_equal(j1.positionOffset, vector(10,0,0)) assert_almost_equal(j1.position(0), convertCGtoNED(vector(10,0,0))) assertQuaternionAlmostEqual(j1.rotation(0), convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy'))) j1end = model.getPoint('j1_end') assert j1end.parent is j1 assert len(j1end.channels) == 0 assert not j1end.isJoint assert_almost_equal(j1end.positionOffset, vector(0,10,0)) assert_almost_equal(j1end.position(0), vector(0,0,0))
def __init__(self, rootdata): """ Construct a new ASFRoot. @param rootdata: The L{ParseResults} object representing the root segment data from the ASF file. """ TreeNode.__init__(self, None) self.name = 'root' self.childoffset = vector(0,0,0) self.isDummy = False axis = vector(*rootdata.axis)[::-1] rotOrder = rootdata.axisRotationOrder[::-1] self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate self.channels = rootdata.channels
def __init__(self, rootdata): """ Construct a new ASFRoot. @param rootdata: The L{ParseResults} object representing the root segment data from the ASF file. """ TreeNode.__init__(self, None) self.name = 'root' self.childoffset = vector(0, 0, 0) self.isDummy = False axis = vector(*rootdata.axis)[::-1] rotOrder = rootdata.axisRotationOrder[::-1] self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate self.channels = rootdata.channels
def __init__(self, transform=None, rz=0, ry=0, rx=0, scale=1, translation=vector(0,0,0)): """ Construct an affine transform. A generic transformation matrix can be supplied, or a transform composed of rotation and scaling can be built. Generating rotations using supplied Euler angles is performed from the perspective of a fixed camera viewing a rotating object, i.e. points rotate within a fixed reference frame. Rotations are applied in aerospace (ZYX) order. @param transform: 3x3 linear transformation matrix. @param rx: X rotation angle in radians. @param ry: Y rotation angle in radians. @param rz: Z rotation angle in radians. @param scale: Scaling factor @param translation: 3x1 translation vector. """ if transform is not None: self._transform = transform else: self._transform = scale * matrixFromEuler((rz,ry,rx),'zyx',False) self._inverseTransform = np.linalg.inv(self._transform) self._translation = translation
def testStaticTrajectory(): p = randomPosition() r = randomRotation() s = StaticTrajectory(p, r) assert_equal(s.position(0), p) assert_equal(s.velocity(0), vector(0,0,0)) assert_equal(s.acceleration(0), vector(0,0,0)) assert_equal(s.rotation(0).components, r.components) assert_equal(s.rotationalVelocity(0), vector(0,0,0)) assert_equal(s.rotationalAcceleration(0), vector(0,0,0)) assert s.startTime == 0 assert s.endTime == np.inf qa = s.rotation((1,2)) assert type(qa) == QuaternionArray assert len(qa) == 2
def __init__(self, magnitude=STANDARD_GRAVITY): """ Construct field model. @param magnitude: Magnitude of field in m/s^2. Default is standard Earth gravity. """ ConstantVectorField.__init__(self, vector(0, 0, magnitude))
def __init__(self, parent, bonedata, scale): """ Consruct a new ASFBone. @param parent: The parent bone of this bone. @param bonedata: The L{ParseResults} object representing the bone data from the ASF file. """ TreeNode.__init__(self, parent) self.name = bonedata.name axis = vector(*bonedata.axis)[::-1] rotOrder = bonedata.axisRotationOrder[::-1] self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate self.childoffset = self.rotationOffset.rotateVector( vector(*bonedata.direction) * bonedata.length * scale) self.isDummy = bonedata.channels is '' self.channels = bonedata.channels
def __init__(self, platform, positionOffset=vector(0,0,0), rotationOffset=Quaternion(1,0,0,0)): """ Initialise sensor. @param platform: L{Platform} this sensor will be attached to. @param positionOffset: Position offset vector of this sensor in the local co-ordinate frame of its host platform. @param rotationOffset: Rotation offset quaternion of this sensor in the local co-ordinate frame of its host platform. """ self._positionOffset = positionOffset self._rotationOffset = rotationOffset Component.__init__(self, platform)
def __init__(self, positionOffset=vector(0, 0, 0), rotationOffset=Quaternion(1, 0, 0, 0), simulation=None, trajectory=None): self.accelerometer = IdealAccelerometer(self, positionOffset=positionOffset, rotationOffset=rotationOffset) self.magnetometer = IdealMagnetometer(self, positionOffset=positionOffset, rotationOffset=rotationOffset) self.gyroscope = IdealGyroscope(self, positionOffset=positionOffset, rotationOffset=rotationOffset) self.adc = IdealADC(self) self.timer = IdealTimer(self) self.radio = IdealRadio(self) StandardIMU.__init__(self, simulation, trajectory)
def calibrate(self, imu): calibration = {} position = vectors.vector(0, 0, 0) expected = {} measured = {} for sensor in imu.sensors: expected[sensor] = [] measured[sensor] = [] for trajectory in self.testTrajectories(): sim = Simulation(environment=self.environment) sim.time = 0 imu.simulation = sim imu.trajectory = trajectory BasicIMUBehaviour(imu, self.samplingPeriod) sim.run(self.samplingPeriod * self.samples, printProgress=False) t = sensor.rawMeasurements.timestamps for sensor in imu.sensors: expected[sensor].append(sensor.trueValues(t)) measured[sensor].append(sensor.rawMeasurements.values) for sensor in imu.sensors: calibration[sensor] = ScaleAndOffsetCalibration.fromMeasurements( np.hstack(expected[sensor]), np.hstack(measured[sensor])) return calibration
def calibrate(self, imu): calibration = {} position = vectors.vector(0,0,0) expected = {} measured = {} for sensor in imu.sensors: expected[sensor] = [] measured[sensor] = [] for trajectory in self.testTrajectories(): sim = Simulation(environment=self.environment) sim.time = 0 imu.simulation = sim imu.trajectory = trajectory BasicIMUBehaviour(imu, self.samplingPeriod) sim.run(self.samplingPeriod * self.samples, printProgress=False) t = sensor.rawMeasurements.timestamps for sensor in imu.sensors: expected[sensor].append(sensor.trueValues(t)) measured[sensor].append(sensor.rawMeasurements.values) for sensor in imu.sensors: calibration[sensor] = ScaleAndOffsetCalibration.fromMeasurements( np.hstack(expected[sensor]), np.hstack(measured[sensor])) return calibration
def __init__(self, magnitude=50e-6, inclination=70, declination=0): """ Construct field model. Default parameters result in a field with a magnitude of 50 microTesla, a 70 degree downwards inclination and 0 degrees declination. These defaults are based on the nominal values for Edinburgh, UK, with declination set to 0 degrees so that magnetic and true North are equivalent. @param magnitude: Magnitude of the field, in Teslas. @param inclination: Inclination of the field to the horizontal, in degrees. @param declination: Delcination of the field from true North, in degrees. """ theta = math.radians(declination) ctheta = math.cos(theta) stheta = math.sin(theta) phi = math.radians(inclination) cphi = math.cos(phi) sphi = math.sin(phi) value = magnitude * vectors.vector(cphi*ctheta, cphi*stheta, sphi) ConstantVectorField.__init__(self, value)
def checkBaseField(field, nominal): assert_almost_equal(field.nominalValue, nominal) assert_almost_equal(field(vector(0,0,0), 0), nominal)
# 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/>. 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 dt = 0.01 GRAVITY = vector(0,0,1) NORTH = vector(1,0,0) filterParameters = { orientation.OrientCF : { 'k' : 1, 'aT' : 1}, orientation.BachmannCF : { 'k' : 1}, orientation.DistLinAccelCF : { 'k' : 1, 'joint' : Joint(None), 'offset' : np.zeros((3,1)), 'gravFieldReference' : GRAVITY, 'magFieldReference' : NORTH },
assert_almost_equal(transforms.polarToCartesian(*polar), cartesian) def checkCartesianToPolar(cartesian, polar): assert_almost_equal(transforms.cartesianToPolar(*cartesian), polar) def testCartesianPolarConversion(): for testParams in CARTESIAN_POLAR_TEST_VALUES: cartesian, polar = testParams yield checkPolarToCartesian, cartesian, polar yield checkCartesianToPolar, cartesian, polar # each element of the list is a tuple of vectors in NED and CG co-ords NED_CG_TEST_VALUES = [(vector(1, 0, 0), vector(0, 0, 1)), (vector(0, 1, 0), vector(-1, 0, 0)), (vector(0, 0, 1), vector(0, -1, 0))] def checkNED_to_CG(ned, cg): assert_almost_equal(transforms.convertNEDtoCG(ned), cg) def checkCG_to_NED(ned, cg): assert_almost_equal(transforms.convertCGtoNED(cg), ned) def testCoordinateChange(): for ned, cg in NED_CG_TEST_VALUES: yield checkNED_to_CG, ned, cg
class DistLinAccelCF(OrientationFilter): """ Implementation of the multi-IMU orientation algorithm by Young & Ling. See A. D. Young, M. J. Ling and D. K. Arvind. "Distributed Estimation of Linear Acceleration for Improved Accuracy in Wireless Inertial Motion Capture", in Proc. 9th ACM/IEEE International Conference on Information Processing in Sensor Networks, ACM, 2010, pp. 256-267. @ivar qMeas: Result of latest vector observation (L{Quaternion}). @ivar vectorObservation: L{TimeSeries} of vector observation results. @ivar jointAccel: Latest estimate of joint acceleration (3x1 L{np.ndarray}). @ivar jointAcceleration: L{TimeSeries} of acceleration estimates. """ GRAVITY_VECTOR = vectors.vector(0, 0, STANDARD_GRAVITY) @prepend_method_doc(BachmannCF) def __init__(self, initialTime, initialRotation, k, joint, offset, **kwargs): """ @param joint: The L{Joint} the IMU is attached to. @param offset: The offset of the IMU in the joint's co-ordinate frame (3x1 L{np.ndarray}) """ OrientationFilter.__init__(self, initialTime, initialRotation) self.rotation.add(initialTime, initialRotation) self.qHat = initialRotation.copy() self.k = k self.joint = joint self.offset = offset self._vectorObservation = vector_observation.GramSchmidt() self.qMeas = Quaternion.nan() self.jointAccel = vectors.nan() self.gyroFIFO = collections.deque([], maxlen=3) self.accelFIFO = collections.deque([], maxlen=2) self.magFIFO = collections.deque([], maxlen=2) self.isRoot = not joint.hasParent self.children = joint.childJoints self.vectorObservation = TimeSeries() self.jointAcceleration = TimeSeries() def handleLinearAcceleration(self, jointAcceleration, dt): """ Perform drift correction based on incoming joint acceleration estimate. @param jointAcceleration: Acceleration estimate (3x1 L{np.ndarray}). """ self.jointAccel = jointAcceleration if len(self.gyroFIFO) < 3: return # Estimate linear acceleration o = self.offset w = self.gyroFIFO q = self.qHat_minus_1 a = (w[0] - w[2]) / (2 * dt) lt = vectors.cross(a, o) lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2 l = (q.rotateFrame(self.jointAccel) + lt + lr) # Apply drift correction self.qMeas = self._vectorObservation(-(self.accelFIFO[1] - l), self.magFIFO[1]) if q.dot(self.qMeas) < 0: self.qMeas.negate() q = q + 1.0 / self.k * dt * (self.qMeas - q) dotq = 0.5 * self.qHat * Quaternion(0, *w[0]) self.qHat = q + dotq * dt self.qHat.normalise() def childAcceleration(self, o, dt): """ Compute acceleration for child joint. @param o: Offset of child joint (3x1 L{np.ndarray}). """ w = self.gyroFIFO if len(w) < 3: return None q = self.qHat_minus_1 a = (w[0] - w[2]) / (2 * dt) lt = vectors.cross(a, o) lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2 l = self.jointAccel + q.rotateVector(lt + lr) return l def _update(self, accel, mag, gyro, dt, t): # Store measurements in queues for processing self.gyroFIFO.appendleft(gyro) self.accelFIFO.appendleft(accel) self.magFIFO.appendleft(mag) # save current qHat for drift correction self.qHat_minus_1 = copy.copy(self.qHat) # Inertial update dotq = 0.5 * self.qHat * Quaternion(0, *gyro) self.qHat += dotq * dt self.qHat.normalise() if self.isRoot and len(self.accelFIFO) >= 2: # Root can't receive acceleration data so must estimate it. measuredAccel = self.qHat_minus_1.rotateVector(self.accelFIFO[1]) # Assume root joint acceleration was zero. self.jointAccel = np.zeros((3, 1)) # Calculate tangential acceleration for root joint IMU offset. linAccel = self.childAcceleration(self.offset, dt) if linAccel is not None: # Calculate expected measurement. expectedAccel = linAccel - self.GRAVITY_VECTOR # Set root joint acceleration to the residual. self.jointAccel = measuredAccel - expectedAccel self.handleLinearAcceleration(self.jointAccel, dt) self.rotation.add(t, self.qHat) self.vectorObservation.add(t, self.qMeas) self.jointAcceleration.add(t, self.jointAccel)
def loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod): """ Load motion capture data from an ASF and AMC file pair. @param asfFileName: Name of the ASF file containing the description of the rigid body model @param amcFileName: Name of the AMC file containing the motion data @param scaleFactor: Scaling factor to convert lengths to m. For data from the CMU motion capture corpus this should be 2.54/100 to convert from inches to metres. @return: A {SampledBodyModel} representing the root of the rigid body model structure. """ with open(asfFileName, 'r') as asfFile: data = asfParser.parseFile(asfFile) scale = (1.0/data.units.get('length',1)) * scaleFactor bones = dict((bone.name,bone) for bone in data.bones) asfModel = ASFRoot(data.root) for entry in data.hierarchy: parent = asfModel.getBone(entry.parent) for childName in entry.children: ASFBone(parent, bones[childName], scale) imusimModel = SampledBodyModel('root') for subtree in asfModel.children: for bone in subtree: if not bone.isDummy: offset = vector(0,0,0) ancestors = bone.ascendTree() while True: ancestor = ancestors.next().parent offset += ancestor.childoffset if not ancestor.isDummy: break SampledJoint(parent=imusimModel.getJoint(ancestor.name), name=bone.name, offset=offset) if not bone.hasChildren: PointTrajectory( parent=imusimModel.getJoint(bone.name), name=bone.name+'_end', offset=bone.childoffset ) with open(amcFileName) as amcFile: motion = amcParser.parseFile(amcFile) t = 0 for frame in motion.frames: for bone in frame.bones: bonedata = asfModel.getBone(bone.name) if bone.name == 'root': data = dict((chan.lower(), v) for chan,v in zip(bonedata.channels,bone.channels)) position = convertCGtoNED(scale * vector(data['tx'], data['ty'],data['tz'])) imusimModel.positionKeyFrames.add(t, position) axes, angles = zip(*[(chan[-1], angle) for chan, angle in zip(bonedata.channels, bone.channels) if chan.lower().startswith('r')]) rotation = (bonedata.rotationOffset.conjugate * Quaternion.fromEuler(angles[::-1], axes[::-1])) joint = imusimModel.getJoint(bone.name) if joint.hasParent: parentRot = joint.parent.rotationKeyFrames.latestValue parentRotOffset = bonedata.parent.rotationOffset rotation = parentRot * parentRotOffset * rotation else: rotation = convertCGtoNED(rotation) joint.rotationKeyFrames.add(t, rotation) t += framePeriod return imusimModel
def noiseVoltages(self, t): return vector(0,0,0)
def loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod): """ Load motion capture data from an ASF and AMC file pair. @param asfFileName: Name of the ASF file containing the description of the rigid body model @param amcFileName: Name of the AMC file containing the motion data @param scaleFactor: Scaling factor to convert lengths to m. For data from the CMU motion capture corpus this should be 2.54/100 to convert from inches to metres. @return: A {SampledBodyModel} representing the root of the rigid body model structure. """ with open(asfFileName, 'r') as asfFile: data = asfParser.parseFile(asfFile) scale = (1.0 / data.units.get('length', 1)) * scaleFactor bones = dict((bone.name, bone) for bone in data.bones) asfModel = ASFRoot(data.root) for entry in data.hierarchy: parent = asfModel.getBone(entry.parent) for childName in entry.children: ASFBone(parent, bones[childName], scale) imusimModel = SampledBodyModel('root') for subtree in asfModel.children: for bone in subtree: if not bone.isDummy: offset = vector(0, 0, 0) ancestors = bone.ascendTree() while True: ancestor = ancestors.next().parent offset += ancestor.childoffset if not ancestor.isDummy: break SampledJoint(parent=imusimModel.getJoint(ancestor.name), name=bone.name, offset=offset) if not bone.hasChildren: PointTrajectory(parent=imusimModel.getJoint(bone.name), name=bone.name + '_end', offset=bone.childoffset) with open(amcFileName) as amcFile: motion = amcParser.parseFile(amcFile) t = 0 for frame in motion.frames: for bone in frame.bones: bonedata = asfModel.getBone(bone.name) if bone.name == 'root': data = dict((chan.lower(), v) for chan, v in zip( bonedata.channels, bone.channels)) position = convertCGtoNED( scale * vector(data['tx'], data['ty'], data['tz'])) imusimModel.positionKeyFrames.add(t, position) axes, angles = zip(*[(chan[-1], angle) for chan, angle in zip( bonedata.channels, bone.channels) if chan.lower().startswith('r')]) rotation = (bonedata.rotationOffset.conjugate * Quaternion.fromEuler(angles[::-1], axes[::-1])) joint = imusimModel.getJoint(bone.name) if joint.hasParent: parentRot = joint.parent.rotationKeyFrames.latestValue parentRotOffset = bonedata.parent.rotationOffset rotation = parentRot * parentRotOffset * rotation else: rotation = convertCGtoNED(rotation) joint.rotationKeyFrames.add(t, rotation) t += framePeriod return imusimModel
# 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/>. from imusim.environment.magnetic_fields import EarthMagneticField from imusim.environment.magnetic_fields import DistortedMagneticField from imusim.environment.magnetic_fields import SolenoidMagneticField from numpy.testing import assert_almost_equal from imusim.maths.vectors import vector from imusim.maths.transforms import AffineTransform from numpy import random # Tuple of constructor args and nominal field vectors for Earth fields TEST_PARAMS = [ ((1, 0, 0), vector(1,0,0)), ((1, 90, 0), vector(0,0,1)), ((1, 0, 90), vector(0,1,0)) ] def checkBaseField(field, nominal): assert_almost_equal(field.nominalValue, nominal) assert_almost_equal(field(vector(0,0,0), 0), nominal) def testEarthField(): for args, nominal in TEST_PARAMS: yield checkBaseField, EarthMagneticField(*args), nominal def checkZeroHeadingVariation(field, position): assert_almost_equal(field.headingVariation(position, 0), 0)
def checkBaseField(field, nominal): assert_almost_equal(field.nominalValue, nominal) assert_almost_equal(field(vector(0, 0, 0), 0), nominal)
# 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/>. from imusim.environment.magnetic_fields import EarthMagneticField from imusim.environment.magnetic_fields import DistortedMagneticField from imusim.environment.magnetic_fields import SolenoidMagneticField from numpy.testing import assert_almost_equal from imusim.maths.vectors import vector from imusim.maths.transforms import AffineTransform from numpy import random # Tuple of constructor args and nominal field vectors for Earth fields TEST_PARAMS = [((1, 0, 0), vector(1, 0, 0)), ((1, 90, 0), vector(0, 0, 1)), ((1, 0, 90), vector(0, 1, 0))] def checkBaseField(field, nominal): assert_almost_equal(field.nominalValue, nominal) assert_almost_equal(field(vector(0, 0, 0), 0), nominal) def testEarthField(): for args, nominal in TEST_PARAMS: yield checkBaseField, EarthMagneticField(*args), nominal def checkZeroHeadingVariation(field, position): assert_almost_equal(field.headingVariation(position, 0), 0)
] def checkPolarToCartesian(cartesian, polar): assert_almost_equal(transforms.polarToCartesian(*polar), cartesian) def checkCartesianToPolar(cartesian, polar): assert_almost_equal(transforms.cartesianToPolar(*cartesian), polar) def testCartesianPolarConversion(): for testParams in CARTESIAN_POLAR_TEST_VALUES: cartesian, polar = testParams yield checkPolarToCartesian, cartesian, polar yield checkCartesianToPolar, cartesian, polar # each element of the list is a tuple of vectors in NED and CG co-ords NED_CG_TEST_VALUES = [ (vector(1,0,0), vector(0,0,1)), (vector(0,1,0), vector(-1,0,0)), (vector(0,0,1), vector(0,-1,0)) ] def checkNED_to_CG(ned, cg): assert_almost_equal(transforms.convertNEDtoCG(ned), cg) def checkCG_to_NED(ned, cg): assert_almost_equal(transforms.convertCGtoNED(cg), ned) def testCoordinateChange(): for ned, cg in NED_CG_TEST_VALUES: yield checkNED_to_CG, ned, cg yield checkCG_to_NED, ned, cg
# 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/>. 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 dt = 0.01 GRAVITY = vector(0, 0, 1) NORTH = vector(1, 0, 0) filterParameters = { orientation.OrientCF: { 'k': 1, 'aT': 1 }, orientation.BachmannCF: { 'k': 1 }, orientation.DistLinAccelCF: { 'k': 1, 'joint': Joint(None), 'offset': np.zeros((3, 1)), 'gravFieldReference': GRAVITY,