gy_raw = gy_raw - gy_offset
gz_raw = gz_raw - gz_offset
ax, ay, az, gx, gy, gz = ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw
# get measure
fi = np.arctan(ax/max(np.sqrt(np.sum(ay**2+az**2)), 1e-8))*180/np.pi
theta = np.arctan(ay/max(az, 1e-8))*180/np.pi
if should_init_kf:
should_init_kf = False
fi_kf.set_init_x = np.array([[fi], [0]])
theta_kf.set_init_x = np.array([[theta], [0]])
fi_predict = fi_kf.predict()[0, 0]
fi_estimate = fi_kf.measurement(np.array([[fi]]))[0, 0]
theta_predict = theta_kf.predict()[0, 0]
theta_estimate = theta_kf.measurement(np.array([[theta]]))[0, 0]
fi_raw_list.append(fi)
theta_raw_list.append(theta)
fi_predict_list.append(fi_predict)
theta_predict_list.append(theta_predict)
fi_estimate_list.append(fi_estimate)
theta_estimate_list.append(theta_estimate)
plt.figure()
gy_raw = gy_raw - gy_offset
gz_raw = gz_raw - gz_offset
ax, ay, az, gx, gy, gz = ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw
# get measure
fi = np.arctan(ax/max(np.sqrt(np.sum(ay**2+az**2)), 1e-8))*180/np.pi
theta = np.arctan(ay/max(az, 1e-8))*180/np.pi
if should_init_kf:
should_init_kf = False
fi_kf.set_init_x = np.array([[fi], [0]])
theta_kf.set_init_x = np.array([[theta], [0]])
fi_kf.predict()
fi_predict = fi_kf.measurement(np.array([[fi]]))[0, 0]
theta_kf.predict()
theta_predict = theta_kf.measurement(np.array([[theta]]))[0, 0]
fi_raw_list.append(fi)
theta_raw_list.append(theta)
fi_predict_list.append(fi_predict)
theta_predict_list.append(theta_predict)
plt.figure()
plt.plot(x_time[offset_num:], fi_raw_list)
plt.plot(x_time[offset_num:], fi_predict_list)
plt.title('fi'+'q_'+str(q)+' r_'+str(r))
plt.legend(['raw', 'predict'])
