| 209 | |
| 210 | |
| 211 | def interpolate(y, dt, target_dt, gt=None): |
| 212 | x = np.arange(y.shape[0]) * dt |
| 213 | x_target = np.arange(int(y.shape[0] * dt / target_dt)) * target_dt |
| 214 | cs = CubicSpline(x, y) |
| 215 | vel = cs.derivative()(x_target) |
| 216 | vel_smooth = gaussian_filter1d(vel, sigma=2 * dt / target_dt, axis=0) |
| 217 | if gt is not None: |
| 218 | plt.plot(x, gt) |
| 219 | plt.plot(x_target, vel, 'x') |
| 220 | plt.plot(x_target, vel_smooth, '--') |
| 221 | plt.show() |
| 222 | return cs(x_target), vel_smooth |
| 223 | |
| 224 | |
| 225 | def _compute_angular_velocity(r, time_delta: float): |