def run_simulation(args): t_end = args.time dt = args.dt start_angle = 0.05 accel = Accel((args.accel_stddev_x, args.accel_stddev_z), args.gravity) gyro = Gyro(args.gyro_stddev, args.gyro_biasdrift_stddev) filt = ComplementaryFilter(args.filter_acc_weight, args.gravity, start_angle) accel_data = [] gyro_data = [] angles = [] true_angle = start_angle true_angvel = 0.0 ts = np.arange(0, t_end, dt) true_angles = true_angle * np.ones(shape=ts.shape) for i, t in enumerate(ts): acc_obs = accel.get(true_angles[i]) angvel_obs = gyro.get(true_angvel, dt) accel_data.append(acc_obs) gyro_data.append(angvel_obs) ang_est = filt.get(acc_obs, angvel_obs, dt) angles.append(ang_est) angles = np.array(angles) angle_error = angles - true_angles mean_abs_error = np.mean(np.abs(angle_error)) if not args.no_print: print("Mean abs error is %f radians (%f degrees)" % (mean_abs_error, np.degrees(mean_abs_error))) if not args.no_plot: import matplotlib.pyplot as plt plt.figure("Filter Results") plt.plot(ts, angles, label='Estimated') plt.plot(ts, true_angles, label='True') plt.xlabel('Time (s)') plt.ylabel('Angle (rad)') plt.title("Estimated vs. True Angle") plt.legend() plt.figure("Angle Error") plt.plot(ts, angle_error) plt.xlabel('Time (s)') plt.ylabel('Angle Error (rad)') plt.title("Angle Error") accel_data = np.array(accel_data) accel.plot(ts, accel_data) gyro_data = np.array(gyro_data) gyro.plot(ts, gyro_data) plt.show() return mean_abs_error