forked from xioTechnologies/Fusion
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathadvanced_example.py
109 lines (81 loc) · 3.89 KB
/
advanced_example.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
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
import imufusion
import matplotlib.pyplot as pyplot
import numpy
import sys
# Import sensor data
data = numpy.genfromtxt("sensor_data.csv", delimiter=",", skip_header=1)
sample_rate = 100 # 100 Hz
timestamp = data[:, 0]
gyroscope = data[:, 1:4]
accelerometer = data[:, 4:7]
magnetometer = data[:, 7:10]
# Instantiate algorithms
offset = imufusion.Offset(sample_rate)
ahrs = imufusion.Ahrs()
ahrs.settings = imufusion.Settings(0.5, # gain
10, # acceleration rejection
20, # magnetic rejection
5 * sample_rate) # rejection timeout = 5 seconds
# Process sensor data
delta_time = numpy.diff(timestamp, prepend=timestamp[0])
euler = numpy.empty((len(timestamp), 3))
internal_states = numpy.empty((len(timestamp), 6))
flags = numpy.empty((len(timestamp), 5))
for index in range(len(timestamp)):
gyroscope[index] = offset.update(gyroscope[index])
ahrs.update(gyroscope[index], accelerometer[index], magnetometer[index], delta_time[index])
euler[index] = ahrs.quaternion.to_euler()
ahrs_internal_states = ahrs.internal_states
internal_states[index] = numpy.array([ahrs_internal_states.acceleration_error,
ahrs_internal_states.accelerometer_ignored,
ahrs_internal_states.acceleration_rejection_timer,
ahrs_internal_states.magnetic_error,
ahrs_internal_states.magnetometer_ignored,
ahrs_internal_states.magnetic_rejection_timer])
ahrs_flags = ahrs.flags
flags[index] = numpy.array([ahrs_flags.initialising,
ahrs_flags.acceleration_rejection_warning,
ahrs_flags.acceleration_rejection_timeout,
ahrs_flags.magnetic_rejection_warning,
ahrs_flags.magnetic_rejection_timeout])
def plot_bool(axis, x, y, label):
axis.plot(x, y, "tab:cyan", label=label)
pyplot.sca(axis)
pyplot.yticks([0, 1], ["False", "True"])
axis.grid()
axis.legend()
# Plot Euler angles
figure, axes = pyplot.subplots(nrows=12, sharex=True, gridspec_kw={"height_ratios": [6, 1, 2, 1, 1, 1, 1, 2, 1, 1, 1, 1]})
figure.suptitle("Euler angles, internal states, and flags")
axes[0].plot(timestamp, euler[:, 0], "tab:red", label="Roll")
axes[0].plot(timestamp, euler[:, 1], "tab:green", label="Pitch")
axes[0].plot(timestamp, euler[:, 2], "tab:blue", label="Yaw")
axes[0].set_ylabel("Degrees")
axes[0].grid()
axes[0].legend()
# Plot initialising flag
plot_bool(axes[1], timestamp, flags[:, 0], "Initialising")
# Plot acceleration rejection internal states and flags
axes[2].plot(timestamp, internal_states[:, 0], "tab:olive", label="Acceleration error")
axes[2].set_ylabel("Degrees")
axes[2].grid()
axes[2].legend()
plot_bool(axes[3], timestamp, internal_states[:, 1], "Accelerometer ignored")
axes[4].plot(timestamp, internal_states[:, 2], "tab:orange", label="Acceleration rejection timer")
axes[4].grid()
axes[4].legend()
plot_bool(axes[5], timestamp, flags[:, 1], "Acceleration rejection warning")
plot_bool(axes[6], timestamp, flags[:, 2], "Acceleration rejection timeout")
# Plot magnetic rejection internal states and flags
axes[7].plot(timestamp, internal_states[:, 3], "tab:olive", label="Magnetic error")
axes[7].set_ylabel("Degrees")
axes[7].grid()
axes[7].legend()
plot_bool(axes[8], timestamp, internal_states[:, 4], "Magnetometer ignored")
axes[9].plot(timestamp, internal_states[:, 5], "tab:orange", label="Magnetic rejection timer")
axes[9].grid()
axes[9].legend()
plot_bool(axes[10], timestamp, flags[:, 3], "Magnetic rejection warning")
plot_bool(axes[11], timestamp, flags[:, 4], "Magnetic rejection timeout")
if len(sys.argv) == 1: # don't show plots when script run by CI
pyplot.show()