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 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