def update_imu_new(self, gyroscope, accelerometer): # Normalise accelerometer measurement if norm(accelerometer) is 0: warnings.warn("accelerometer is zero") return accelerometer /= norm(accelerometer) v = [2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]), 2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]), self.q[0]**2 - self.q[1]**2 - self.q[2]**2 + self.q[3]**2] error = np.cross(v, accelerometer.transpose()) self.IntError = self.IntError + error # Apply feedback terms ref = gyroscope - (self.beta*error).transpose() # Ref = Gyroscope - (obj.Kp*error + obj.Ki*obj.IntError)'; # Compute rate of change of quaternion pDot = Quaternion.__mul__(Quaternion.__mul__(self.q, Quaternion([0, ref[0], ref[1], ref[2]])), 0.5) # Compute rate of change of quaternion self.q = self.q + pDot * self.samplePeriod # Integrate rate of change of quaternion self.q = Quaternion(self.q / norm(self.q)) # Normalise quaternion # Store conjugate self.quaternion = Quaternion.conj(self.q)
def rot_point(arr, _quaternion): x = arr[0] y = arr[1] z = arr[2] u_q = Quaternion(0, x, y, z) _q_n = _quaternion._q / np.sqrt(_quaternion._q[0]**2 + _quaternion._q[1]**2 + _quaternion._q[2]**2 + _quaternion._q[3]**2) temp_q = Quaternion(_q_n) v_q = temp_q.__mul__(u_q) v_q = v_q.__mul__(temp_q.conj()) v = v_q._q return v