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()
def find_CT_orientation():
'''Find the angles to bring an "Axiom Artis dTC" into a desired
orientation.'''
# Calculate R_total
R_total = R_s(2, 'alpha')*R_s(0, 'beta')*R_s(1, 'gamma')
# Use pprint, which gives a nicer display for the matrix
import pprint
pprint.pprint(R_total)
# Find the desired orientation of the CT-scanner
bullet_1 = np.array([5,2,2])
bullet_2 = np.array([5,-2,-4])
n = np.cross(bullet_1, bullet_2)
ct = np.nan * np.ones((3,3))
ct[:,1] = normalize(bullet_1)
ct[:,2] = normalize(n)
ct[:,0] = np.cross(ct[:,1], ct[:,2])
print('Desired Orientation (ct):')
print(ct)
# Calculate the angle of each CT-element
beta = np.arcsin( ct[2,1] )
gamma = np.arcsin( -ct[2,0]/np.cos(beta) )
# next I assume that -pi/2 < gamma < pi/2:
if np.sign(ct[2,2]) < 0:
gamma = np.pi - gamma
# display output between +/- pi:
if gamma > np.pi:
gamma -= 2*np.pi
alpha = np.arcsin( -ct[0,1]/np.cos(beta) )
alpha_deg = np.rad2deg( alpha )
beta_deg = np.rad2deg( beta )
gamma_deg = np.rad2deg( gamma )
# Check if the calculated orientation equals the desired orientation
print('Calculated Orientation (R):')
rot_mat = R(2, alpha_deg) @ R(0, beta_deg) @ R(1, gamma_deg)
print(rot_mat)
return (alpha_deg, beta_deg, gamma_deg)
def setUp(self):
self.qz = r_[cos(0.1), 0,0,sin(0.1)]
self.qy = r_[cos(0.1),0,sin(0.1), 0]
self.quatMat = vstack((self.qz,self.qy))
self.q3x = r_[sin(0.1), 0, 0]
self.q3y = r_[2, 0, sin(0.1), 0]
self.delta = 1e-4
# Simulate IMU-data
duration_movement = 1 # [sec]
duration_total = 1 # [sec]
rate = 100 # [Hz]
B0 = vector.normalize([1, 0, 1])
rotation_axis = [0, 1, 0]
angle = 90
translation = [1,0,0]
distance = 0
self.q_init = [0,0,0]
self.pos_init = [0,0,0]
self.imu_signals, self.body_pos_orient = simulate_imu(rate, duration_movement, duration_total,
q_init = self.q_init, rotation_axis=rotation_axis, deg=angle,
pos_init = self.pos_init, direction=translation, distance=distance,
B0=B0)
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)
def get_init_on_dir_rh_scc(reid_initial_pitch_angle_deg):
"""
get the initial orientation of the right horizontal semi-circular canal's on direction vector
:param reid_initial_pitch_angle_deg: the angle about which Reid's line is pitched upwards in degrees, with respect
to fixed space horizontal plane
:return:
"""
# set the vector about which rotations stimulate the right horizontal semi-circular canal respective to the
# orientation of Reid's line given by the exercise and normalize it
on_dir = vector.normalize(np.array([0.32269, -0.03837, -0.94573]))
# convert the initial pitch angle to radian
reid_initial_pitch_angle_rad = reid_initial_pitch_angle_deg / 180 * np.pi
# set the rotation vector for the pitch of Reid's line with the defined angle
reid_initial_pitch = np.array(
[0, 0, np.sin(reid_initial_pitch_angle_rad / 2)])
# adjust the on direction vector of the semi-circular canal according to the pitch of Reid's line so that it now
# represents the orientation in the global coordinate system
on_dir_adjusted = vector.rotate_vector(on_dir, reid_initial_pitch)
return on_dir_adjusted
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)
def setUp(self):
# Those are currently not needed
#self.qz = r_[cos(0.1), 0, 0, sin(0.1)]
#self.qy = r_[cos(0.1), 0, sin(0.1), 0]
#self.quatMat = vstack((self.qz,self.qy))
#self.q3x = r_[sin(0.1), 0, 0]
#self.q3y = r_[ 0, sin(0.1), 0]
#self.delta = 1e-4
# Simulate IMU-data
duration_movement = 1 # [sec]
duration_total = 1 # [sec]
rate = 100 # [Hz]
B0 = vector.normalize([1, 0, 1]) # geomagnetic field, re earth
rotation_axis = [0, 1, 0]
angle = 90
translation = [1, 0, 0]
distance = 0
self.q_init = [0, 0, 0]
self.pos_init = [0, 0, 0]
self.imu_signals, self.body_pos_orient = simulate_imu(
rate,
duration_movement,
duration_total,
q_init=self.q_init,
rotation_axis=rotation_axis,
deg=angle,
pos_init=self.pos_init,
direction=translation,
distance=distance,
B0=B0)
def kalman_quat(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 * 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.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 * 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, :] = 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:] = 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.quatmult(qOut, quat.quatinv(qOut[0]))
return qOut
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_normalize(self):
result = vector.normalize([3, 0, 0])
correct = array([[1., 0., 0.]])
error = norm(result - correct)
self.assertAlmostEqual(error, 0)
# Input data
v0 = [0, 0, 0]
v1 = [1, 0, 0]
v2 = [1, 1, 0]
# Length of a vector
length_v2 = np.linalg.norm(v2)
print('The length of v2 is {0:5.3f}\n'.format(length_v2))
# Angle between two vectors
angle = vector.angle(v1, v2)
print('The angle between v1 and v2 is {0:4.1f} degree\n'.format(np.rad2deg(angle)))
# Vector normalization
v2_normalized = vector.normalize(v2)
print('v2 normalized is {0}\n'.format(v2_normalized))
# Projection
projected = vector.project(v1, v2)
print('The projection of v1 onto v2 is {0}\n'.format(projected))
# Plane orientation
n = vector.plane_orientation(v0, v1, v2)
print('The plane spanned by v0, v1, and v2 is orthogonal to {0}\n'.format(n))
# Gram-Schmidt orthogonalization
gs = vector.GramSchmidt(v0, v1, v2)
print('The Gram-Schmidt orthogonalization of the points v0, v1, and v2 is {0}\n'.format(np.reshape(gs, (3,3))))
# Shortest rotation
def kalman(rate,
acc,
omega,
mag,
D=[0.4, 0.4, 0.4],
tau=[0.5, 0.5, 0.5],
Q_k=None,
R_k=None,
initialPosition=np.zeros(3),
initialVelocity=np.zeros(3),
initialOrientation=np.zeros(3),
timeVector=np.zeros((5, 1)),
accMeasured=np.column_stack((np.zeros((5, 2)), 9.81 * np.ones(5))),
referenceOrientation=np.array([1., 0., 0., 0.])):
'''
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
D : (,3) ndarray
noise variance, for x/y/z [rad^2/sec^2]
parameter for tuning the filter; defaults from Yun et al.
can also be entered as list
tau : (,3) ndarray
time constant for the process model, for x/y/z [sec]
parameter for tuning the filter; defaults from Yun et al.
can also be entered as list
Q_k : None, or (7,7) ndarray
covariance matrix of process noises
parameter for tuning the filter
If set to "None", the defaults from Yun et al. are taken!
R_k : None, or (7,7) ndarray
covariance matrix of measurement noises
parameter for tuning the filter; defaults from Yun et al.
If set to "None", the defaults from Yun et al. are taken!
Returns
-------
qOut : (N,4) ndarray
unit quaternion, describing the orientation relativ to the coordinate
system spanned by the local magnetic field, and gravity
pos : (N,3) ndarray
Position array
vel : (N,3) ndarray
Velocity
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
# check input
assert len(tau) == 3
tau = np.array(tau)
# Initializations
x_k = np.zeros(7) # state vector
z_k = np.zeros(7) # measurement vector
z_k_pre = np.zeros(7)
P_k = np.eye(7) # error covariance matrix P_k
Phi_k = np.eye(7) # discrete state transition matrix Phi_k
for ii in range(3):
Phi_k[ii, ii] = np.exp(-tstep / tau[ii])
H_k = np.eye(7) # Identity matrix
D = np.r_[0.4, 0.4, 0.4] # [rad^2/sec^2]; from Yun, 2006
if Q_k is None:
# Set the default input, from Yun et al.
Q_k = np.zeros((7, 7)) # process noise matrix Q_k
for ii in range(3):
Q_k[ii,
ii] = D[ii] / (2 * tau[ii]) * (1 -
np.exp(-2 * tstep / tau[ii]))
else:
# Check the shape of the input
assert Q_k.shape == (7, 7)
# Evaluate measurement noise covariance matrix R_k
if R_k is None:
# Set the default input, from Yun et al.
r_angvel = 0.01
# [rad**2/sec**2]; from Yun, 2006
r_quats = 0.0001
# from Yun, 2006
r_ii = np.zeros(7)
for ii in range(3):
r_ii[ii] = r_angvel
for ii in range(4):
r_ii[ii + 3] = r_quats
R_k = np.diag(r_ii)
else:
# Check the shape of the input
assert R_k.shape == (7, 7)
# 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 @ magVec)
magVec_n = vector.normalize(magVec_hor)
basisVectors = np.column_stack(
[magVec_n, np.cross(accelVec_n, magVec_n), accelVec_n])
quatRef = quat.q_inv(rotmat.convert(basisVectors, to='quat')).ravel()
# Calculate Kalman Gain
# K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
K_k = P_k @ np.linalg.inv(P_k + R_k)
# Update measurement vector z_k
z_k[:3] = angvelVec
z_k[3:] = quatRef
# Update state vector x_k
x_k += np.array(K_k @ (z_k - z_k_pre)).ravel()
# Evaluate discrete state transition matrix Phi_k
Delta = np.zeros((7, 7))
Delta[3, :] = np.r_[-x_k[4], -x_k[5], -x_k[6], 0, -x_k[0], -x_k[1],
-x_k[2]]
Delta[4, :] = np.r_[x_k[3], -x_k[6], x_k[5], x_k[0], 0, x_k[2],
-x_k[1]]
Delta[5, :] = np.r_[x_k[6], x_k[3], -x_k[4], x_k[1], -x_k[2], 0,
x_k[0]]
Delta[6, :] = np.r_[-x_k[5], x_k[4], x_k[3], x_k[2], x_k[1], -x_k[0],
0]
Delta *= tstep / 2
Phi_k += Delta
# Update error covariance matrix
P_k = (np.eye(7) - K_k) @ P_k
# Projection of state
# 1) quaternions
x_k[3:] += tstep * 0.5 * quat.q_mult(x_k[3:], np.r_[0,
x_k[:3]]).ravel()
x_k[3:] = vector.normalize(x_k[3:])
# 2) angular velocities
x_k[:3] -= tstep * tau * x_k[:3]
qOut[ii, :] = x_k[3:]
# Projection of error covariance matrix
P_k = Phi_k @ P_k @ Phi_k.T + Q_k
# Calculate Position from Orientation
# pos, vel = calc_position(qOut, acc, initialVelocity, initialPosition, timeVector)
# Make the first position the reference position
qOut = quat.q_mult(qOut, quat.q_inv(referenceOrientation))
return qOut
def kalman_quat(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*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.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 * 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,:] = 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:] = 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.quatmult(qOut, quat.quatinv(qOut[0]))
return qOut
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 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 = vector.normalize( np.cross(refOrientation, curOrientation) )
q1 = n1 * np.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)
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 = vector.normalize(np.cross(refOrientation, curOrientation))
q1 = n1 * np.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)
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
