def compute_quat(all_data,
len_sensor_list,
quat_cal_offset,
rot_inds,
num_sensors=5,
t_offset=0,
signals_per_sensor=6,
beta=0.2 * np.ones(5),
rate=60.0,
verbose=False,
beta_init=0.4):
d2g = ahrs.common.DEG2RAD # Constant to convert degrees to radians
Qi = np.zeros((num_sensors, quat_cal_offset, 4))
Q = np.zeros((num_sensors, all_data.shape[0] - quat_cal_offset, 4))
Qi[:, 0, :] = np.array([0.7071, 0.7071, 0.0, 0.0])
z_neg_90 = np.array([[0, 1.0, 0], [-1., 0, 0], [0, 0, 1.0]])
y_180 = np.array([[-1.0, 0, 0], [0, 1.0, 0], [0, 0, -1.0]])
z_180 = np.array([[-1.0, 0, 0], [0, -1.0, 0], [0, 0, 1.0]])
y_neg_90 = np.array([[0, 0, -1.0], [0, 1.0, 0], [1.0, 0, 0]])
y_pos_90 = np.array([[0, 0, 1.0], [0, 1.0, 0], [-1.0, 0, 0]])
ankle_offset = -100. * d2g
x_pos_ankle = np.array([[1.0, 0, 0],
[0, np.cos(ankle_offset), -np.sin(ankle_offset)],
[0, np.sin(ankle_offset),
np.cos(ankle_offset)]])
hip_rot = np.matmul(y_neg_90, z_180)
foot_rot = np.matmul(x_pos_ankle, hip_rot)
r_leg_rot = z_neg_90
l_leg_rot = np.matmul(z_neg_90, y_180)
rot_mats = np.zeros((len_sensor_list, 3, 3))
for i in range(len_sensor_list): # define rotation type
if rot_inds[i] == 0: # hip, torso, head
rot_mats[i, :, :] = hip_rot
elif rot_inds[i] == 1: # left side
rot_mats[i, :, :] = l_leg_rot
elif rot_inds[i] == 2: # right side
rot_mats[i, :, :] = r_leg_rot
elif rot_inds[i] == 3: # foot
rot_mats[i, :, :] = foot_rot
for i in range(
len_sensor_list): # processing quaternions one sensor at a time
s_off = i * signals_per_sensor
accel = np.matmul(all_data[:, s_off + t_offset:s_off + t_offset + 3],
rot_mats[i, :, :])
gyro = np.matmul(
all_data[:, s_off + t_offset + 3:s_off + t_offset + 6],
rot_mats[i, :, :])
#mag = np.matmul(all_data[:,s_off+t_offset+6:s_off+t_offset+9],rot_mats[i,:,:])
# calibrating the initial quaternion with a large beta_init value
madgwick_i = ahrs.filters.Mahony(frequency=float(rate))
for t in range(1, quat_cal_offset):
Qi[i, t, :] = madgwick_i.updateIMU(Qi[i, t - 1, :], gyro[0, :],
accel[0, :])
quat_ang = np.zeros(num_sensors)
for i in range(len_sensor_list):
quat_ang[i], _ = orientMat(
q2R(np.array(Qi[i, -1, :]))
) # multiply rot_mats by the gyro correction angle to correct to same heading as pelvis? Then
mean_ang = np.mean(quat_ang)
return Qi[:, -1, :], mean_ang, rot_mats
def h_mag(self, q):
C = q2R(q).T
return C @ self.m_ref
def do_gyro_integration(mag_data):
global csv, timeArr, orientation, dt
import ahrs.filters as filters
import ahrs.common.orientation as orient
if "wx_raw" in csv:
gyro_data = np.array(
[csv["wx_raw"] / 32.8, csv["wy_raw"] / 32.8,
csv["wz_raw"] / 32.8]).T
gyro_data = gyro_data * M_PI / 180.0 # deg/s to rad/s
elif "imu1_gyro_x_real" in csv:
gyro_data = np.array([
-csv["imu1_gyro_x_real"], -csv["imu1_gyro_z_real"],
csv["imu1_gyro_y_real"]
]).T / 4 # /4 for sensitivity bug fix
else:
gyro_data = np.array([csv["wx"], csv["wy"], csv["wz"]]).T
gyro_data = gyro_data * M_PI / 180.0 # deg/s to rad/s
# Crappy gyro cal -- subtract out the average of the first 10
gyro_offset = [
np.mean(gyro_data[:30, 0]),
np.mean(gyro_data[:30]),
np.mean(gyro_data[:30, 2])
]
gyro_data = gyro_data - gyro_offset
if "ax" in csv:
acc_data = np.array(
[-csv["ax"] * 9.81, -csv["az"] * 9.81, csv["ay"] * 9.81]).T
else:
acc_data = np.array([
-csv["imu1_accel_x_real"], csv["imu1_accel_z_real"],
-csv["imu1_accel_y_real"]
]).T
# gyro_data = gyro_data[900:,:]
if mag_data is not None:
initial_tilt = orient.ecompass(acc_data[0],
mag_data[0],
frame='ENU',
representation='quaternion')
else:
initial_tilt = orient.acc2q(acc_data[1, :])
plt.figure()
plt.plot(timeArr, gyro_data[:, 0], label="X")
plt.plot(timeArr, gyro_data[:, 1], label="Y")
plt.plot(timeArr, gyro_data[:, 2], label="Z")
plt.plot(timeArr, acc_data[:, 0], label="aX")
plt.plot(timeArr, acc_data[:, 1], label="aY")
plt.plot(timeArr, acc_data[:, 2], label="aZ")
plt.legend()
rotM = orient.q2R(initial_tilt)
newX = np.array([1, 0, 0]).T @ rotM
angleBetween = newX @ np.array([1, 0, 0]).T
print(f"Initial tilt: {initial_tilt} angle {acos(angleBetween) * RAD2DEG}")
# orientation = filters.Mahony(gyr=gyro_data, acc=acc_data, frequency=1.0/np.mean(np.diff(timeArr)))
# orientation = filters.Tilt(acc=acc_data)
# orientation = filters.AngularRate(gyr=gyro_data[1000:,:], q0=initial_tilt, frequency=1/dt)
orientation = filters.AngularRate(gyr=gyro_data,
q0=initial_tilt,
frequency=1 / dt)
import RocketEKF
from ahrs.utils.wmm import WMM
# MDRA
wmm = WMM(latitude=39.0785319, longitude=-75.87425944, height=0)
# X is north, Y is East, Z is down
# So by default, it's NED
# We want East-North-Up
m_ref = np.array([wmm.Y, wmm.X, -wmm.Z]) / 1000
print(f"Mag ref: {m_ref} microTesla")
# o2 = RocketEKF.EKF(gyr=gyro_data, mag=mag_data, frequency=1/np.mean(np.diff(timeArr)), q0=initial_tilt, m_ref=m_ref, frame='ENU')
# orientation = o2
# np.savetxt('orient.mat', json.dumps(orientation.Q.tolist())
bigarr = []
i = 0
for row in orientation.Q:
# bigarr.append([row[0], row[1], row[2], row[3], csv["high_g_accel_x_real"][i], csv["high_g_accel_y_real"][i], csv["high_g_accel_z_real"][i], timeArr[i]])
bigarr.append([row[0], row[1], row[2], row[3], timeArr[i]])
i = i + 1
bigarr = np.array(bigarr)
print(bigarr.shape)
json_dump = json.dumps(bigarr, cls=NumpyEncoder)
# print(json_dump)
with open("orient.json", "w") as txt:
txt.write(json_dump)
np.savetxt("orient.csv", bigarr)
def h_both(self, q: np.ndarray) -> np.ndarray:
C = q2R(q).T
if len(self.z) < 4:
return C @ self.a_ref
return np.r_[C @ self.a_ref, C @ self.m_ref]