<jupyter_start><jupyter_text>CONTROL DATA<jupyter_code>cs_t = []
cs_action_throttle = []
cs_action_steer = []
cs_error_lateral = []
cs_error_yaw = []
cs_actual_x = []
cs_actual_y = []
cs_actual_yaw = []
cs_actual_speed = []
cs_ref_x = []
cs_ref_y = []
cs_ref_yaw = []
cs_ref_curvature = []
cs_wp_idx = []
for topic, msg, _ in bag.read_messages(topics=['/control_signal']):
    cs_t.append(msg.header.stamp.to_sec())
    cs_action_throttle.append(msg.action_throttle)
    cs_action_steer.append(msg.action_steer)
    cs_error_lateral.append(msg.error_lateral)
    cs_error_yaw.append(msg.error_yaw)
    cs_actual_x.append(msg.actual_x)
    cs_actual_y.append(msg.actual_y)
    cs_actual_yaw.append(msg.actual_yaw)
    cs_actual_speed.append(msg.actual_speed)
    cs_ref_x.append(msg.ref_x)
    cs_ref_y.append(msg.ref_y)
    cs_ref_yaw.append(msg.ref_yaw)
    cs_ref_curvature.append(msg.ref_curvature)
    cs_wp_idx.append(msg.wp_idx)
cs_t = np.array(cs_t)
cs_action_throttle = np.array(cs_action_throttle)
cs_action_steer = np.array(cs_action_steer)
cs_error_lateral = np.array(cs_error_lateral)
cs_error_yaw = np.array(cs_error_yaw)
cs_actual_x = np.array(cs_actual_x)
cs_actual_y = np.array(cs_actual_y)
cs_actual_yaw = np.array(cs_actual_yaw)
cs_actual_speed = np.array(cs_actual_speed)
cs_ref_x = np.array(cs_ref_x)
cs_ref_y = np.array(cs_ref_y)
cs_ref_yaw = np.array(cs_ref_yaw)
cs_ref_curvature = np.array(cs_ref_curvature)
cs_wp_idx = np.array(cs_wp_idx)
cs_actual_x.shape
xf = savgol_filter(cs_actual_x, 201, 3)
yf = savgol_filter(cs_actual_y, 201, 3)
plt.plot(cs_actual_x, cs_actual_y, label='actual')
plt.plot(xf, yf, label='filtered')
plt.legend()
plt.show()
dxf = xf[2:] - xf[:-2]
dyf = yf[2:] - yf[:-2]
yaw = np.zeros_like(xf)
yaw[1:-1] = np.arctan2(dyf, dxf)
yaw[0] = yaw[1]
yaw[-1] = yaw[-2]
yawf = wrap_angle(savgol_filter(np.unwrap(yaw), 201, 3))
plt.plot(yaw, label='yaw')
plt.plot(yawf, label='filtered')
plt.legend()
plt.show()
dx = cs_actual_x[2:] - cs_actual_x[:-2]
dy = cs_actual_y[2:] - cs_actual_y[:-2]
yaw = np.zeros_like(xf)
yaw[1:-1] = np.arctan2(dy, dx)
yaw[0] = yaw[1]
yaw[-1] = yaw[-2]
yawf = wrap_angle(savgol_filter(np.unwrap(yaw), 151, 3))
plt.plot(yaw, label='yaw')
plt.plot(yawf, label='filtered')
plt.legend()
plt.show()<jupyter_output><empty_output><jupyter_text>ANDROID IMU DATA<jupyter_code>imu_t = []
imu_a = []
imu_w = []
imu_q = []
imu_rpy = []
for topic, msg, _ in bag.read_messages(topics=['/imu']):
    imu_t.append(msg.header.stamp.to_sec())
    acc = msg.linear_acceleration
    imu_a.append([acc.x, acc.y, acc.z])
    acc = msg.angular_velocity
    imu_w.append([acc.x, acc.y, acc.z])
    q = msg.orientation
    imu_q.append([q.x, q.y, q.z, q.w])
    imu_rpy.append(to_euler(q.x, q.y, q.z, q.w))
imu_t = np.array(imu_t)
imu_a = np.array(imu_a)
imu_w = np.array(imu_w)
imu_q = np.array(imu_q)
imu_rpy = np.array(imu_rpy)<jupyter_output><empty_output><jupyter_text>PERBANDINGAN<jupyter_code>plt.plot(cs_t - cs_t[0], np.unwrap(yaw)*180/np.pi)
lim = 248
plt.plot(imu_t[lim:-1] - imu_t[lim], (np.unwrap(imu_rpy[:,-1])[lim:-1] + np.unwrap(yaw)[0] - np.unwrap(imu_rpy[:,-1])[lim])*180/np.pi)
plt.plot(imu_a[:,:])
# plt.plot(imu_a[:220,0])
plt.plot(imu_a[:220,2])
vx = cumtrapz(imu_a[:,0] - np.mean(imu_a[:220,0]), imu_t, initial=0)
vy = cumtrapz(imu_a[:,1] - np.mean(imu_a[:220,1]), imu_t, initial=0)
vz = cumtrapz(imu_a[:,2] - np.mean(imu_a[:220,2]), imu_t, initial=0)
plt.plot(vx)
plt.plot(vy)
plt.plot(vz)
plt.plot(imu_rpy[:,0]*180/np.pi)
plt.plot(imu_rpy[:,1]*180/np.pi)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.18179255918827508, 'fraction_numerical': 0.02452085682074408, 'mean_word_length': 5.68361581920904, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 13, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '3', 'dir': 'jupyter-structured-clean-dedup', 'id': '400889', 'license': 'MIT', 'n_tokens_mistral': 1823, 'n_tokens_neox': 1755, 'n_words': 203, 'path': 'Archieved FP/Coba EKF Imu GNSS/yaw comparison android vs dy dx.ipynb', 'repo_name': 'fadamsyah/final-project', 'size': '3548'}