Commit 327b30bb authored by dhomm's avatar dhomm
Browse files

add preparation files to main branch

parent dbc4eab5
%% Cell type:code id:729808fa tags:
``` python
import matplotlib.pyplot as plt
import numpy as np
from numpy import mean, sqrt, square, var, corrcoef
```
%% Cell type:code id:1a94bc3d tags:
``` python
def lr_distinction(body_part, first_component, second_component):
"""
This function distinguishes if the sensor is placed on the left or right
body part
:args:
body_part: the body_part the sensor was assigned to
first_component, second_component: the components which will be compared
The components should be as following for differen bps
upper arm:
first = sternum orientation (theta_x), second = upper arm z-accelerometer
upper leg:
first = pelvis orientation (theta_x), second upper leg z-accelerometer
shoulder:
first = shoulder, second = upper arm : z-accelerometer
forearm:
first = forearm, second = upper arm : x-accelerometer
hands:
first = hands, second = forearms : y-accelerometer
lower legs:
first = lower legs, second = upper legs : x-accelerometer
feet:
first = feet, second = lower legs : x-accelerometer
:return:
left or right as string
"""
cc = corrcoef(first_component, second_component)
if body_part == "upper arm":
if cc <= 1:
return "left"
else:
return "right"
if body_part == "upper leg":
if cc <= 1:
return "left"
else:
return "right"
if body_part == "shoulder":
if cc <= xxx:
return "left"
else:
return "right"
if body_part == "forearm":
if cc <= xxx:
return "left"
else:
return "right"
if body_part == "hands":
if cc <= xxx:
return "left"
else:
return "right"
if body_part == "lower legs":
if cc <= xxx:
return "left"
else:
return "right"
if body_part == "feet":
if cc <= xxx:
return "left"
else:
return "right"
```
This source diff could not be displayed because it is too large. You can view the blob instead.
%% Cell type:code id:3f0b605f tags:
``` python
import numpy as np
from scipy.constants import g
from ahrs.filters import Madgwick
# import ../../../used_scripts/geometry/tf/src/tf/transformations
import transformations
```
%% Cell type:code id:e2f84fd9 tags:
``` python
def get_inital_quaternions(acc_data, gyro_data, magnetic_data=0, frequency=100):
"""
See build quaternions, the sensor data however should only include static standing
data, for estimating the initial orientation of the sensors.
"""
return build_quaternions(acc_data, gyro_data, magnetic_data=magnetic_data, q0=0, frequency=frequency)
```
%% Cell type:code id:2cd34a63 tags:
``` python
def calc_q0(initial_madgwick):
"""
This function calculates the mean of every value in the inital madgwick object
"""
q0 = [[], [], [], []]
for q in initial_madgwick.Q:
for i in range(len(q0)):
q0[i].append(q[i])
for i in range(len(q0)):
q0[i] = sum(q0[i])/len(q0[i])
return q0
```
%% Cell type:code id:bbbd93d6 tags:
``` python
def build_quaternions(acc_data, gyro_data, magnetic_data=0, q0=0, frequency=100):
"""
This function creates quaternions based on the given data
:args:
accel_data: a set of accelerometer data (Nx3 array)
gyro_data: a set of gyroscope data (Nx3 array)
magnetic_data: if given a set of magnetic data (Nx3 array)
q_init: an initial quaterion showing the initial rotation
:return:
if every given data set has the same length N: N quaternions showing the rotation of N samples
else:
raising Value Error
"""
if magnetic_data == 0 and q0 == 0:
madgwick = Madgwick(acc=acc_data, gyr=gyro_data, frequency=frequency)
elif magnetic_data == 0 and q0 != 0:
madgwick = Madgwick(acc=acc_data, gyr=gyro_data, q0=q0, frequency=frequency)
elif magnetic_data != 0 and q0 == 0:
madgwick = Madgwick(acc=acc_data, gyr=gyro_data, mag=mag_data, frequency=frequency)
elif magnetic_data != 0 and q0 != 0:
madgwick = Madgwick(acc=acc_data, gyr=gyro_data, mag=mag_data, q0=q0, frequency=frequency)
return madgwick
```
%% Cell type:code id:043e84f4 tags:
``` python
# rotate vector v1 by quaternion q1
# quelle: https://answers.ros.org/question/196149/how-to-rotate-vector-by-quaternion-in-python/
def qv_mult(q1, v1):
# v1 = transformations.unit_vector(v1)
q2 = list(v1)
q2.append(0.0)
return transformations.quaternion_multiply(
transformations.quaternion_multiply(q1, q2),
transformations.quaternion_conjugate(q1)
)[:3]
```
%% Cell type:code id:91ddebcd tags:
``` python
def rotate_samples(quaternions, data_set):
"""
This function rotates the samples given in this data_set
with the help of the quaternions
:args:
quaternions:
data_set:
:return:
rotated samples
"""
if len(quaternions) != len(data_set):
raise Exception("Length of quaternion does not fit length of data set")
rotated_data = []
for index in range(len(data_set)):
rotated_sample = qv_mult(quaternions[index], data_set[index])
rotated_data.append(rotated_sample)
return np.array(rotated_data)
```
%% Cell type:code id:c90fc793 tags:
``` python
def preprocess_data(standing_acc, standing_gyro, walking_acc, walking_gyro, freq):
"""
This function combines the preprocessing steps to be able to compare samples from different
body_parts and subjects.
:args:
acc_standing:
gyro_standing:
acc_walking:
gyro_walking:
freq:
:return:
rotated accerlation and gyro data from their walking samples
"""
initial_madgwick = get_inital_quaternions(standing_acc, standing_gyro, magnetic_data=0, frequency=freq)
q0 = calc_q0(initial_madgwick)
quaternions = build_quaternions(walking_acc, walking_gyro, magnetic_data=0, q0=q0, frequency=freq)
rotated_acc = rotate_samples(quaternions.Q, walking_acc)
rotated_gyro = rotate_samples(quaternions.Q, walking_gyro)
return rotated_acc, rotated_gyro
```