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'])