Уточнение калибровки датчика-сегмента IMU для алгоритма голени ⇐ Python

[Anonymous]

В настоящее время я работаю над проектом, целью которого является интеграция системы захвата движения IMU для обеспечения вибротактильной обратной связи через вибромоторы. Проект ориентирован на бегунов и может быть использован либо для повышения производительности, либо для предотвращения травм. Прямо сейчас я сосредоточен на использовании ИДУ для сбора необходимых параметров, в данном случае — ускорения большеберцовой кости. Я работал над алгоритмом калибровки датчика по сегменту и уже получил некоторые результаты, в достоверности которых я не совсем уверен. Я надеялся, что кто-нибудь сможет дать мне какой-нибудь совет или сказать, если я упускаю что-то важное.
Некоторая актуальная информация:

• Я использую датчики Movella Dot, которые уже позволяют определить ориентацию датчика с помощью фильтра Калмана.
• Я размещаю датчик в медиальной передней части голени (костной части).
• Пока я проведение статической и функциональной калибровки. Где статика — это короткая запись стоя, а функционал — запись медленной ходьбы. При статической калибровке я получаю вертикальную ось, измеряющую вектор гравитации. Из функциональной калибровки я получаю передне-заднюю ось, используя анализ главных компонентов записи ускорения. Наконец, я просто корректирую оси, чтобы обеспечить ортогональность, вычисляю ось Y как векторное произведение, нормализую и, наконец, получаю матрицу преобразования.

Вот что у меня есть на данный момент:
import numpy as np
import pandas as pd
from scipy import signal
from scipy.spatial.transform import Rotation as R
from sklearn.decomposition import PCA
from sklearn.preprocessing import StandardScaler
import matplotlib.pyplot as plt

# Butterworth filter
def filtering(fs, cutoff, data):
order = 2
b, a = signal.butter(order, (2 * cutoff / fs), btype='low')
filt_data = signal.filtfilt(b, a, data, axis=0)
return filt_data

# Extract accelerometer, gyroscope, and quaternion data
def get_data(filename):
data = pd.read_csv(filename, delimiter=',', header=0)

acc_data = data[['Acc_X', 'Acc_Y', 'Acc_Z']].values / 9.81 # Convert to m/s^2
gyr_data = data[['Gyr_X', 'Gyr_Y', 'Gyr_Z']].values * (np.pi / 180) # Convert to rad/s
quaternions = data[['Quat_W', 'Quat_X', 'Quat_Y', 'Quat_Z']].values

return acc_data, gyr_data, quaternions

# Rotate vector using quaternion
def rotate_vector(quaternion, vector):
r = R.from_quat(quaternion) # Quaternion (w, x, y, z)
rotated_vector = r.apply(vector)
return rotated_vector

# Static calibration: Z-axis alignment (NOT NORMALIZED)
def static_calibration(filename):
[acc_data, gyr_data, quaternions] = get_data(filename)

fs = 120 # Sampling frequency
N = len(acc_data)

# Rotate accelerometer data to global frame using quaternions
a_global = np.zeros((N, 3))
for i in range(120, N-1):
q = quaternions # Get quaternion for this sample
a_global[i, :] = rotate_vector(q, acc_data[i, :]) # Rotate using quaternion

# Filter the rotated accelerometer data
a_global_filtered = filtering(fs, 15, a_global)

# Estimate gravity vector during the static upright position
Tstart, Tend = 0, N # Replace with actual indices for static period
gravity_vector = np.mean(a_global_filtered[Tstart:Tend, :], axis=0)
gravity_magnitude = np.linalg.norm(gravity_vector)
sensor_z_axis = gravity_vector

return sensor_z_axis, a_global_filtered, gyr_data

# Functional calibration: Rotate raw data and apply PCA to define X-axis (NOT NORMALIZED)
def functional_calibration(filename):
[acc_data,_, quaternions] = get_data(filename)

fs = 120
N = len(acc_data)

# Rotate accelerometer data to global frame using quaternions
a_global = np.zeros((N, 3))
for i in range(120, N-1):
q = quaternions # Get quaternion for this sample
a_global[i, :] = rotate_vector(q, acc_data[i, :]) # Rotate using quaternion

# Filter the rotated accelerometer data
a_global_filtered = filtering(fs, 15, a_global)

# Now apply PCA to rotated accelerometer data to define the X-axis
scaler = StandardScaler()
a_global_scaled = scaler.fit_transform(a_global_filtered)

pca = PCA(n_components=1)
pca.fit(a_global_scaled)
sensor_x_axis = pca.components_[0]
#sensor_x_axis /= np.linalg.norm(sensor_x_axis) # Normalize

return sensor_x_axis, a_global_filtered

