-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.py
146 lines (120 loc) · 4.87 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
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
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
import secrets
import numpy as np
import math
import pandas as pd
from sklearn.metrics import mean_squared_error as mse
from dtaidistance import dtw
from alpha_beta_filter import alphaBetaFilter
from mse import devMse
from re_orientation import reorientation
input_data = pd.read_excel(r'input.xlsx')
summation = np.zeros((6,), dtype=int)
mse_sum = np.zeros((6,), dtype=int)
b_mse_sum = np.zeros((6,), dtype=int)
dtw_sum = np.zeros((6,), dtype=int)
b_dtw_sum = np.zeros((6,), dtype=int)
iter_num = 0
index = 330
acc = np.array([input_data['ACCELEROMETER X (m/s²)']
, input_data['ACCELEROMETER Y (m/s²)']
, input_data['ACCELEROMETER Z (m/s²)']])
mag = np.array([input_data['MAGNETIC FIELD X (μT)']
, input_data['MAGNETIC FIELD Y (μT)']
, input_data['MAGNETIC FIELD Z (μT)']])
speed = np.array([input_data['LOCATION Speed ( Kmh)']])
def increaseSpeed(speed: list) -> int:
"""
Returns 1 if the overal speed has increased otherwise returns -1.
:param speed: A list of speeds at each point
:return: Has the speed increased or not (1, -1)
"""
number_of_points = speed.__len__()
speed_change = 0
for i in range(number_of_points - 1):
speed_change = speed(i + 1) - speed(i)
speed_change = speed_change / (number_of_points - 1)
if speed_change > 0:
has_increase = 1
else:
has_increase = -1
return has_increase
def makeYZero(ay: list, az: list) -> float:
"""
Calculate theta_x angle
:param ay: A list of Y-axis accelerations
:param az: A list of Z-axis accelerations
:return: Theta_x angle
"""
avgy = np.mean(ay)
avgz = np.mean(az)
theta_x = math.atan(avgy / avgz)
if abs(theta_x) < 0.1:
theta_x = 0
return theta_x
def makeXZero(ax: list, az: lsit) -> float:
"""
Calculate theta_y angle
:param ax: A list of X-axis accelerations
:param az: A list of Z-axis accelerations
:return: Theta_y angle
"""
avgx = np.mean(ax)
avgz = np.mean(az)
theta_y = math.atan(avgx / avgz)
if abs(theta_y) < 0.1:
theta_y = 0
return theta_y
for k in range(2):
teta_x = 2 * math.pi * secrets.SystemRandom().random()
teta_y = 2 * math.pi * secrets.SystemRandom().random()
teta_z = 2 * math.pi * secrets.SystemRandom().random()
print("random degrees:\nteta_x: {}\nteta_y: {}\nteta_z: {}\n".format(teta_x, teta_y, teta_z))
oAcc = reorientation(acc, teta_x, teta_y, teta_z)
oMag = reorientation(mag, teta_x, teta_y, teta_z)
for i in range(3):
b_mse_sum[i] = b_mse_sum[i] + devMse(alphaBetaFilter(oAcc[i], 0.01, 0), alphaBetaFilter(acc[i], 0.01, 0))
for i in range(3):
b_mse_sum[i + 3] = b_mse_sum[i + 3] + mse(oMag[i], mag[i])
teta = makeYZero(oAcc[1, index:index + 50], oAcc[2, index:index + 50])
oAcc = reorientation(oAcc, teta, 0, 0)
oMag = reorientation(oMag, teta, 0, 0)
print('first teta: {}'.format(teta))
teta = makeXZero(oAcc[0, index:index + 50], oAcc[2, index:index + 50])
oAcc = reorientation(oAcc, teta, 0, 0)
oMag = reorientation(oMag, teta, 0, 0)
print('second teta: {}'.format(teta))
if np.mean(oAcc[2]) < 0:
oAcc = reorientation(oAcc, 0, math.pi, 0)
oMag = reorientation(oMag, 0, math.pi, 0)
# Final phase:
teta = math.atan(np.mean(oAcc[0, index:index + 20]) / np.mean(oAcc[1, index:index + 20]))
print('second teta: {}\n'.format(teta))
oAcc = reorientation(oAcc, 0, 0, teta)
oMag = reorientation(oMag, 0, 0, teta)
if increaseSpeed(speed[index:index + 20] * np.mean(oAcc[1])) > 0:
oAcc = reorientation(oAcc, 0, 0, math.pi)
oMag = reorientation(oMag, 0, 0, math.pi)
print("turned")
print("final cross correlation is: {}".format((np.mean(np.correlate(mag[1], oMag[1])) + 1) / 2))
print(np.mean(oAcc[2]))
iter_num = iter_num + 1
for i in range(3):
summation[i] = (np.mean(np.correlate(alphaBetaFilter(acc[i], 0.01, 0),
alphaBetaFilter(oAcc[i], 0.01, 0)))) / 2 + summation[i]
mse_sum[i] = mse_sum[i] + devMse(alphaBetaFilter(acc[i], 0.01, 0), alphaBetaFilter(oAcc[i], 0.01, 0))
dtw_sum[i] = dtw.distance(alphaBetaFilter(acc[i], 0.01, 0), alphaBetaFilter(oAcc[i], 0.01, 0)) + dtw_sum[i]
for i in range(3):
summation[i + 3] = (np.mean(np.correlate(mag[i], oMag[i])) + 1) / 2 + summation[i + 3]
mse_sum[i + 3] = mse_sum[i + 3] + devMse(mag[i], oMag[i])
dtw_sum[i + 3] = dtw.distance(mag[i], oMag[i]) + dtw_sum[i + 3]
print(iter_num)
print('====&final result&=====')
print('cross correlation:')
for i in range(6):
print(i, ') ', summation[i] / iter_num)
print('MSE:')
for i in range(6):
print(i, ') ', mse_sum[i] / b_mse_sum[i])
print('DTW:')
for i in range(6):
print(i, ') ', dtw_sum[i] / iter_num)