Skip to content

Commit

Permalink
Update _piece_wise_linear_dedrifted_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 1e0b49d commit f14dfdb
Showing 1 changed file with 2 additions and 2 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -181,11 +181,11 @@ def estimate(self, data: SingleSensorData, *, sampling_rate_hz: float, **_) -> S
# shift zupts to fit to the padded acc data!
zupts_padded = self.zupts_ + 1

velocity = cumtrapz(acc_data_padded, axis=0, initial=0) / self.sampling_rate_hz
velocity = cumulative_trapezoid(acc_data_padded, axis=0, initial=0) / self.sampling_rate_hz
drift_model = self._estimate_piece_wise_linear_drift_model(velocity, zupts_padded)
velocity -= drift_model

position = cumtrapz(velocity, axis=0, initial=0) / self.sampling_rate_hz
position = cumulative_trapezoid(velocity, axis=0, initial=0) / self.sampling_rate_hz

if self.level_assumption is True:
position[:, -1] -= self._estimate_piece_wise_linear_drift_model(position[:, -1], zupts_padded)
Expand Down

0 comments on commit f14dfdb

Please sign in to comment.