# Compute Y-axis to ensure orthogonality and right-handed system
def gram_schmidt(sensor_x_axis, sensor_z_axis):
# Apply Gram-Schmidt process to get orthogonal vectors
sensor_x_axis_corrected = sensor_x_axis - (np.dot(sensor_x_axis, sensor_z_axis)/np.dot(sensor_z_axis, sensor_z_axis)) * sensor_z_axis

# Compute Y-axis as cross product of Z and X to enforce right-handed system
sensor_y_axis = np.cross(sensor_x_axis_corrected, sensor_z_axis)

# Get orthonormal vectors by normalizing
sensor_x_axis_norm = sensor_x_axis_corrected / np.linalg.norm(sensor_x_axis_corrected)
sensor_y_axis_norm = sensor_y_axis / np.linalg.norm(sensor_y_axis)
sensor_z_axis_norm = sensor_z_axis / np.linalg.norm(sensor_z_axis)

return sensor_x_axis_norm, sensor_y_axis_norm, sensor_z_axis_norm

# Combine axes into transformation matrix and align data
def apply_transformation(a_global_filtered, gyr_data, transformation_matrix):
aligned_acc = np.dot(a_global_filtered, transformation_matrix.T)
aligned_gyr = np.dot(gyr_data, transformation_matrix.T)
return aligned_acc, aligned_gyr

# Main function to align data
def align_tibia_data(static_file, functional_file):
sensor_z_axis, static_acc, gyr_data = static_calibration(static_file) # Z-axis normalized
sensor_x_axis, functional_acc = functional_calibration(functional_file)
tibia_x_axis_norm, tibia_y_axis_norm, tibia_z_axis_norm = gram_schmidt(sensor_x_axis, sensor_z_axis)

print(f"X & Z: {np.dot(tibia_z_axis_norm, tibia_x_axis_norm)}\nX & Y: {np.dot(tibia_y_axis_norm, tibia_x_axis_norm)}\nZ % Y: {np.dot(tibia_y_axis_norm, tibia_z_axis_norm)}")
# Transformation matrix
transformation_matrix = np.vstack([tibia_x_axis_norm, tibia_y_axis_norm, tibia_z_axis_norm])

# Align accelerometer and gyroscope data
aligned_acc, aligned_gyr = apply_transformation(functional_acc, gyr_data, transformation_matrix)

plot_acceleration(functional_acc, aligned_acc)

return aligned_acc, aligned_gyr, transformation_matrix

def plot_acceleration(a_global, aligned_acc, fs = 120):
time = np.arange(len(a_global)) / fs

# Plot raw vs aligned acceleration
fig, axs = plt.subplots(3, 1, figsize=(10, 8))

for i, axis in enumerate(['X', 'Y', 'Z']):
axs.plot(time, a_global[:, i], label=f'Raw {axis}')
axs.plot(time, aligned_acc[:, i], label=f'Aligned {axis}')
axs.set_xlabel('Time (s)')
axs.set_ylabel(f'Acceleration {axis} (m/s^2)')
axs.legend()
axs.grid(True)

plt.tight_layout()
plt.show()

# Calculate and plot PTA (Peak Tibial Acceleration) over time
def plot_pta(aligned_tibia_acc, fs=120):
# Calculate time array
time = np.arange(len(aligned_tibia_acc)) / fs

# Compute PTA (norm of each acceleration vector at each time step)
pta = np.linalg.norm(aligned_tibia_acc, axis=1)

# Plot PTA over time
plt.figure(figsize=(10, 6))
plt.plot(time, pta, label='PTA (m/s^2)', color='b')
plt.xlabel('Time (s)')
plt.ylabel('PTA (m/s^2)')
plt.title('Peak Tibial Acceleration Over Time')
plt.grid(True)
plt.legend()
plt.show()

return pta

# Main execution
static_calibration_file = r"\test 16-10\standing still upright.csv"
functional_calibration_file = r"\test 16-10\walking slow calibration.csv"
aligned_acc, _, transformation_matrix = align_tibia_data(static_calibration_file, functional_calibration_file)

test_data_file = r"\test 16-10\test data.csv"
acc_data, _, quaternions = get_data(test_data_file)
global_acc = np.array([rotate_vector(q, acc) for q, acc in zip(quaternions, acc_data)]) # Apply quaternion rotation
global_filtered_acc = filtering(120, 15, global_acc)

# Align the test data to the tibial reference frame
aligned_tibia_acc = np.dot(global_filtered_acc, transformation_matrix.T) - [0, 0, 9.81]

# Plot tibial acceleration over time
tibial_acceleration = plot_pta(aligned_tibia_acc)


Подробнее здесь: https://stackoverflow.com/questions/790 ... -algorithm