def get_series_quat2euler(q0, q1, q2, q3, msg_name=""):
"""Given pandas series q0-q4, compute series roll, pitch, yaw.
Arguments:
q0-q4 -- quaternion entries
Keyword arguments:
msg_name -- name of the message for which the euler angles should be computed (default "")
"""
yaw, pitch, roll = np.array([
tf.quat2euler([q0i, q1i, q2i, q3i])
for q0i, q1i, q2i, q3i in zip(q0, q1, q2, q3)
]).T
yaw = pd.Series(name=combine_names(msg_name, "yaw"),
data=yaw,
index=q0.index)
pitch = pd.Series(name=combine_names(msg_name, "pitch"),
data=pitch,
index=q0.index)
roll = pd.Series(name=combine_names(msg_name, "roll"),
data=roll,
index=q0.index)
return roll, pitch, yaw
def main():
''' modify this part accordingly '''
# flags
UKF = True # use UKF or just gyro data for estimate
Panorama = False # generate panorama or not
Estimate = False # the panorama is based on estimate or ground truth
# dataset idx
idx = 8
# mat file location+prefix
imu_prefix = "imu/imuRaw"
vicon_prefix = "vicon/viconRot"
cam_prefix = "cam/cam"
# load data
imu_ts, imu_vals, vicon_ts, vicon_euler = load_data(
idx, imu_prefix, vicon_prefix)
# init
qk = np.array([1, 0, 0, 0]) # last mean in quaternion
time = imu_ts.shape[0]
gyro_euler = np.zeros((time, 3)) # represent orientation in euler angles
acc_estimate = np.zeros((time, 3)) # represent orientation in euler angles
acc_truth = imu_vals[:, :3]
g_q = np.array([0, 0, 0, 1])
for t in range(time):
# extract sensor data
gyro = imu_vals[t, 3:]
# Prediction
if t == time - 1: # last iter
dt = np.mean(imu_ts[-10:] - imu_ts[-11:-1])
else:
dt = imu_ts[t + 1] - imu_ts[t]
qk = quat_multiply(qk, vec2quat(gyro * dt))
# save for visualization
gyro_euler[t, :] = taitbryan.quat2euler(qk)
acc_estimate[t, :] = quat_multiply(
quat_multiply(quat_inverse(qk), g_q), qk)[1:]
# orientation plot gyro
orientation_plot(idx, imu_ts, gyro_euler, vicon_ts, vicon_euler)
# measurement plot acc
measurement_plot(idx, imu_ts, acc_estimate, acc_truth)
# panoramic by image stitching
if Panorama:
if Estimate:
panorama(idx, cam_prefix, imu_ts, gyro_euler)
else:
panorama(idx, cam_prefix, vicon_ts, vicon_euler)
return 0
def series_quat2euler(q0, q1, q2, q3, msg_name):
"""
Given pandas series q0-q4, compute series roll, pitch, yaw
"""
yaw, pitch, roll = np.array([
tf.quat2euler([q0i, q1i, q2i, q3i]) for
q0i, q1i, q2i, q3i in zip(q0, q1, q2, q3)]).T
yaw = pd.Series(name=msg_name + '__f_yaw', data=yaw, index=q0.index)
pitch = pd.Series(name=msg_name + '__f_pitch', data=pitch, index=q0.index)
roll = pd.Series(name=msg_name + '__f_roll', data=roll, index=q0.index)
return roll, pitch, yaw
def series_quat2euler(q0, q1, q2, q3, msg_name):
"""
Given pandas series q0-q4, compute series roll, pitch, yaw
"""
yaw, pitch, roll = np.array([
tf.quat2euler([q0i, q1i, q2i, q3i]) for
q0i, q1i, q2i, q3i in zip(q0, q1, q2, q3)]).T
yaw = pd.Series(name=msg_name + '__f_yaw', data=yaw, index=q0.index)
pitch = pd.Series(name=msg_name + '__f_pitch', data=pitch, index=q0.index)
roll = pd.Series(name=msg_name + '__f_roll', data=roll, index=q0.index)
return roll, pitch, yaw
def test_quats():
for x, y, z in euler_tuples:
M1 = ttb.euler2mat(z, y, x)
quatM = tq.mat2quat(M1)
quat = ttb.euler2quat(z, y, x)
assert tq.nearly_equivalent(quatM, quat)
quatS = sympy_euler2quat(z, y, x)
assert tq.nearly_equivalent(quat, quatS)
zp, yp, xp = ttb.quat2euler(quat)
# The parameters may not be the same as input, but they give the
# same rotation matrix
M2 = ttb.euler2mat(zp, yp, xp)
assert_array_almost_equal(M1, M2)
def main():
''' modify this part accordingly '''
# flags
UKF = True # use UKF or just gyro data for estimate
Panorama = True # generate panorama or not
Estimate = True # the panorama is based on estimate or ground truth, must be set as True if no vicon data provided
# dataset idx
idx = 11
# mat file location+prefix
imu_prefix = "imu/imuRaw"
vicon_prefix = "vicon/viconRot"
cam_prefix = "cam/cam"
# load data
imu_ts, imu_vals, vicon_ts, vicon_euler = load_data(
idx, imu_prefix, vicon_prefix)
# Unscented Kalman Filter
# init
qk = np.array([1, 0, 0, 0]) # last mean in quaternion
Pk = np.identity(3) * 0.1 # last cov in vector
Q = np.identity(3) * 2 # process noise cov
R = np.identity(3) * 2 # measurement noise cov
time = imu_ts.shape[0]
ukf_euler = np.zeros((time, 3)) # represent orientation in euler angles
for t in range(time):
# extract sensor data
acc = imu_vals[t, :3]
gyro = imu_vals[t, 3:]
# Prediction
X = compute_sigma_pts(qk, Pk, Q)
if t == time - 1: # last iter
dt = np.mean(imu_ts[-10:] - imu_ts[-11:-1])
else:
dt = imu_ts[t + 1] - imu_ts[t]
Y = process_model(X, gyro, dt)
q_pred, P_pred, W = prediction(Y, qk)
if UKF:
# Measurement
vk, Pvv, Pxz = measurement_model(Y, acc, W, R)
# Update
K = np.dot(Pxz, np.linalg.inv(Pvv)) # Kalman gain
qk, Pk = update(q_pred, P_pred, vk, Pvv, K)
else:
# estimate just based on control input gyro
qk, Pk = q_pred, P_pred
# save for visualization
ukf_euler[t, :] = taitbryan.quat2euler(qk)
np.save('result/' + 'ukf' + str(idx), ukf_euler)
# orientation plot UKF + only gyro
orientation_plot(idx, imu_ts, ukf_euler, vicon_ts, vicon_euler)
# panoramic by image stitching
if Panorama:
if Estimate:
panorama(idx, cam_prefix, imu_ts, ukf_euler)
else:
panorama(idx, cam_prefix, vicon_ts, vicon_euler)
return 0
