def run(self, s1_a, s2_a, s1_w, s2_w, s1_m, s2_m, dt):
"""
Run the SSRO algorithm on the data from two body-segment adjacent sensors. Rotation quaternion
is from the second sensor to the first sensor.
Parameters
----------
s1_w : numpy.ndarray
Sensor 1 angular velocity in rad/s.
s2_w : numpy.ndarray
Sensor 2 angular velocity in rad/s.
s1_a : numpy.ndarray
Sensor 1 acceleration in m/s^2.
s2_a : numpy.ndarray
Sensor 2 acceleration in m/s^2.
s1_m : numpy.ndarray
Sensor 1 magnetometer readings.
s2_m : numpy.ndarray
Sensor 2 magnetometer readings.
dt : float, optional
Sample rate in seconds.
Returns
-------
q : numpy.ndarray
Rotation quaternion containing the rotation from sensor 2's frame to that of sensor 1's
"""
# initialize values
self.x = zeros(10) # state vector
g1_init = mean(s1_a[:self.init_window, :], axis=0)
g2_init = mean(s2_a[:self.init_window, :], axis=0)
self.x[:3] = g1_init / norm(g1_init)
self.x[3:6] = g2_init / norm(g2_init)
m1_init = mean(s1_m[:self.init_window, :], axis=0)
m2_init = mean(s2_m[:self.init_window, :], axis=0)
mg1_init = m1_init - sum(m1_init * self.x[0:3]) * self.x[0:3]
mg2_init = m2_init - sum(m2_init * self.x[3:6]) * self.x[3:6]
q_g = utility.vec2quat(self.x[3:6], self.x[:3])
q_m = utility.vec2quat(utility.quat2matrix(q_g) @ mg2_init, mg1_init)
self.x[6:] = utility.quat_mult(q_m, q_g)
self.P = identity(10) * 0.01
self.a1, self.a2 = zeros(3), zeros(3)
self.a1_, self.a2_ = zeros(s1_w.shape), zeros(s1_w.shape)
self.eps1, self.eps2 = zeros((3, 3)), zeros((3, 3))
# storage for the states
self.x_ = zeros((s1_w.shape[0], 10))
# run the update step over all the data
for i in range(s1_w.shape[0]):
self.update(s1_a[i, :], s2_a[i, :], s1_w[i, :], s2_w[i, :],
s1_m[i, :], s2_m[i, :], dt, i)
self.x_[i] = self.x
self.a1_[i] = self.a1
self.a2_[i] = self.a2
return self.x_
def run(self, s1_a, s2_a, s1_w, s2_w, s1_h, s2_h, dt):
"""
Run the filter on the data from the joint. Rotate from sensor 2 to sensor 1
Parameters
----------
s1_a : numpy.ndarray
Nx3 array of accelerations from the first sensor.
s2_a : numpy.ndarray
Nx3 array of accelerations from the second sensor
s1_w : numpy.ndarray
Nx3 array of angular velocities from the first sensor.
s2_w : numpy.ndarray
Nx3 array of angular velocities from the second sensor.
s1_h : numpy.ndarray
Nx3 array of magnetometer readings from the first sensor.
s2_h : numpy.ndarray
Nx3 array of magnetometer readings from the second sensor.
dt : float
Sampling period in seconds.
Returns
-------
q : numpy.ndarray
Nx4 array of quaternions representing the rotation from sensor 2 to sensor 1.
"""
# number of elements
n = s1_a.shape[0]
# get the mean of the first few samples of acceleration which will be used to compute an initial guess for the
# gravity vector esimation.
s1_a0 = mean(s1_a[:self.init_window], axis=0)
s2_a0 = mean(s2_a[:self.init_window], axis=0)
# get the rotation from the initial accelerations to the global gravity vector
s1_q_init = utility.vec2quat(s1_a0, array([0, 0, 1]))
s2_q_init = utility.vec2quat(s2_a0, array([0, 0, 1]))
# setup the AHRS algorithms
s1_ahrs = MadgwickAHRS(dt, q_init=s1_q_init, beta=self.s1_ahrs_beta)
s2_ahrs = MadgwickAHRS(dt, q_init=s2_q_init, beta=self.s2_ahrs_beta)
# run the algorithms
s1_q = zeros((n, 4))
s2_q = zeros((n, 4))
for i in range(n):
s1_q[i] = s1_ahrs.updateIMU(s1_w[i], s1_a[i] / self.g)
s2_q[i] = s2_ahrs.updateIMU(s2_w[i], s2_a[i] / self.g)
# get the gravity axis from the rotation matrices
s1_z = utility.quat2matrix(s1_q)[:, 2, :]
s2_z = utility.quat2matrix(s2_q)[:, 2, :]
# get the magnetometer readings that are not in the direction of gravity
s1_m = s1_h - sum(s1_h * s1_z, axis=1, keepdims=True) * s1_z
s2_m = s2_h - sum(s2_h * s2_z, axis=1, keepdims=True) * s2_z
# get the initial guess for the orientation. Used to initialize the UKF
# Initial guess from aligning gravity axes
q_z_init = utility.vec2quat(s2_z[0], s1_z[0])
# Initial guess from aligning non-gravity vector magnetometer readings
q_m_init = utility.vec2quat(
utility.quat2matrix(q_z_init) @ s2_m[0], s1_m[0])
q_init = utility.quat_mult(q_m_init, q_z_init)
# Initial Kalman Filter parameters
P_init = identity(4) * 0.1 # assume our initial guess is fairly good
R = identity(4) # Measurement will have some error
ukf = UnscentedKalmanFilter(q_init, P_init, OldSROFilter._F,
OldSROFilter._H, self.Q, R)
self.q_ = zeros((n, 4)) # storage for ukf output
self.q_[0] = q_init # store the initial guess
self.err = zeros(n)
for i in range(1, n):
# get the measurement guess for the orientation
q_z = utility.vec2quat(s2_z[i], s1_z[i])
q_m = utility.vec2quat(utility.quat2matrix(q_z) @ s2_m[i], s1_m[i])
q = utility.quat_mult(q_m, q_z).reshape((4, 1))
# get an estimate of the measurement error, based on how dynamic the motion is
err = abs(norm(s1_a[i]) - self.g) + abs(norm(s2_a[i]) - self.g)
if self.error_mode == 'linear':
err *= self.error_factor
elif self.error_mode == 'exp' or self.error_mode == 'exponential':
err = err**self.error_factor
else:
raise ValueError(
'error_mode must either be linear or exp (exponential)')
self.err[i] = err
# update the measurement noise matrix based upon this error estimate
ukf.R = identity(4) * err
self.q_[i] = ukf.run(q,
f_kwargs=dict(s1_w=s1_w[i],
s2_w=s2_w[i],
dt=dt)).flatten()
self.q_[i] /= norm(
self.q_[i]
) # output isn't necessarily normalized as rotation quaternions need to be
if self.smooth:
q = self.q_.copy()
pad = 16
for i in range(pad, self.q_.shape[0] - pad):
self.q_[i] = utility.quat_mean(q[i - pad:i + pad, :])
return self.q_
def update(self, y_a1, y_a2, y_w1, y_w2, y_m1, y_m2, dt, j):
"""
Update the state vector and covariance for the next time point.
"""
# short hand names for parts of the state vector
g1 = self.x[:3]
g2 = self.x[3:6]
q_21 = self.x[6:]
# compute the difference in angular velocities in the second sensor's frame
R_21 = utility.quat2matrix(q_21) # rotation from sensor 2 to sensor 1
y_w1_2 = R_21.T @ y_w1 # sensor 1 angular velocity in sensor 2 frame
diff_y_w = y_w2 - y_w1_2
# create the state transition matrix
A = zeros((10, 10))
A[0:3, 0:3] = identity(3) - dt * SSRO._skew(y_w1)
A[3:6, 3:6] = identity(3) - dt * SSRO._skew(y_w2)
A[6:, 6:] = identity(4) - 0.5 * dt * SSRO._skew_quat(diff_y_w)
# state transition covariance
Q = zeros((10, 10))
Q[0:3, 0:3] = -dt**2 * SSRO._skew(g1) @ (self.sigma_g**2 *
identity(3)) @ SSRO._skew(g1)
Q[3:6, 3:6] = -dt**2 * SSRO._skew(g2) @ (self.sigma_g**2 *
identity(3)) @ SSRO._skew(g2)
Q[6:, 6:] = -dt**2 * SSRO._skew_quat(q_21) @ (
self.sigma_g**2 * identity(4)) @ SSRO._skew_quat(q_21)
# predicted state and covariance
xhat = A @ self.x
Phat = A @ self.P @ A.T + Q
# create the measurements
zt = zeros(10)
zt[0:3] = y_a1 - self.c * self.a1
zt[3:6] = y_a2 - self.c * self.a2
# quaternion measurement
q_g = utility.vec2quat(g2, g1)
mg1 = y_m1 - sum(y_m1 * g1) * g1
mg2 = y_m2 - sum(y_m2 * g2) * g2
q_m = utility.vec2quat(utility.quat2matrix(q_g) @ mg2, mg1)
zt[6:] = utility.quat_mult(q_m, q_g)
# measurement estimation matrix
H = zeros((10, 10))
H[:6, :6] = identity(6) * self.grav
H[6:, 6:] = identity(4)
# update estimates of the acceleration variance
self.eps1 += self.c**2 / self.N * outer(self.a1, self.a1)
self.eps1 -= self.c**2 / self.N * outer(self.a1_[j - self.N],
self.a1_[j - self.N])
self.eps2 += self.c**2 / self.N * outer(self.a2, self.a2)
self.eps2 -= self.c**2 / self.N * outer(self.a2_[j - self.N],
self.a2_[j - self.N])
# measurement covariance - essentially how much we trust the measurement vector
M = zeros((10, 10))
M[0:3, 0:3] = self.sigma_a**2 * identity(3) + self.eps1
M[3:6, 3:6] = self.sigma_a**2 * identity(3) + self.eps2
M[6:,
6:] = identity(4) * self.err_factor * (norm(self.a1) + norm(self.a2))
# kalman gain and state estimation
K = Phat @ H.T @ np_inv(H @ Phat @ H.T + M)
xbar = xhat + K @ (zt - H @ xhat) # not yet normalized
self.P = (identity(10) - K @ H) @ Phat
# normalize the state vector
self.x[0:3] = xbar[0:3] / norm(xbar[0:3])
self.x[3:6] = xbar[3:6] / norm(xbar[3:6])
self.x[6:] = xbar[6:] / norm(xbar[6:])
# acceleration values
self.a1 = y_a1 - self.x[0:3] * self.grav
self.a2 = y_a2 - self.x[3:6] * self.grav