forked from SensibilityTestbed/indoor-localization
-
Notifications
You must be signed in to change notification settings - Fork 0
/
offline_thm1.r2py
94 lines (77 loc) · 3.42 KB
/
offline_thm1.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
90
91
92
93
94
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
dy_import_module_symbols('orientation_filter.r2py')
dy_import_module_symbols('stat_stream.r2py')
dy_import_module_symbols('precalibration.r2py')
#caldata = import_csv("samsung_100hz_calibration.csv")
#caldata = import_csv("180cal.csv")
caldata = import_csv("sony_50hz_calibration.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
y_vector = matrix_minus(gravity_vector, [[acc_bias, acc_bias, acc_bias]])[0]
pre.gravity_vector = [-1.055575563, 1.546005253, 9.4199443]
#pre.bias_variance = [0.000101659, 0.000131985, 0.000330916]
pre.bias_variance = [0.00018941, 0.000173484, 0.000569715]
pre.gyro_stats = gyro_stream
#pre.gyro_stats.variance = [0.000317183, 2.45004E-05, 1.80867E-06]
pre.gyro_stats.variance = [2.82456E-05, 2.01803E-05, 3.64261E-05]
pre.magnet_stats = magnet_stream
#pre.magnet_stats.variance = [0.141777582, 0.058222208, 0.650667031]
pre.magnet_stats.variance = [0.069341112, 0.107692745, 0.034996693]
#pre.magnetic_field = [[-21.5200004], [-3.26000004], [-36.2800002]]
pre.magnetic_field = [[-8.7419892], [-22.9457665], [-24.9451445]]
compass = OrientationFilter(pre)
#data = import_csv("samsung_100hz_rotation.csv")
#data = import_csv("180.csv")
#data = import_csv("samsung_100hz_trousers2.csv")
data = import_csv("sony_trousers_hand_mv2.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()
fh = openfile('kalman.csv', True)
fh.writeat('theta\n', 0)
EOF = 6
for entry in data:
theta = compass.get_heading(entry)
fh.writeat(str(theta) + '\n', EOF)
EOF += len(str(theta)) + 1
fh.close()
# 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')