'''
Utilities for movements in 3D space
'''
'''
Author: Thomas Haslwanter
Version: 1.8
Date: April-2015
'''
from numpy.linalg import norm, inv
import numpy as np
import matplotlib.pyplot as plt
from numpy import matrix, r_, sum, pi, sqrt, zeros, shape, sin, tile, arange, float, array, nan, ones_like, rad2deg, deg2rad
from scipy.integrate import cumtrapz
from thLib import quat, vector, ui
import pandas as pd
import os
from scipy import constants
[docs]class IMU:
'''
Class for working with working with inertial measurement units (IMUs)
An IMU object can be initialized
- by giving a filename
- by providing measurement data and a samplerate
- without giving any parameter; in that case the user is prompted
to select an infile
Parameters
----------
inFile : string
path- and file-name of infile, if you get the sound from a file.
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]
Notes
-----
IMU-Properties:
- source
- acc
- omega
- mag
- rate
- totalSamples
- duration
- dataType
Examples
--------
>>> myimu = IMU(r'.\Data\Walking_02.txt')
>>>
>>> initialOrientation = np.array([[1,0,0],
>>> [0,0,-1],
>>> [0,1,0]])
>>> initialPosition = np.r_[0,0,0]
>>>
>>> myimu.calc_orientation(initialOrientation)
>>> q_simple = myimu.quat[:,1:]
>>>
>>> calcType = 'Madgwick'
>>> myimu.calc_orientation(initialOrientation, type=calcType)
>>> q_Kalman = myimu.quat[:,1:]
'''
def __init__(self, inFile = None, inData = None):
'''Initialize an IMU-object'''
if inData is not None:
self.setData(inData, inRate)
else:
if inFile is None:
inFile = self._selectInput()
if os.path.exists(inFile):
self.source = inFile
try:
data = getXSensData(self.source, ['Acc', 'Gyr', 'Mag'])
self.rate = data[0]
self.acc= data[1]
self.omega = data[2]
self.mag = data[3]
self._setInfo()
print('data read in!')
except IOError:
print('Could not read ' + inFile)
else:
print(inFile + ' does NOT exist!')
[docs] def setData(self, data):
''' Set the properties of an IMU-object. '''
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._setInfo()
[docs] def calc_orientation(self, R_initialOrientation, type='exact'):
'''
Calculate the current orientation
Parameters
----------
R_initialOrientation : 3x3 array
approximate alignment of sensor-CS with space-fixed CS
type : string
- 'acc_gyr' ...... quaternion integration of angular velocity
- 'Kalman' ..... quaternion Kalman filter
- 'Madgwick' .. gradient descent method, efficient
- 'Mahony' ... Formula from Mahony, as implemented by Madgwick
'''
if type == 'accGyr':
quaternion = acc_gyr(self.omega, self.acc, R_initialOrientation, self.rate)[0]
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 ui.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 ui.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
[docs] def calc_position(self, initialPosition):
'''Calculate the position, assuming that the orientation is already known.'''
# 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.quatinv(self.quat))
accReSpace = vector.rotate_vector(accReSensor, self.quat)
# Position and Velocity through integration, assuming 0-velocity at t=0
vel = nan*ones_like(accReSpace)
pos = nan*ones_like(accReSpace)
for ii in range(accReSpace.shape[1]):
vel[:,ii] = cumtrapz(accReSpace[:,ii], dx=1./self.rate, initial=0)
pos[:,ii] = cumtrapz(vel[:,ii], dx=1./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 _selectInput(self):
'''GUI for the selection of an in-file. '''
(inFile, inPath) = ui.getfile('*.txt', 'Select input data: ')
fullInFile = os.path.join(inPath, inFile)
print('Selection: ' + fullInFile)
return fullInFile
def _setInfo(self):
'''Set the information properties of that IMU'''
self.totalSamples = len(self.omega)
self.duration = float(self.totalSamples)/self.rate # [sec]
self.dataType = str(self.omega.dtype)
[docs]def getXSensData(inFile, paramList):
'''
Read in Rate and stored 3D parameters from XSens IMUs
Parameters:
-----------
inFile : string
Path and name of input file
paramList: list of strings
Starting names of stored parameters
Returns:
--------
List, containing
rate : float
Sampling rate
[List of x/y/z Parameter Values]
Examples:
---------
>>> data = getXSensData(fullInFile, ['Acc', 'Gyr'])
>>> rate = data[0]
>>> accValues = data[1]
>>> Omega = data[2]
'''
# Get the sampling rate from the second line in the file
try:
fh = open(inFile)
fh.readline()
line = fh.readline()
rate = np.float(line.split(':')[1].split('H')[0])
fh.close()
returnValues = [rate]
except FileNotFoundError:
print('{0} does not exist!'.format(inFile))
return -1
# Read the data
data = pd.read_csv(inFile,
sep='\t',
skiprows=4,
index_col=False)
# Extract the columns that you want, by name
for param in paramList:
Expression = param + '*'
returnValues.append(data.filter(regex=Expression).values)
return returnValues
[docs]def analyze3Dmarkers(MarkerPos, ReferencePos):
'''
Take recorded positions from 3 markers, and calculate center-of-mass (COM) and orientation
Can be used e.g. for the analysis of Optotrac data.
Parameters
----------
MarkerPos : ndarray, shape (N,9)
x/y/z coordinates of 3 markers
ReferencePos : ndarray, shape (1,9)
x/y/z coordinates of markers in the reference position
Returns
-------
Position : ndarray, shape (N,3)
x/y/z coordinates of COM, relative to the reference position
Orientation : ndarray, shape (N,3)
Orientation relative to reference orientation, expressed as quaternion
Example
-------
>>> (PosOut, OrientOut) = analyze3Dmarkers(MarkerPos, ReferencePos)
'''
# Specify where the x-, y-, and z-position are located in the data
cols = {'x' : r_[(0,3,6)]}
cols['y'] = cols['x'] + 1
cols['z'] = cols['x'] + 2
# Calculate the position
cog = np.vstack(( sum(MarkerPos[:,cols['x']], axis=1),
sum(MarkerPos[:,cols['y']], axis=1),
sum(MarkerPos[:,cols['z']], axis=1) )).T/3.
cog_ref = np.vstack(( sum(ReferencePos[cols['x']]),
sum(ReferencePos[cols['y']]),
sum(ReferencePos[cols['z']]) )).T/3.
position = cog - cog_ref
# Calculate the orientation
numPoints = len(MarkerPos)
orientation = np.ones((numPoints,3))
refOrientation = vector.plane_orientation(ReferencePos[:3], ReferencePos[3:6], ReferencePos[6:])
for ii in range(numPoints):
'''The three points define a triangle. The first rotation is such
that the orientation of the reference-triangle, defined as the
direction perpendicular to the triangle, is rotated along the shortest
path to the current orientation.
In other words, this is a rotation outside the plane spanned by the three
marker points.'''
curOrientation = vector.plane_orientation( MarkerPos[ii,:3], MarkerPos[ii,3:6], MarkerPos[ii,6:])
alpha = vector.angle(refOrientation, curOrientation)
if alpha > 0:
n1 = np.cross(refOrientation, curOrientation)
n1 = n1/np.linalg.norm(n1)
q1 = n1 * sin(alpha/2)
else:
q1 = r_[0,0,0]
# Now rotate the triangle into this orientation ...
refPos_after_q1 = vector.rotate_vector(np.reshape(ReferencePos,(3,3)), q1)
'''Find which further rotation in the plane spanned by the three marker points
is necessary to bring the data into the measured orientation.'''
Marker_0 = MarkerPos[ii,:3]
Marker_1 = MarkerPos[ii,3:6]
Vector10 = Marker_0 - Marker_1
vector10_ref = refPos_after_q1[0]-refPos_after_q1[1]
beta = vector.angle(Vector10, vector10_ref)
q2 = curOrientation * np.sin(beta/2)
if np.cross(vector10_ref,Vector10).dot(curOrientation)<=0:
q2 = -q2
orientation[ii,:] = quat.quatmult(q2, q1)
return (position, orientation)
[docs]def movement_from_markers(r0, Position, Orientation):
'''
Movement trajetory of a point on an object, from the position and
orientation of a set of markers, and the relative position of the
point at t=0.
Parameters
----------
r0 : ndarray (3,)
Position of point relative to center of markers, when the object is
in the reference position.
Position : ndarray, shape (N,3)
x/y/z coordinates of COM, relative to the reference position
Orientation : ndarray, shape (N,3)
Orientation relative to reference orientation, expressed as quaternion
Returns
-------
mov : ndarray, shape (N,3)
x/y/z coordinates of the position on the object, relative to the
reference position of the markers
Notes
-----
.. math::
\\vec C(t) = \\vec M(t) + \\vec r(t) = \\vec M(t) +
{{\\bf{R}}_{mov}}(t) \\cdot \\vec r({t_0})
Examples
--------
>>> t = np.arange(0,10,0.1)
>>> translation = (np.c_[[1,1,0]]*t).T
>>> M = np.empty((3,3))
>>> M[0] = np.r_[0,0,0]
>>> M[1]= np.r_[1,0,0]
>>> M[2] = np.r_[1,1,0]
>>> M -= np.mean(M, axis=0)
>>> q = np.vstack((np.zeros_like(t), np.zeros_like(t),quat.deg2quat(100*t))).T
>>> M0 = vector.rotate_vector(M[0], q) + translation
>>> M1 = vector.rotate_vector(M[1], q) + translation
>>> M2 = vector.rotate_vector(M[2], q) + translation
>>> data = np.hstack((M0,M1,M2))
>>> (pos, ori) = signals.analyze3Dmarkers(data, data[0])
>>> r0 = np.r_[1,2,3]
>>> movement = movement_from_markers(r0, pos, ori)
'''
return Position + vector.rotate_vector(r0, Orientation)
[docs]def vel2quat(vel, q0, rate, CStype):
'''
Take an angular velocity (in deg/s), and convert it into the
corresponding orientation quaternion.
Parameters
----------
vel : array, shape (3,) or (N,3)
angular velocity [deg/s].
q0 : array (3,)
vector-part of quaternion (!!)
rate : float
sampling rate (in [Hz])
CStype: string
coordinate_system, space-fixed ("sf") or body_fixed ("bf")
Returns
-------
quats : array, shape (N,4)
unit quaternion vectors.
Notes
-----
1) The output has the same length as the input. As a consequence, the last velocity vector is ignored.
2) For angular velocity with respect to space ("sf"), the orientation is given by
.. math::
q(t) = \\Delta q(t_n) \\circ \\Delta q(t_{n-1}) \\circ ... \\circ \\Delta q(t_2) \\circ \\Delta q(t_1) \\circ q(t_0)
.. math::
\\Delta \\vec{q_i} = \\vec{n(t)}\\sin (\\frac{\\Delta \\phi (t_i)}{2}) = \\frac{\\vec \\omega (t_i)}{\\left| {\\vec \\omega (t_i)} \\right|}\\sin \\left( \\frac{\\left| {\\vec \\omega ({t_i})} \\right|\\Delta t}{2} \\right)
3) For angular velocity with respect to the body ("bf"), the sequence of quaternions is inverted.
4) Take care that you choose a high enough sampling rate!
Examples
--------
>>> v0 = [0., 0., 100.] * np.pi/180.
>>> vel = tile(v0, (1000,1))
>>> rate = 100
>>> out = quat.vel2quat(vel, [0., 0., 0.], rate, 'sf')
>>> out[-1:]
array([[-0.76040597, 0. , 0. , 0.64944805]])
'''
# convert from deg/s to rad/s
vel = np.atleast_2d(deg2rad(vel))
vel_t = sqrt(sum(vel**2, 1))
vel_nonZero = vel_t>0
# initialize the quaternion
q_delta = zeros(shape(vel))
q_pos = zeros((len(vel),4))
q_pos[0,:] = quat.vect2quat(q0)
# magnitude of position steps
dq_total = sin(vel_t[vel_nonZero]/(2*rate))
q_delta[vel_nonZero,:] = vel[vel_nonZero,:] * tile(dq_total/vel_t[vel_nonZero], (3,1)).T
for ii in range(len(vel)-1):
q1 = quat.vect2quat(q_delta[ii,:])
q2 = q_pos[ii,:]
if CStype == 'sf':
qm = quat.quatmult(q1,q2)
elif CStype == 'bf':
qm = quat.quatmult(q2,q1)
else:
print('I don''t know this type of coordinate system!')
q_pos[ii+1,:] = qm
return q_pos
[docs]def acc_gyr(omega, accMeasured, initialPosition, R_initialOrientation, rate):
''' Reconstruct position and orientation, from angular velocity and linear acceleration.
Assumes a start in a stationary position. No compensation for drift.
Parameters
----------
omega : ndarray(N,3)
Angular velocity, in [rad/s]
accMeasured : ndarray(N,3)
Linear acceleration, in [m/s^2]
initialPosition : ndarray(3,)
initial Position, in [m]
R_initialOrientation: ndarray(3,3)
Rotation matrix describing the initial orientation of the sensor,
except a mis-orienation with respect to gravity
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 = acc_gyr(omega, acc, initialPosition, R_initialOrientation, rate)
'''
# Transform recordings to angVel/acceleration in space --------------
# Orientation of \vec{g} with the sensor in the "R_initialOrientation"
g = 9.81
g0 = inv(R_initialOrientation).dot(r_[0,0,g])
# for the remaining deviation, assume the shortest rotation to there
q0 = vector.qrotate(accMeasured[0],g0)
R0 = quat.quat2rotmat(q0)
# combine the two, to form a reference orientation. Note that the sequence
# is very important!
R_ref = R_initialOrientation.dot(R0)
q_ref = quat.rotmat2quat(R_ref)
# Calculate orientation q by "integrating" omega -----------------
q = vel2quat(rad2deg(omega), q_ref, rate, 'bf')
# Acceleration, velocity, and position ----------------------------
# From q and the measured acceleration, get the \frac{d^2x}{dt^2}
g_v = r_[0, 0, g]
accReSensor = accMeasured - vector.rotate_vector(g_v, quat.quatinv(q))
accReSpace = vector.rotate_vector(accReSensor, q)
# Make the first position the reference position
q = quat.quatmult(q, quat.quatinv(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 = nan*ones_like(accReSpace)
pos = nan*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_quat(rate, acc, gyr, mag):
'''
Calclulate the orientation from IMU magnetometer data.
Parameters
----------
rate : float
sample rate [Hz]
acc : (N,3) ndarray
linear acceleration [m/sec^2]
gyr : (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 = zeros(7) # state vector
z_k = zeros(7) # measurement vector
z_k_pre = zeros(7)
P_k = matrix( np.eye(7) ) # error covariance matrix P_k
Phi_k = matrix( 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 = matrix( np.eye(7) ) # Identity matrix
Q_k = matrix( zeros((7,7)) ) # process noise matrix Q_k
D = 0.0001*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 = matrix( 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 = zeros( (numData,4) )
for ii in range(numData):
accelVec = acc[ii,:]
magVec = mag[ii,:]
angvelVec = gyr[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 = accelVec/norm(accelVec)
magVec_hor = magVec - accelVec_n * accelVec_n.dot(magVec)
magVec_n = magVec_hor/norm(magVec_hor)
basisVectors = np.vstack( (magVec_n, np.cross(accelVec_n, magVec_n), accelVec_n) ).T
quatRef = quat.quatinv(quat.rotmat2quat(basisVectors)).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 * 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,:] = 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,:] = 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,:] = 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,:] = 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.quatmult(0.5*x_k[3:],r_[0, x_k[:3]]).flatten()
x_k[3:] = x_k[3:]/norm(x_k[3:])
x_k[:3] = 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.quatmult(qOut, quat.quatinv(qOut[0]))
return qOut
class MadgwickAHRS:
def __init__(self, SamplePeriod=1./256, Beta=1.0, Quaternion=[1,0,0,0]):
'''Initialization
Parameters
----------
SamplePeriod : sample period
Beta : algorithm gain
Quaternion : output quaternion describing the Earth relative to the sensor
'''
self.SamplePeriod = SamplePeriod
self.Beta = Beta
self.Quaternion = Quaternion
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.'''
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.quatmult(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 MahonyAHRS:
'''Madgwick's implementation of Mayhony's AHRS algorithm
'''
def __init__(self, SamplePeriod=1./256, Kp=1.0, Ki=0, Quaternion=[1,0,0,0]):
'''Initialization
Parameters
----------
SamplePeriod : sample period
Kp : algorithm proportional gain
Ki : algorithm integral gain
Quaternion : output quaternion describing the Earth relative to the sensor
'''
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.'''
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.quatmult(q, np.hstack([0, Gyroscope])).flatten()
# Integrate to yield quaternion
q += qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q)
if __name__ == '__main__':
myimu = IMU(r'C:\Users\p20529\Documents\Teaching\ETH\CSS\Exercises\Ex_Vestibular\Data\Walking_02.txt')
initialOrientation = np.array([[1,0,0],
[0,0,-1],
[0,1,0]])
initialPosition = np.r_[0,0,0]
myimu.calc_orientation(initialOrientation)
q_simple = myimu.quat[:,1:]
#calcType = 'Mahony'
calcType = 'Madgwick'
myimu.calc_orientation(initialOrientation, type=calcType)
q_Kalman = myimu.quat[:,1:]
#myimu.calc_position(initialPosition)
t = arange(myimu.totalSamples)/myimu.rate
plt.plot(t, q_simple, '-', label='simple')
plt.hold(True)
plt.plot(t, q_Kalman, '--', label=calcType)
plt.legend()
#plt.plot(t, myimu.pos)
plt.show()
print('Done')