Skip to content

Commit

Permalink
Update _forward_backwards_integration.py
Browse files Browse the repository at this point in the history
  • Loading branch information
Aksei authored and AKuederle committed Jan 29, 2025
1 parent f14dfdb commit 4016e0b
Showing 1 changed file with 4 additions and 4 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -149,11 +149,11 @@ def estimate(self, data: SingleSensorData, *, sampling_rate_hz: float, **_) -> S
# Add an implicit 0 to the beginning of the acc data
padded_acc = np.pad(acc_data, pad_width=((1, 0), (0, 0)), constant_values=0)
velocity = self._forward_backward_integration(padded_acc)
position_xy = cumtrapz(velocity[:, :2], axis=0, initial=0) / self.sampling_rate_hz
position_xy = cumulative_trapezoid(velocity[:, :2], axis=0, initial=0) / self.sampling_rate_hz
if self.level_assumption is True:
position_z = self._forward_backward_integration(velocity[:, [2]])
else:
position_z = cumtrapz(velocity[:, [2]], axis=0, initial=0) / self.sampling_rate_hz
position_z = cumulative_trapezoid(velocity[:, [2]], axis=0, initial=0) / self.sampling_rate_hz
position = np.hstack((position_xy, position_z))

self.velocity_ = pd.DataFrame(velocity, columns=GF_VEL)
Expand All @@ -171,9 +171,9 @@ def _sigmoid_weight_function(self, n_samples: int) -> np.ndarray:

def _forward_backward_integration(self, data: np.ndarray) -> np.ndarray:
# TODO: different steepness and turning point for velocity and position?
integral_forward = cumtrapz(data, axis=0, initial=0) / self.sampling_rate_hz
integral_forward = cumulative_trapezoid(data, axis=0, initial=0) / self.sampling_rate_hz
# for backward integration, we flip the signal and inverse the time by using a negative sampling rate.
integral_backward = cumtrapz(data[::-1], axis=0, initial=0) / -self.sampling_rate_hz
integral_backward = cumulative_trapezoid(data[::-1], axis=0, initial=0) / -self.sampling_rate_hz
weights = self._sigmoid_weight_function(integral_forward.shape[0])
combined = (integral_forward.T * (1 - weights) + integral_backward[::-1].T * weights).T
return combined

0 comments on commit 4016e0b

Please sign in to comment.