AHRSalgorithm = MadgwickAHRS(
sampleperiod=np.round(1 / Fs, decimals=4))
# Initial convergence
initPeriod = tempo_parado # usually 2 seconds
# indexSel = 1 : find(sign(time-(time(1)+initPeriod))+1, 1);
np.nonzero((np.sign(time - startTime) + 1) > 0)[0][0]
indexSel = np.arange(
0,
np.nonzero(np.sign(time - (time[0] + initPeriod)) + 1)[0][0],
1)
for i in range(1, 2000):
AHRSalgorithm.update_imu_new([0, 0, 0], [
accX[indexSel].mean(), accY[indexSel].mean(),
accZ[indexSel].mean()
])
# For all data
for t in range(len(time)):
if stationary[t]:
AHRSalgorithm.beta = 0.5
else:
AHRSalgorithm.beta = 0
AHRSalgorithm.update_imu_new(
np.deg2rad([gyrX[t], gyrY[t], gyrZ[t]]),
[accX[t], accY[t], accZ[t]])
quat[t] = AHRSalgorithm.quaternion
quats = []
quat = [[0] * 4] * len(time)
AHRSalgorithm = MadgwickAHRS(sampleperiod=np.round(1 / Fs, decimals=4))
# Initial convergence
initPeriod = tempo_parado # usually 2 seconds
# indexSel = 1 : find(sign(time-(time(1)+initPeriod))+1, 1);
np.nonzero((np.sign(time - startTime) + 1) > 0)[0][0]
indexSel = np.arange(
0,
np.nonzero(np.sign(time - (time[0] + initPeriod)) + 1)[0][0] - 1, 1)
for i in range(1, 2000):
AHRSalgorithm.update_imu_new([0, 0, 0], [
np.around(accX[indexSel].mean(), decimals=4),
np.around(accY[indexSel].mean(), decimals=4),
np.around(accZ[indexSel].mean(), decimals=4)
])
# For all data
for t in range(len(time)):
if stationary[t]:
AHRSalgorithm.beta = 0.5
else:
AHRSalgorithm.beta = 0
AHRSalgorithm.update_imu_new(
np.deg2rad([
np.around(gyrX[t], decimals=4),
np.around(gyrY[t], decimals=4),
np.around(gyrZ[t], decimals=4)