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