-
Notifications
You must be signed in to change notification settings - Fork 2
/
main.py
74 lines (54 loc) · 1.76 KB
/
main.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
import argparse
import numpy as np
from accel import Accel
from est import ComplementaryFilter
from gyro import Gyro
from utils import get_args
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
if __name__ == "__main__":
run_simulation(get_args())