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 rotation(self, t): if len(self.rotationKeyFrames) == 0: if np.ndim(t) == 0: return Quaternion.nan() else: return QuaternionArray.nan(len(t)) else: return self.rotationKeyFrames(t)
def _update(self, accel, mag, gyro, dt, t): dotq = 0.5 * self.qHat * Quaternion(0, *gyro) self.qHat += dotq * dt if abs(vectors.norm(accel) - 1) < self._aT: qMeas = self._vectorObservation(-accel, mag) if self.qHat.dot(qMeas) < 0: qMeas.negate() qError = qMeas - self.qHat self.qHat += (1 / self._k) * dt * qError else: qMeas = Quaternion.nan() self.vectorObservation.add(t, qMeas) self.qHat.normalise() self.rotation.add(t, self.qHat)
def _update(self, accel, mag, gyro, dt, t): dotq = 0.5 * self.qHat * Quaternion(0,*gyro) self.qHat += dotq * dt if abs(vectors.norm(accel) - 1) < self._aT: qMeas = self._vectorObservation(-accel, mag) if self.qHat.dot(qMeas) < 0: qMeas.negate() qError = qMeas - self.qHat self.qHat += (1/self._k) * dt * qError else: qMeas = Quaternion.nan() self.vectorObservation.add(t, qMeas) self.qHat.normalise() self.rotation.add(t, self.qHat)