Source code for imus

'''
Abstract base class for analyzing movements recordings with inertial measurement units (IMUs)

The advantage of this approach is that it allows to write code that is independent of the IMU-sensor. All IMUs provide acceleration and angular velocities, and most of them also the direction of the local magnetic field. The specifics of each sensor are hidden in the sensor-object (specifically, in the "get\_data" method which has to be implemented once for each sensor). Initialization of a sensor object includes a number of activities:
        - Reading in the data.
        - Making acceleration, angular\_velocity etc. accessible in a sensor-independent way
        - Calculating duration, totalSamples, etc.
        - Calculating orientation (expressed as "quat"), with the method specified in "q\_type"
'''

'''
Author: Thomas Haslwanter
'''

import numpy as np
import matplotlib.pyplot as plt
import pandas as pd 
import scipy as sp
from scipy import constants     # for "g"
from scipy.integrate import cumtrapz
import re

# The following construct is required since I want to run the module as a script
# inside the skinematics-directory
import os
import sys
sys.path.append( os.path.join( os.path.dirname(__file__), os.path.pardir ) ) 

from skinematics import quat, vector, misc, rotmat

# For deprecation warnings
# import deprecation
import warnings

# For the definition of the abstract base class IMU_Base
import abc

[docs]class IMU_Base(metaclass=abc.ABCMeta): ''' Abstract BaseClass for working with working with inertial measurement units (IMUs) A concrete class must be instantiated, which implements "get_data". (See example below.) Attributes: acc (Nx3 array) : 3D linear acceleration dataType (string) : Type of data (commonly float) duration (float) : Duration of recording [sec] mag (Nx3 array) : 3D orientation of local magnectic field omega (Nx3 array) : 3D angular velocity pos_init (3-vector) : Initial position. default is np.ones(3) quat (Nx4 array) : q_type (string) : Method of calculation for orientation quaternion rate (int) : Sampling rate R_init (3x3 array) : Rotation matrix defining the initial orientation. Default is np.eye(3) source (str) : Name of data-file totalSamples (int) : Number of samples Parameters ---------- inFile : string path- and file-name of data file / input source inData : dictionary The following fields are required: acc : (N x 3) array Linear acceleration [m/s^2], in the x-, y-, and z-direction omega : (N x 3) array Angular velocity [deg/s], about the x-, y-, and z-axis [mag] : (N x 3) array (optional) Local magnetic field, in the x-, y-, and z-direction rate: integer sample rate [Hz] Examples -------- >>> # Set the in-file, initial sensor orientation >>> in_file = r'tests/data/data_xsens.txt' >>> initial_orientation = np.array([[1,0,0], >>> [0,0,-1], >>> [0,1,0]]) >>> >>> # Choose a sensor >>> from skinematics.sensors.xsens import XSens >>> sensor = XSens(in_file=in_file, R_init=initial_orientation) >>> >>> # By default, the orientation quaternion gets automatically calculated, using the option "analytical" >>> q_analytical = sensor.quat >>> >>> # Automatic re-calculation of orientation if "q_type" is changed >>> sensor.q_type = 'madgwick' >>> q_Madgwick = sensor.quat >>> >>> sensor.q_type = 'kalman' >>> q_Kalman = sensor.quat >>> >>> # Demonstrate how to fill up a sensor manually >>> in_data = {'rate':sensor.rate, >>> 'acc': sensor.acc, >>> 'omega':sensor.omega, >>> 'mag':sensor.mag} >>> my_sensor = MyOwnSensor(in_file='My own 123 sensor.', in_data=in_data) '''
[docs] @abc.abstractmethod def get_data(self, in_file=None, in_data=None): """Retrieve "rate", "acc", "omega", "mag" from the input source and set the corresponding values of "self". With some sensors, "rate" has to be provided, and is taken from "in_data". """
def __init__(self, in_file = None, q_type='analytical', R_init = np.eye(3), pos_init = np.zeros(3), in_data = None ): """Initialize an IMU-object. Note that this includes a number of activities: - Read in the data - Make acceleration, angular_velocity etc. accessible, in a sensor-independent way - Calculates duration, totalSamples, etc - Calculates orientation (expressed as "quat"), with the method specified in "q_type" in_file : string Location of infile / input q_type : string Determines how the orientation gets calculated: - 'analytical' .... quaternion integration of angular velocity - 'kalman' ..... quaternion Kalman filter - 'madgwick' ... gradient descent method, efficient - 'mahony' .... formula from Mahony, as implemented by Madgwick - 'None' ... data are only read in, no orientation calculated R_init : 3x3 array approximate alignment of sensor-CS with space-fixed CS currently only used in "analytical" pos_init : (,3) vector initial position currently only used in "analytical" in_data : dictionary If the data are provided directly, not from a file Also used to provide "rate" for "polulu" sensors. """ if in_data is None and in_file is None: raise ValueError('Either in_data or in_file must be provided.') elif in_file is None: # Get the data from "in_data" # In that case, "in_data" # - must contain the fields ['acc', 'omega'] # - can contain the fields ['rate', 'mag'] # self.source is set to "None" self._set_data(in_data) else: # Set rate, acc, omega, mag # Note: this is implemented in the concrete class, implenented in # the corresponding module in "sensors" self.source = in_file self.get_data(in_file, in_data) # Set information not determined by the IMU-data self.R_init = R_init self.pos_init = pos_init # Set the analysis method, and consolidate the object (see below) # This also means calculating the orientation quaternion!! self.q_type = q_type @property def q_type(self): """q_type determines how the orientation is calculated. If "q_type" is "None", no orientation gets calculated; otherwise, the orientation calculation is performed with "_calc_orientation", using the option "q_type". It has to be one of the following values: * analytical * kalman * madgwick * mahony * None """ return self._q_type @q_type.setter def q_type(self, value): allowed_values = ['analytical', 'kalman', 'madgwick', 'mahony', None] if value in allowed_values: self._q_type = value if value == None: self.quat = None else: self._calc_orientation() else: raise ValueError('q_type must be one of the following: {0}, not {1}'.format(allowed_values, value)) def _set_data(self, data): # Set the properties of an IMU-object directly if 'rate' not in data.keys(): print('Set the "rate" to the default value (100 Hz).') data['rate'] = 100.0 self.rate = data['rate'] self.acc= data['acc'] self.omega = data['omega'] if 'mag' in data.keys(): self.mag = data['mag'] self.source = None self._set_info() 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
[docs] def calc_position(self): '''Calculate the position, assuming that the orientation is already known.''' initialPosition = self.pos_init # Acceleration, velocity, and position ---------------------------- # From q and the measured acceleration, get the \frac{d^2x}{dt^2} g = constants.g g_v = np.r_[0, 0, g] accReSensor = self.acc - vector.rotate_vector(g_v, quat.q_inv(self.quat)) accReSpace = vector.rotate_vector(accReSensor, self.quat) # Position and Velocity through integration, assuming 0-velocity at t=0 vel = np.nan*np.ones_like(accReSpace) pos = np.nan*np.ones_like(accReSpace) for ii in range(accReSpace.shape[1]): vel[:,ii] = cumtrapz(accReSpace[:,ii], dx=1./np.float(self.rate), initial=0) pos[:,ii] = cumtrapz(vel[:,ii], dx=1./np.float(self.rate), initial=initialPosition[ii]) self.pos = pos
def _checkRequirements(self): '''Check if all the necessary variables are available.''' required = [ 'rate', 'acc', 'omega', 'mag' ] for field in required: if field not in vars(self): print('Cannot find {0} in calc_orientation!'.format(field)) def _set_info(self): '''Complete the information properties of that IMU''' self.totalSamples = len(self.omega) self.duration = np.float(self.totalSamples)/self.rate # [sec] self.dataType = str(self.omega.dtype)
[docs]def analytical(R_initialOrientation=np.eye(3), omega=np.zeros((5,3)), initialPosition=np.zeros(3), accMeasured=np.column_stack((np.zeros((5,2)), 9.81*np.ones(5))), rate=100): ''' Reconstruct position and orientation with an analytical solution, from angular velocity and linear acceleration. Assumes a start in a stationary position. No compensation for drift. Parameters ---------- R_initialOrientation: ndarray(3,3) Rotation matrix describing the initial orientation of the sensor, except a mis-orienation with respect to gravity omega : ndarray(N,3) Angular velocity, in [rad/s] initialPosition : ndarray(3,) initial Position, in [m] accMeasured : ndarray(N,3) Linear acceleration, in [m/s^2] rate : float sampling rate, in [Hz] Returns ------- q : ndarray(N,3) Orientation, expressed as a quaternion vector pos : ndarray(N,3) Position in space [m] Example ------- >>> q1, pos1 = analytical(R_initialOrientation, omega, initialPosition, acc, rate) ''' # Transform recordings to angVel/acceleration in space -------------- # Orientation of \vec{g} with the sensor in the "R_initialOrientation" g = 9.81 g0 = np.linalg.inv(R_initialOrientation).dot(np.r_[0,0,g]) # for the remaining deviation, assume the shortest rotation to there q0 = vector.q_shortest_rotation(accMeasured[0], g0) q_initial = rotmat.convert(R_initialOrientation, to='quat') # combine the two, to form a reference orientation. Note that the sequence # is very important! q_ref = quat.q_mult(q_initial, q0) # Calculate orientation q by "integrating" omega ----------------- q = quat.calc_quat(omega, q_ref, rate, 'bf') # Acceleration, velocity, and position ---------------------------- # From q and the measured acceleration, get the \frac{d^2x}{dt^2} g_v = np.r_[0, 0, g] accReSensor = accMeasured - vector.rotate_vector(g_v, quat.q_inv(q)) accReSpace = vector.rotate_vector(accReSensor, q) # Make the first position the reference position q = quat.q_mult(q, quat.q_inv(q[0])) # compensate for drift #drift = np.mean(accReSpace, 0) #accReSpace -= drift*0.7 # Position and Velocity through integration, assuming 0-velocity at t=0 vel = np.nan*np.ones_like(accReSpace) pos = np.nan*np.ones_like(accReSpace) for ii in range(accReSpace.shape[1]): vel[:,ii] = cumtrapz(accReSpace[:,ii], dx=1./rate, initial=0) pos[:,ii] = cumtrapz(vel[:,ii], dx=1./rate, initial=initialPosition[ii]) return (q, pos)
[docs]def kalman(rate, acc, omega, mag): ''' Calclulate the orientation from IMU magnetometer data. Parameters ---------- rate : float sample rate [Hz] acc : (N,3) ndarray linear acceleration [m/sec^2] omega : (N,3) ndarray angular velocity [rad/sec] mag : (N,3) ndarray magnetic field orientation Returns ------- qOut : (N,4) ndarray unit quaternion, describing the orientation relativ to the coordinate system spanned by the local magnetic field, and gravity Notes ----- Based on "Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking" Yun, X. and Bachman, E.R., IEEE TRANSACTIONS ON ROBOTICS, VOL. 22, 1216-1227 (2006) ''' numData = len(acc) # Set parameters for Kalman Filter tstep = 1./rate tau = [0.5, 0.5, 0.5] # from Yun, 2006 # Initializations x_k = np.zeros(7) # state vector z_k = np.zeros(7) # measurement vector z_k_pre = np.zeros(7) P_k = np.matrix( np.eye(7) ) # error covariance matrix P_k Phi_k = np.matrix( np.zeros((7,7)) ) # discrete state transition matrix Phi_k for ii in range(3): Phi_k[ii,ii] = np.exp(-tstep/tau[ii]) H_k = np.matrix( np.eye(7) ) # Identity matrix Q_k = np.matrix( np.zeros((7,7)) ) # process noise matrix Q_k D = 0.0001*np.r_[0.4, 0.4, 0.4] # [rad^2/sec^2]; from Yun, 2006 # check 0.0001 in Yun for ii in range(3): Q_k[ii,ii] = D[ii]/(2*tau[ii]) * ( 1-np.exp(-2*tstep/tau[ii]) ) # Evaluate measurement noise covariance matrix R_k R_k = np.matrix( np.zeros((7,7)) ) r_angvel = 0.01; # [rad**2/sec**2]; from Yun, 2006 r_quats = 0.0001; # from Yun, 2006 for ii in range(7): if ii<3: R_k[ii,ii] = r_angvel else: R_k[ii,ii] = r_quats # Calculation of orientation for every time step qOut = np.zeros( (numData,4) ) for ii in range(numData): accelVec = acc[ii,:] magVec = mag[ii,:] angvelVec = omega[ii,:] z_k_pre = z_k.copy() # watch out: by default, Python passes the reference!! # Evaluate quaternion based on acceleration and magnetic field data accelVec_n = vector.normalize(accelVec) magVec_hor = magVec - accelVec_n * accelVec_n.dot(magVec) magVec_n = vector.normalize(magVec_hor) basisVectors = np.vstack( (magVec_n, np.cross(accelVec_n, magVec_n), accelVec_n) ).T quatRef = quat.q_inv(rotmat.convert(basisVectors, to='quat')).flatten() # Update measurement vector z_k z_k[:3] = angvelVec z_k[3:] = quatRef # Calculate Kalman Gain # K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k) # Check: why is H_k used in the original formulas? K_k = P_k * np.linalg.inv(P_k + R_k) # Update state vector x_k x_k += np.array( K_k.dot(z_k-z_k_pre) ).ravel() # Evaluate discrete state transition matrix Phi_k Phi_k[3,:] = np.r_[-x_k[4]*tstep/2, -x_k[5]*tstep/2, -x_k[6]*tstep/2, 1, -x_k[0]*tstep/2, -x_k[1]*tstep/2, -x_k[2]*tstep/2] Phi_k[4,:] = np.r_[ x_k[3]*tstep/2, -x_k[6]*tstep/2, x_k[5]*tstep/2, x_k[0]*tstep/2, 1, x_k[2]*tstep/2, -x_k[1]*tstep/2] Phi_k[5,:] = np.r_[ x_k[6]*tstep/2, x_k[3]*tstep/2, -x_k[4]*tstep/2, x_k[1]*tstep/2, -x_k[2]*tstep/2, 1, x_k[0]*tstep/2] Phi_k[6,:] = np.r_[-x_k[5]*tstep/2, x_k[4]*tstep/2, x_k[3]*tstep/2, x_k[2]*tstep/2, x_k[1]*tstep/2, -x_k[0]*tstep/2, 1] # Update error covariance matrix #P_k = (eye(7)-K_k*H_k)*P_k # Check: why is H_k used in the original formulas? P_k = (H_k - K_k) * P_k # Projection of state quaternions x_k[3:] += quat.q_mult(0.5*x_k[3:],np.r_[0, x_k[:3]]).flatten() x_k[3:] = vector.normalize( x_k[3:] ) x_k[:3] = np.zeros(3) x_k[:3] = tstep * (-x_k[:3]+z_k[:3]) qOut[ii,:] = x_k[3:] # Projection of error covariance matrix P_k = Phi_k * P_k * Phi_k.T + Q_k # Make the first position the reference position qOut = quat.q_mult(qOut, quat.q_inv(qOut[0])) return qOut
[docs]class Madgwick: '''Madgwick's gradient descent filter. Parameters ---------- SamplePeriod : double sample period [s] Beta : double algorithm gain Quaternion : array, shape (N,4) output quaternion describing the Earth relative to the sensor ''' def __init__(self, SamplePeriod=1./256, Beta=1.0, Quaternion=[1,0,0,0]): '''Initialization ''' self.SamplePeriod = SamplePeriod self.Beta = Beta self.Quaternion = Quaternion
[docs] def Update(self, Gyroscope, Accelerometer, Magnetometer): '''Calculate the best quaternion to the given measurement values. Parameters ---------- Gyroscope : array, shape (N,3) Angular velocity [rad/s] Accelerometer : array, shape (N,3) Linear acceleration (Only the direction is used, so units don't matter.) Magnetometer : array, shape (N,3) Orientation of local magenetic field. (Again, only the direction is used, so units don't matter.) ''' q = self.Quaternion; # short name local variable for readability # Reference direction of Earth's magnetic field h = vector.rotate_vector(Magnetometer, q) b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2])) # Gradient decent algorithm corrective step F = [2*(q[1]*q[3] - q[0]*q[2]) - Accelerometer[0], 2*(q[0]*q[1] + q[2]*q[3]) - Accelerometer[1], 2*(0.5 - q[1]**2 - q[2]**2) - Accelerometer[2], 2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]) - Magnetometer[0], 2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]) - Magnetometer[1], 2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - Magnetometer[2]] J = np.array([ [-2*q[2], 2*q[3], -2*q[0], 2*q[1]], [ 2*q[1], 2*q[0], 2*q[3], 2*q[2]], [0, -4*q[1], -4*q[2], 0], [-2*b[3]*q[2], 2*b[3]*q[3], -4*b[1]*q[2]-2*b[3]*q[0], -4*b[1]*q[3]+2*b[3]*q[1]], [-2*b[1]*q[3]+2*b[3]*q[1], 2*b[1]*q[2]+2*b[3]*q[0], 2*b[1]*q[1]+2*b[3]*q[3], -2*b[1]*q[0]+2*b[3]*q[2]], [ 2*b[1]*q[2], 2*b[1]*q[3]-4*b[3]*q[1], 2*b[1]*q[0]-4*b[3]*q[2], 2*b[1]*q[1]]]) step = J.T.dot(F) step = vector.normalize(step) # normalise step magnitude # Compute rate of change of quaternion qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])) - self.Beta * step # Integrate to yield quaternion q = q + qDot * self.SamplePeriod self.Quaternion = vector.normalize(q).flatten()
[docs]class Mahony: '''Madgwick's implementation of Mayhony's AHRS algorithm Parameters ---------- SamplePeriod : sample period Kp : algorithm proportional gain Ki : algorithm integral gain Quaternion : output quaternion describing the Earth relative to the sensor ''' def __init__(self, SamplePeriod=1./256, Kp=1.0, Ki=0, Quaternion=[1,0,0,0]): '''Initialization ''' self.SamplePeriod = SamplePeriod self.Kp = Kp self.Ki = Ki self.Quaternion = Quaternion self._eInt = [0, 0, 0] # integral error
[docs] def Update(self, Gyroscope, Accelerometer, Magnetometer): '''Calculate the best quaternion to the given measurement values. Parameters ---------- Gyroscope : array, shape (N,3) Angular velocity [rad/s] Accelerometer : array, shape (N,3) Linear acceleration (Only the direction is used, so units don't matter.) Magnetometer : array, shape (N,3) Orientation of local magenetic field. (Again, only the direction is used, so units don't matter.) ''' q = self.Quaternion; # short name local variable for readability # Reference direction of Earth's magnetic field h = vector.rotate_vector(Magnetometer, q) b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2])) # Estimated direction of gravity and magnetic field v = np.array([ 2*(q[1]*q[3] - q[0]*q[2]), 2*(q[0]*q[1] + q[2]*q[3]), q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2]) w = np.array([ 2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]), 2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]), 2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2)]) # Error is sum of cross product between estimated direction and measured direction of fields e = np.cross(Accelerometer, v) + np.cross(Magnetometer, w) if self.Ki > 0: self._eInt += e * self.SamplePeriod else: self._eInt = np.array([0, 0, 0], dtype=np.float) # Apply feedback terms Gyroscope += self.Kp * e + self.Ki * self._eInt; # Compute rate of change of quaternion qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])).flatten() # Integrate to yield quaternion q += qDot * self.SamplePeriod self.Quaternion = vector.normalize(q)
if __name__ == '__main__': ''' in_file = r'../others/skinematics-invalid-sqrt.txt' import pandas as pd data = pd.read_csv(in_file, delimiter='\t') omega = data.filter(regex='Gyr*').values acc = data.filter(regex='Acc*').values analytical(omega = omega, accMeasured=acc) ''' from sensors.xsens import XSens in_file = r'tests/data/data_xsens.txt' initial_orientation = np.array([ [1,0,0], [0,0,-1], [0,1,0]]) #in_file = r'tests/data/data_xsens2.txt' #initial_orientation = np.array([[0,0,-1], #[1, 0, 0], #[0,-1,0]]) initial_position = np.r_[0,0,0] sensor = XSens(in_file=in_file, R_init=initial_orientation, pos_init=initial_position) # By default, the orientation quaternion gets automatically calculated, using "analytical" q_analytical = sensor.quat # Automatic re-calculation of orientation if "q_type" is changed sensor.q_type = 'madgwick' q_Madgwick = sensor.quat sensor.q_type = 'kalman' q_Kalman = sensor.quat def show_result(imu_data): fig, axs = plt.subplots(3,1) axs[0].plot(imu_data.omega) axs[0].set_ylabel('Omega') axs[0].set_title(imu_data.q_type) axs[1].plot(imu_data.acc) axs[1].set_ylabel('Acc') axs[2].plot(imu_data.quat[:,1:]) axs[2].set_ylabel('Quat') plt.show() show_result(sensor) print('Done')