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
