src.trajectory_estimation package

src.trajectory_estimation.filter module

This module contains various implementations of trajectory estimators and necessary helper functions.

src.trajectory_estimation.filter.acc_to_euler(acc)

Calculate the 3D orientation vector (rad) from 3D acceleration measurement, assuming that there is no translational movement. Note: the z-coordinate is always zero since horizontal rotation cannot be tracked by an accelerometer.

Parameters

acc (np.array) – acceleration measurement

Returns

oriantation vector

Return type

np.array

src.trajectory_estimation.filter.complementary_filter(time, acc, gyro, a=0.1)

Complementary filter to fusion accelerometer and gyroscope measurements. Calculate the deduced orientation time series from accelerometer and gyroscope Note: a quantifies the “trust” for the accelerometer or the “misstrust” for the gyroscope.

Parameters

  • time (np.array) – timestamps

  • acc (np.array) – 3D acceleration measurements

  • gyro (np.array) – 3D gyroscope measurements

  • a (float) – complementary factor

Returns

Orientation vector timeseries

Return type

np.array

src.trajectory_estimation.filter.error_state_kalman_filter(imu, imu_ic, zero_z=True, zero_xz=False, zero_xyz=False, stance_magnitude_threshold=0.5, stance_count_threshold=8, method='euler')

Error-state Kalman Filter inspired by https://doi.org/10.3390/s17040825.

Parameters

  • imu (IMU) – IMU object with measurement data

  • imu_ic (float) – timestamp of initial contact

  • zero_z (bool) – flag if height should be reset each stride

  • zero_xz (bool) – flag if height and sideward movement should be reset each stride (if true, zero_z is ignored)

  • zero_xyz (bool) flag if all position dimensions should be reset each stride (if true, zero_z and zero_xz are ignored) –

  • stance_magnitude_threshold (float) – gyroscope magnitude threshold for stance event_detection

  • stance_count_threshold (int) – grace period in samples before and after stance detection

  • method (str) – “euler” or “midpoint”, select used integration technique

Returns

DataFrame with trajectory data for each timestep (time, position, velocity, rotation, stance)

Return type

DataFrame

src.trajectory_estimation.filter.gyro_to_euler(orientation, gyro)

Applies a 3D gyroscope measurement in the local frame (deg/s) to a global 3D orientation vector (rad) and calculates the 3D rotation vector (rad/s) in the global coordinate frame.

Parameters

  • orientation (np.array) – initial orientation vector

  • gyro (np.array) – gyroscope signal vector

Returns

rotation vector

Return type

np.array

src.trajectory_estimation.filter.kalman_filter(time, acc, gyro)

Kalman filter implementation inspired by http://philsal.co.uk/projects/imu-attitude-estimation.

Parameters

  • time (np.array) – timestamps

  • acc (np.array) – 3D acceleration measurements

  • gyro (np.array) – 3D gyroscope measurements

Returns

3D position timeseries

Return type

list[list[float]]