From 101a8c8d2de7fd6748710befbba92b5217cce9f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arne=20K=C3=BCderle?= Date: Thu, 20 Feb 2025 16:58:07 +0100 Subject: [PATCH] Small fix for madgwick --- .../orientation_methods/_madgwick.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gaitmap/trajectory_reconstruction/orientation_methods/_madgwick.py b/gaitmap/trajectory_reconstruction/orientation_methods/_madgwick.py index a9357a62..391a6205 100644 --- a/gaitmap/trajectory_reconstruction/orientation_methods/_madgwick.py +++ b/gaitmap/trajectory_reconstruction/orientation_methods/_madgwick.py @@ -206,9 +206,9 @@ def _madgwick_update(gyro, acc, initial_orientation, sampling_rate_hz, beta): # Switch the component order back s = np.array([s1, s2, s3, s0]) - mag_s = np.sqrt(np.sum(s**2)) - if mag_s != 0.0: - s /= np.sqrt(np.sum(s**2)) + norm_s = np.sqrt(np.sum(s**2)) + if norm_s != 0.0: + s /= norm_s # Apply feedback step qdot -= beta * s @@ -298,9 +298,9 @@ def _madgwick_update_mag(gyro, acc, mag, initial_orientation, sampling_rate_hz, # Switch the component order back s = np.array([s1, s2, s3, s0]) - mag_s = np.sqrt(np.sum(s**2)) - if mag_s != 0.0: - s /= np.sqrt(np.sum(s**2)) + norm_s = np.sqrt(np.sum(s**2)) + if norm_s != 0.0: + s /= norm_s # Apply feedback step qdot -= beta * s