-
Notifications
You must be signed in to change notification settings - Fork 3
/
offline.r2py
89 lines (77 loc) · 3.33 KB
/
offline.r2py
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
datakeys = ['time', 'xforce', 'yforce', 'zforce', 'pitch', 'roll', 'yaw', 'xmag', 'ymag', 'zmag']
def import_csv(filename):
data = []
fh = openfile(filename, False)
fstr = fh.readat(None, 0)
fstr = fstr[fstr.find('\n')+1:-1]
for line in fstr.split('\n'):
strdata = line.split(',')
zipped = zip(datakeys, strdata)
entry = {}
for key, value in zipped:
entry[key] = float(value)
data.append(entry)
return data
log("=============================\n")
dy_import_module_symbols('orientation_filter.r2py')
dy_import_module_symbols('stat_stream.r2py')
dy_import_module_symbols('precalibration.r2py')
log("=============================\n")
#caldata = import_csv("samsung_100hz_calibration.csv")
caldata = import_csv("180cal.csv")
acc_stream = StatStream()
gyro_stream = StatStream()
magnet_stream = StatStream()
gravity_vector = matrix_init(1,3)
for meas in caldata:
acc = [meas['xforce'], meas['yforce'], meas['zforce']]
gravity_vector = matrix_add(gravity_vector, [acc])
acc_stream.update(matrix_row_magnitude(acc))
gyro_stream.update(matrix_row_magnitude([meas['pitch'], meas['roll'], meas['yaw']]))
magnet_stream.update(matrix_row_magnitude([meas['xmag'], meas['ymag'], meas['zmag']]))
gravity_vector = matrix_scale(gravity_vector, 1.0 / len(caldata))
pre = PreCalibration(1, 0)
pre.acc_stats = acc_stream
acc_bias = ((acc_stream.mean - 9.81) ** 2 / 3) ** 0.5
pre.gravity_vector = matrix_minus(gravity_vector, [[acc_bias, acc_bias, acc_bias]])[0]
#pre.bias_variance = [0.000101659, 0.000131985, 0.000330916]
pre.bias_variance = [0.000156394, 0.000154625, 0.000480442]
pre.gyro_stats = gyro_stream
#pre.gyro_stats.variance = [0.000317183, 2.45004E-05, 1.80867E-06]
pre.gyro_stats.variance = [0.000352667, 4.34755E-05, 1.1015E-05]
pre.magnet_stats = magnet_stream
#pre.magnet_stats.variance = [0.004040362, 0.004545723, 0.149950678]
pre.magnet_stats.variance = [0.025490904, 0.026824079, 0.041586818]
#pre.magnetic_field = [[-27.300001], [9.2], [-20.7]]
pre.magnetic_field = [[16.0], [79.3], [-63.4]]
#pre.magnetic_intensity = 51.8345
"""
bias:
flat 0.030357569 -0.107287924 -0.28257366
drop 0.101382471 -0.147303259 -0.080313496
0.107027756 -0.13419674 -0.116084084
flip 0.006756171 -0.015592093 -0.25117373
"""
compass = OrientationFilter(pre)
#data = import_csv("samsung_100hz_rotation.csv")
data = import_csv("180.csv")
# v = matrix_transpose([pre.gravity_vector])
# g = [[0.], [0.], [1.]]
# log('0', matrix_multiply(quaternion_to_matrix(quaternion_from_gravity(pre.gravity_vector)), g), '\n')
#acc = [data[0]['xforce'], data[0]['yforce'], data[0]['zforce'], 0.]
#acc = matrix_scale([acc], 1.0 / matrix_row_magnitude(acc))[0]
#log('start:', matrix_multiply(quaternion_to_matrix(quaternion_inverse(quaternion_from_gravity(pre.gravity_vector))), acc), '\n')
#log('start:', quaternion_rotate(quaternion_from_gravity(pre.gravity_vector), acc)[:3], '\n')
# cnt = 1
start = getruntime()
for entry in data:
q = compass.get_orientation(entry)
# log(cnt, quaternion_rotate(q, v)[:3], '\n')
# log(cnt, matrix_multiply(quaternion_to_matrix(q), g), '\n')
# cnt += 1
acc = [data[-1]['xforce'], data[-1]['yforce'], data[-1]['zforce'], 0.]
acc = matrix_scale([acc], 1.0 / matrix_row_magnitude(acc))[0]
log('end:', quaternion_rotate(q, acc)[:3], '\n\n')
#log('acc:', acc, '\n\n')
end = getruntime() - start
#log("\n Total runtime: ", end, '\n\n')