def calc_orientation(self, R_initialOrientation, type='quatInt'):
'''
Calculate the current orientation
Parameters
----------
R_initialOrientation : 3x3 array
approximate alignment of sensor-CS with space-fixed CS
type : string
- 'quatInt' .... quaternion integration of angular velocity
- 'Kalman' ..... quaternion Kalman filter
- 'Madgwick' ... gradient descent method, efficient
- 'Mahony' .... formula from Mahony, as implemented by Madgwick
'''
initialPosition = np.r_[0, 0, 0]
if type == 'quatInt':
(quaternion, position) = calc_QPos(R_initialOrientation,
self.omega, initialPosition,
self.acc, self.rate)
elif type == 'Kalman':
self._checkRequirements()
quaternion = kalman_quat(self.rate, self.acc, self.omega, self.mag)
elif type == 'Madgwick':
self._checkRequirements()
# Initialize object
AHRS = MadgwickAHRS(SamplePeriod=1. / self.rate, Beta=0.1)
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples),
'Calculating the Quaternions ', 25):
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
elif type == 'Mahony':
self._checkRequirements()
# Initialize object
AHRS = MahonyAHRS(SamplePeriod=1. / np.float(self.rate), Kp=0.5)
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples),
'Calculating the Quaternions ', 25):
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
else:
print('Unknown orientation type: {0}'.format(type))
return
self.quat = quaternion
def test_progressbar(self):
'''Just run it through once'''
for ii in misc.progressbar(range(50), 'Computing ', 25):
#print(ii)
sleep(0.05)
def test_progressbar(self):
'''Just run it through once'''
for ii in misc.progressbar(range(50), 'Computing ', 25):
#print(ii)
sleep(0.05)
def calc_orientation(self, R_initialOrientation, type='quatInt'):
'''
Calculate the current orientation
Parameters
----------
R_initialOrientation : 3x3 array
approximate alignment of sensor-CS with space-fixed CS
type : string
- 'quatInt' .... quaternion integration of angular velocity
- 'Kalman' ..... quaternion Kalman filter
- 'Madgwick' ... gradient descent method, efficient
- 'Mahony' .... formula from Mahony, as implemented by Madgwick
'''
initialPosition = np.r_[0,0,0]
if type == 'quatInt':
(quaternion, position) = calc_QPos(R_initialOrientation, self.omega, initialPosition, self.acc, self.rate)
elif type == 'Kalman':
self._checkRequirements()
quaternion = kalman_quat(self.rate, self.acc, self.omega, self.mag)
elif type == 'Madgwick':
self._checkRequirements()
# Initialize object
AHRS = MadgwickAHRS(SamplePeriod=1./self.rate, Beta=0.1)
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples), 'Calculating the Quaternions ', 25) :
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
elif type == 'Mahony':
self._checkRequirements()
# Initialize object
AHRS = MahonyAHRS(SamplePeriod=1./self.rate, Kp=0.5)
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples), 'Calculating the Quaternions ', 25) :
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
else:
print('Unknown orientation type: {0}'.format(type))
return
self.quat = quaternion
def _calc_orientation(self):
'''
Calculate the current orientation
Parameters
----------
type : string
- 'analytical' .... quaternion integration of angular velocity
- 'kalman' ..... quaternion Kalman filter
- 'madgwick' ... gradient descent method, efficient
- 'mahony' .... formula from Mahony, as implemented by Madgwick
'''
initialPosition = np.r_[0, 0, 0]
method = self._q_type
if method == 'analytical':
(quaternion, position) = analytical(self.R_init, self.omega,
initialPosition, self.acc,
self.rate)
elif method == 'kalman':
self._checkRequirements()
quaternion = kalman(self.rate, self.acc, self.omega, self.mag)
elif method == 'madgwick':
self._checkRequirements()
# Initialize object
AHRS = Madgwick(SamplePeriod=1. / self.rate, Beta=1.5)
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples),
'Calculating the Quaternions ', 25):
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
elif method == 'mahony':
self._checkRequirements()
# Initialize object
AHRS = Mahony(SamplePeriod=1. / np.float(self.rate), Kp=0.5)
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples),
'Calculating the Quaternions ', 25):
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
else:
print('Unknown orientation type: {0}'.format(method))
return
self.quat = quaternion