def test_odd_ext(self): a = np.array([[1, 2, 3, 4, 5], [9, 8, 7, 6, 5]]) odd = odd_ext(a, 2, axis=1) expected = np.array([[-1, 0, 1, 2, 3, 4, 5, 6, 7], [11, 10, 9, 8, 7, 6, 5, 4, 3]]) assert_array_equal(odd, expected) odd = odd_ext(a, 1, axis=0) expected = np.array([[-7, -4, -1, 2, 5], [1, 2, 3, 4, 5], [9, 8, 7, 6, 5], [17, 14, 11, 8, 5]]) assert_array_equal(odd, expected) assert_raises(ValueError, odd_ext, a, 2, axis=0) assert_raises(ValueError, odd_ext, a, 5, axis=1)
def _validate_pad(padtype, padlen, x, axis, ntaps): """Helper to validate padding for filtfilt""" if padtype not in ['even', 'odd', 'constant', None]: raise ValueError( ("Unknown value '%s' given to padtype. padtype " "must be 'even', 'odd', 'constant', or None.") % padtype) if padtype is None: padlen = 0 if padlen is None: # Original padding; preserved for backwards compatibility. edge = ntaps * 3 else: edge = padlen # x's 'axis' dimension must be bigger than edge. if x.shape[axis] <= edge: raise ValueError("The length of the input vector x must be at least " "padlen, which is %d." % edge) if padtype is not None and edge > 0: # Make an extension of length `edge` at each # end of the input array. if padtype == 'even': ext = even_ext(x, edge, axis=axis) elif padtype == 'odd': ext = odd_ext(x, edge, axis=axis) else: ext = const_ext(x, edge, axis=axis) else: ext = x return edge, ext
def _sosfiltfilt(sos, x, axis=-1, padtype='odd', padlen=None, method='pad', irlen=None): """Filtfilt version using Second Order sections. Code is taken from scipy.signal.filtfilt and adapted to make it work with SOS. Note that broadcasting does not work. """ from scipy.signal import sosfilt_zi from scipy.signal._arraytools import odd_ext, axis_slice, axis_reverse x = np.asarray(x) if padlen is None: edge = 0 else: edge = padlen # x's 'axis' dimension must be bigger than edge. if x.shape[axis] <= edge: raise ValueError("The length of the input vector x must be at least " "padlen, which is %d." % edge) if padtype is not None and edge > 0: # Make an extension of length `edge` at each # end of the input array. if padtype == 'even': ext = even_ext(x, edge, axis=axis) elif padtype == 'odd': ext = odd_ext(x, edge, axis=axis) else: ext = const_ext(x, edge, axis=axis) else: ext = x # Get the steady state of the filter's step response. zi = sosfilt_zi(sos) # Reshape zi and create x0 so that zi*x0 broadcasts # to the correct value for the 'zi' keyword argument # to lfilter. #zi_shape = [1] * x.ndim #zi_shape[axis] = zi.size #zi = np.reshape(zi, zi_shape) x0 = axis_slice(ext, stop=1, axis=axis) # Forward filter. (y, zf) = sosfilt(sos, ext, axis=axis, zi=zi * x0) # Backward filter. # Create y0 so zi*y0 broadcasts appropriately. y0 = axis_slice(y, start=-1, axis=axis) (y, zf) = sosfilt(sos, axis_reverse(y, axis=axis), axis=axis, zi=zi * y0) # Reverse y. y = axis_reverse(y, axis=axis) if edge > 0: # Slice the actual signal from the extended signal. y = axis_slice(y, start=edge, stop=-edge, axis=axis) return y
def butter_bandpass_sosfiltfilt(self, lowcut=None, highcut=None, accel_axis="z", order=5, axis=-1, padtype="odd", padlen=None, method='pad', irlen=None): '''Filtfilt version using Second Order sections. Code is taken from scipy.signal.filtfilt and adapted to make it work with sos. Note that broadcasting does not work''' data = np.asarray(getattr(self.df, accel_axis[:])) sos = self.butter_bandpass(lowcut, highcut, order) if padlen is None: edge = 0 else: edge = padlen if data.shape[axis] <= edge: raise ValueError( "The length of the input vector x must be at least padlen, which is %d." % edge) if padtype is not None and edge > 0: if padtype == "even": ext = even_ext(data, edge, axis=axis) elif padtype == "odd": ext = odd_ext(data, edge, axis=axis) else: ext = const_ext(data, edge, axis=axis) else: ext = data # Get the steady state of the filter's first step resopnse zi = sosfilt_zi(sos) # Reshape zi and create x0 so that zi*x0 broadcasts to the correct value for the zi keyword argument to lfilter x0 = axis_slice(ext, stop=1, axis=axis) # Forward filter (y, zf) = sosfilt(sos, ext, axis=axis, zi=zi * x0) y0 = axis_slice(y, start=-1, axis=axis) # Backward filter (y, zf) = sosfilt(sos, axis_reverse(y, axis=axis), axis=axis, zi=zi * y0) y = axis_reverse(y, axis=axis) if edge > 0: y = axis_slice(y, start=edge, stop=-edge, axis=axis) return y
def sosfiltfilt(sos, x, axis=-1, padtype="odd", padlen=0): x = asarray(x) # `method` is "pad" if padtype not in ["even", "odd", "constant", None]: raise ValueError( ("Unknown value '%s' given to padtype. padtype " "must be 'even', 'odd', 'constant', or None.") % padtype ) if padtype is None: padlen = 0 if padlen is None: edge = sos.shape[0] * 6 else: edge = padlen # x's 'axis' dimension must be bigger than edge. if x.shape[axis] <= edge: raise ValueError("The length of the input vector x must be at least " "padlen, which is %d." % edge) if padtype is not None and edge > 0: # Make an extension of length `edge` at each # end of the input array. if padtype == "even": ext = even_ext(x, edge, axis=axis) elif padtype == "odd": ext = odd_ext(x, edge, axis=axis) else: ext = const_ext(x, edge, axis=axis) else: ext = x # Get the steady state of the filter's step response. zi = sosfilt_zi(sos) # Reshape zi and create x0 so that zi*x0 broadcasts # to the correct value for the 'zi' keyword argument # to lfilter. zi_shape = [1] * x.ndim zi_shape[axis] = zi.size zi = reshape(zi, zi_shape) x0 = axis_slice(ext, stop=1, axis=axis) zix0 = reshape(zi * x0, (sos.shape[0], 2)) # Forward filter (y, zf) = sosfilt(sos, ext, axis=axis, zi=zix0) # Backward filter # Create y0 so zi*y0 broadcasts appropriately. y0 = axis_slice(y, start=-1, axis=axis) ziy0 = reshape(zi * y0, (sos.shape[0], 2)) (y, zf) = sosfilt(sos, axis_reverse(y, axis=axis), axis=axis, zi=ziy0) # Reverse y y = axis_reverse(y, axis=axis) if edge > 0: # Slice the actual signal from the extended signal. y = axis_slice(y, start=edge, stop=-edge, axis=axis) return y
def cogve(COP, freq, mass, height, show=False, ax=None): """COGv estimation using COP data based on the inverted pendulum model. This function estimates the center of gravity vertical projection (COGv) displacement from the center of pressure (COP) displacement at the anterior-posterior direction during quiet upright standing. COP and COGv displacements are measurements useful to quantify the postural sway of a person while standing. The COGv displacement is estimated by low-pass filtering the COP displacement in the frequency domain according to the person's moment of rotational inertia as a single inverted pendulum [1]_. Parameters ---------- COP : 1D array_like center of pressure data [cm] freq : float sampling frequency of the COP data mass : float body mass of the subject [kg] height : float height of the subject [cm] show : bool, optional (default = False) True (1) plots data and results in a matplotlib figure False (0) to not plot ax : matplotlib.axes.Axes instance, optional (default = None) Returns ------- COGv : 1D array center of gravity vertical projection data [cm] References ---------- .. [1] http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/IP_Model.ipynb Examples -------- >>> from cogve import cogve >>> y = np.cumsum(np.random.randn(3000))/50 >>> cogv = cogve(y, freq=100, mass=70, height=170, show=True) """ from scipy.signal._arraytools import odd_ext import scipy.fftpack COP = np.asarray(COP) height = height / 100 # cm to m g = 9.8 # gravity acceleration in m/s2 # height of the COG w.r.t. ankle (McGinnis, 2005; Winter, 2005) hcog = 0.56 * height - 0.039 * height # body moment of inertia around the ankle # (Breniere, 1996), (0.0572 for the ml direction) I = mass * 0.0533 * height**2 + mass * hcog**2 # Newton-Euler equation of motion for the inverted pendulum # COGv'' = w02*(COGv - COP) # where w02 is the squared pendulum natural frequency w02 = mass * g * hcog / I # add (pad) data and remove mean to avoid problems at the extremities COP = odd_ext(COP, n=freq) COPm = np.mean(COP) COP = COP - COPm # COGv is estimated by filtering the COP data in the frequency domain # using the transfer function for the inverted pendulum equation of motion N = COP.size COPfft = scipy.fftpack.fft(COP, n=N) / N # COP fft w = 2 * np.pi * scipy.fftpack.fftfreq(n=N, d=1 / freq) # angular frequency # transfer function TF = w02 / (w02 + w**2) COGv = np.real(scipy.fftpack.ifft(TF * COPfft) * N) COGv = COGv[0:N] # get back the mean and pad off data COP, COGv = COP + COPm, COGv + COPm COP, COGv = COP[freq:-freq], COGv[freq:-freq] if show: _plot(COP, COGv, freq, ax) return COGv
def sosfiltfilt(sos, x, axis=-1, padtype='odd', padlen=0): x = asarray(x) # `method` is "pad" if padtype not in ['even', 'odd', 'constant', None]: raise ValueError(("Unknown value '%s' given to padtype. padtype " "must be 'even', 'odd', 'constant', or None.") % padtype) if padtype is None: padlen = 0 if padlen is None: edge = sos.shape[0] * 6 else: edge = padlen # x's 'axis' dimension must be bigger than edge. if x.shape[axis] <= edge: raise ValueError("The length of the input vector x must be at least " "padlen, which is %d." % edge) if padtype is not None and edge > 0: # Make an extension of length `edge` at each # end of the input array. if padtype == 'even': ext = even_ext(x, edge, axis=axis) elif padtype == 'odd': ext = odd_ext(x, edge, axis=axis) else: ext = const_ext(x, edge, axis=axis) else: ext = x # Get the steady state of the filter's step response. zi = sosfilt_zi(sos) # Reshape zi and create x0 so that zi*x0 broadcasts # to the correct value for the 'zi' keyword argument # to lfilter. zi_shape = [1] * x.ndim zi_shape[axis] = zi.size zi = reshape(zi, zi_shape) x0 = axis_slice(ext, stop=1, axis=axis) zix0 = reshape(zi * x0, (sos.shape[0], 2)) # Forward filter (y, zf) = sosfilt(sos, ext, axis=axis, zi=zix0) # Backward filter # Create y0 so zi*y0 broadcasts appropriately. y0 = axis_slice(y, start=-1, axis=axis) ziy0 = reshape(zi * y0, (sos.shape[0], 2)) (y, zf) = sosfilt(sos, axis_reverse(y, axis=axis), axis=axis, zi=ziy0) # Reverse y y = axis_reverse(y, axis=axis) if edge > 0: # Slice the actual signal from the extended signal. y = axis_slice(y, start=edge, stop=-edge, axis=axis) return y
def filtfilt(b, a, x, axis=-1, padtype='odd', padlen=None): """ A forward-backward filter. This function applies a linear filter twice, once forward and once backwards. The combined filter has linear phase. Before applying the filter, the function can pad the data along the given axis in one of three ways: odd, even or constant. The odd and even extensions have the corresponding symmetry about the end point of the data. The constant extension extends the data with the values at end points. On both the forward and backwards passes, the initial condition of the filter is found by using `lfilter_zi` and scaling it by the end point of the extended data. Parameters ---------- b : (N,) array_like The numerator coefficient vector of the filter. a : (N,) array_like The denominator coefficient vector of the filter. If a[0] is not 1, then both a and b are normalized by a[0]. x : array_like The array of data to be filtered. axis : int, optional The axis of `x` to which the filter is applied. Default is -1. padtype : str or None, optional Must be 'odd', 'even', 'constant', or None. This determines the type of extension to use for the padded signal to which the filter is applied. If `padtype` is None, no padding is used. The default is 'odd'. padlen : int or None, optional The number of elements by which to extend `x` at both ends of `axis` before applying the filter. This value must be less than `x.shape[axis]-1`. `padlen=0` implies no padding. The default value is 3*max(len(a),len(b)). Returns ------- y : ndarray The filtered output, an array of type numpy.float64 with the same shape as `x`. See Also -------- lfilter_zi, lfilter Examples -------- First we create a one second signal that is the sum of two pure sine waves, with frequencies 5 Hz and 250 Hz, sampled at 2000 Hz. >>> t = np.linspace(0, 1.0, 2001) >>> xlow = np.sin(2 * np.pi * 5 * t) >>> xhigh = np.sin(2 * np.pi * 250 * t) >>> x = xlow + xhigh Now create a lowpass Butterworth filter with a cutoff of 0.125 times the Nyquist rate, or 125 Hz, and apply it to x with filtfilt. The result should be approximately xlow, with no phase shift. >>> from scipy import signal >>> b, a = signal.butter(8, 0.125) >>> y = filtfilt(b, a, x, padlen=150) >>> print('%.5g' % np.abs(y - xlow).max()) 9.1086e-06 We get a fairly clean result for this artificial example because the odd extension is exact, and with the moderately long padding, the filter's transients have dissipated by the time the actual data is reached. In general, transient effects at the edges are unavoidable. """ if padtype not in ['even', 'odd', 'constant', None]: raise ValueError(("Unknown value '%s' given to padtype. padtype must " "be 'even', 'odd', 'constant', or None.") % padtype) b = np.asarray(b) a = np.asarray(a) x = np.asarray(x) ntaps = max(len(a), len(b)) if padtype is None: padlen = 0 if padlen is None: # Original padding; preserved for backwards compatibility. edge = ntaps * 3 else: edge = padlen # x's 'axis' dimension must be bigger than edge. if x.shape[axis] <= edge: raise ValueError("The length of the input vector x must be at least " "padlen, which is %d." % edge) if padtype is not None and edge > 0: # Make an extension of length `edge` at each # end of the input array. if padtype == 'even': ext = even_ext(x, edge, axis=axis) elif padtype == 'odd': ext = odd_ext(x, edge, axis=axis) else: ext = const_ext(x, edge, axis=axis) else: ext = x # Get the steady state of the filter's step response. zi = lfilter_zi(b, a) # Reshape zi and create x0 so that zi*x0 broadcasts # to the correct value for the 'zi' keyword argument # to lfilter. zi_shape = [1] * x.ndim zi_shape[axis] = zi.size zi = np.reshape(zi, zi_shape) x0 = axis_slice(ext, stop=1, axis=axis) # Forward filter. (y, zf) = lfilter(b, a, ext, axis=axis, zi=zi * x0) # Backward filter. # Create y0 so zi*y0 broadcasts appropriately. y0 = axis_slice(y, start=-1, axis=axis) (y, zf) = lfilter(b, a, axis_reverse(y, axis=axis), axis=axis, zi=zi * y0) # Reverse y. y = axis_reverse(y, axis=axis) if edge > 0: # Slice the actual signal from the extended signal. y = axis_slice(y, start=edge, stop=-edge, axis=axis) return y
def filtfilt(b, a, x, axis=-1, padtype='odd', padlen=None): """ A forward-backward filter. This function applies a linear filter twice, once forward and once backwards. The combined filter has linear phase. Before applying the filter, the function can pad the data along the given axis in one of three ways: odd, even or constant. The odd and even extensions have the corresponding symmetry about the end point of the data. The constant extension extends the data with the values at end points. On both the forward and backwards passes, the initial condition of the filter is found by using `lfilter_zi` and scaling it by the end point of the extended data. Parameters ---------- b : (N,) array_like The numerator coefficient vector of the filter. a : (N,) array_like The denominator coefficient vector of the filter. If a[0] is not 1, then both a and b are normalized by a[0]. x : array_like The array of data to be filtered. axis : int, optional The axis of `x` to which the filter is applied. Default is -1. padtype : str or None, optional Must be 'odd', 'even', 'constant', or None. This determines the type of extension to use for the padded signal to which the filter is applied. If `padtype` is None, no padding is used. The default is 'odd'. padlen : int or None, optional The number of elements by which to extend `x` at both ends of `axis` before applying the filter. This value must be less than `x.shape[axis]-1`. `padlen=0` implies no padding. The default value is 3*max(len(a),len(b)). Returns ------- y : ndarray The filtered output, an array of type numpy.float64 with the same shape as `x`. See Also -------- lfilter_zi, lfilter Examples -------- First we create a one second signal that is the sum of two pure sine waves, with frequencies 5 Hz and 250 Hz, sampled at 2000 Hz. >>> t = np.linspace(0, 1.0, 2001) >>> xlow = np.sin(2 * np.pi * 5 * t) >>> xhigh = np.sin(2 * np.pi * 250 * t) >>> x = xlow + xhigh Now create a lowpass Butterworth filter with a cutoff of 0.125 times the Nyquist rate, or 125 Hz, and apply it to x with filtfilt. The result should be approximately xlow, with no phase shift. >>> from scipy import signal >>> b, a = signal.butter(8, 0.125) >>> y = filtfilt(b, a, x, padlen=150) >>> print('%.5g' % np.abs(y - xlow).max()) 9.1086e-06 We get a fairly clean result for this artificial example because the odd extension is exact, and with the moderately long padding, the filter's transients have dissipated by the time the actual data is reached. In general, transient effects at the edges are unavoidable. """ if padtype not in ['even', 'odd', 'constant', None]: raise ValueError( ("Unknown value '%s' given to padtype. padtype must " "be 'even', 'odd', 'constant', or None.") % padtype) b = np.asarray(b) a = np.asarray(a) x = np.asarray(x) ntaps = max(len(a), len(b)) if padtype is None: padlen = 0 if padlen is None: # Original padding; preserved for backwards compatibility. edge = ntaps * 3 else: edge = padlen # x's 'axis' dimension must be bigger than edge. if x.shape[axis] <= edge: raise ValueError( "The length of the input vector x must be at least " "padlen, which is %d." % edge) if padtype is not None and edge > 0: # Make an extension of length `edge` at each # end of the input array. if padtype == 'even': ext = even_ext(x, edge, axis=axis) elif padtype == 'odd': ext = odd_ext(x, edge, axis=axis) else: ext = const_ext(x, edge, axis=axis) else: ext = x # Get the steady state of the filter's step response. zi = lfilter_zi(b, a) # Reshape zi and create x0 so that zi*x0 broadcasts # to the correct value for the 'zi' keyword argument # to lfilter. zi_shape = [1] * x.ndim zi_shape[axis] = zi.size zi = np.reshape(zi, zi_shape) x0 = axis_slice(ext, stop=1, axis=axis) # Forward filter. (y, zf) = lfilter(b, a, ext, axis=axis, zi=zi * x0) # Backward filter. # Create y0 so zi*y0 broadcasts appropriately. y0 = axis_slice(y, start=-1, axis=axis) (y, zf) = lfilter(b, a, axis_reverse(y, axis=axis), axis=axis, zi=zi * y0) # Reverse y. y = axis_reverse(y, axis=axis) if edge > 0: # Slice the actual signal from the extended signal. y = axis_slice(y, start=edge, stop=-edge, axis=axis) return y
def filt_FFTWEAVE(b, x, padtype='odd', padlen=None): """A forward-backward filter. This function applies a linear filter twice, once forward and once backwards. The combined filter has linear phase. Before applying the filter, the function can pad the data along the given axis in one of three ways: odd, even or constant. The odd and even extensions have the corresponding symmetry about the end point of the data. The constant extension extends the data with the values at end points. On both the forward and backwards passes, the initial condition of the filter is found by using lfilter_zi and scaling it by the end point of the extended data. Parameters ---------- b : array_like, 1-D The numerator coefficient vector of the filter. a : array_like, 1-D The denominator coefficient vector of the filter. If a[0] is not 1, then both a and b are normalized by a[0]. x : array_like The array of data to be filtered. padtype : str or None, optional Must be 'odd', 'even', 'constant', or None. This determines the type of extension to use for the padded signal to which the filter is applied. If `padtype` is None, no padding is used. The default is 'odd'. padlen : int or None, optional The number of elements by which to extend `x` at both ends of `axis` before applying the filter. This value must be less than `x.shape[axis]-1`. `padlen=0` implies no padding. The default value is 3*max(len(a),len(b)). Returns ------- y : ndarray The filtered output, an array of type numpy.float64 with the same shape as `x`. See Also -------- lfilter_zi lfilter Examples -------- First we create a one second signal that is the sum of two pure sine waves, with frequencies 5 Hz and 250 Hz, sampled at 2000 Hz. >>> t = np.linspace(0, 1.0, 2001) >>> xlow = np.sin(2 * np.pi * 5 * t) >>> xhigh = np.sin(2 * np.pi * 250 * t) >>> x = xlow + xhigh Now create a lowpass Butterworth filter with a cutoff of 0.125 times the Nyquist rate, or 125 Hz, and apply it to x with filtfilt. The result should be approximately xlow, with no phase shift. >>> from scipy.signal import butter >>> b, a = butter(8, 0.125) >>> y = filtfilt(b, a, x, padlen=150) >>> np.abs(y - xlow).max() 9.1086182074789912e-06 We get a fairly clean result for this artificial example because the odd extension is exact, and with the moderately long padding, the filter's transients have dissipated by the time the actual data is reached. In general, transient effects at the edges are unavoidable. """ if padtype not in ['even', 'odd', 'constant', None]: raise ValueError(("Unknown value '%s' given to padtype. padtype must " "be 'even', 'odd', 'constant', or None.") % padtype) b = np.asarray(b) x = np.asarray(x) ntaps = len(b) if padtype is None: padlen = 0 if padlen is None: # Original padding; preserved for backwards compatibility. edge = ntaps * 3 else: edge = padlen # x's 'axis' dimension must be bigger than edge. #if x.shape[axis] <= edge: if len(x) <= edge: raise ValueError( "The length of the input vector x must be larger than " "padlen, which is %d." % edge) if padtype is not None and edge > 0: # Make an extension of length `edge` at each # end of the input array. if padtype == 'even': ext = even_ext(x, edge) #, axis=axis) elif padtype == 'odd': ext = odd_ext(x, edge) #, axis=axis) else: ext = const_ext(x, edge) #, axis=axis) else: ext = x # Get the steady state of the filter's step response. #zi = lfilter_zi(b, a) # Reshape zi and create x0 so that zi*x0 broadcasts # to the correct value for the 'zi' keyword argument # to lfilter. #zi_shape = [1] * x.ndim #zi_shape[axis] = zi.size #zi = np.reshape(zi, zi_shape) #x0 = axis_slice(ext, stop=1, axis=axis) # Forward filter. ext = convolve_cython(ext, b) """ if filtConf.useWeave: try : ext = convolve_weave(ext, b) except (CompileError, WindowsError) as e: ext = convolve_noWeave(ext, b) warnings.warn("Weave failed at runtime. Setting useWeave to False. This will results in slower filtering.", RuntimeWarning) filtConf.useWeave = False else: ext = convolve_noWeave(ext, b) """ if edge > 0: # Slice the actual signal from the extended signal. Reverse and return y. return ext[edge:-edge] else: # Reverse and return y. return ext
def cogve(COP, freq, mass, height, show=False, ax=None): """COGv estimation using COP data based on the inverted pendulum model. This function estimates the center of gravity vertical projection (COGv) displacement from the center of pressure (COP) displacement at the anterior-posterior direction during quiet upright standing. COP and COGv displacements are measurements useful to quantify the postural sway of a person while standing. The COGv displacement is estimated by low-pass filtering the COP displacement in the frequency domain according to the person's moment of rotational inertia as a single inverted pendulum [1]_. Parameters ---------- COP : 1D array_like center of pressure data [cm] freq : float sampling frequency of the COP data mass : float body mass of the subject [kg] height : float height of the subject [cm] show : bool, optional (default = False) True (1) plots data and results in a matplotlib figure False (0) to not plot ax : matplotlib.axes.Axes instance, optional (default = None) Returns ------- COGv : 1D array center of gravity vertical projection data [cm] References ---------- .. [1] http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/IP_Model.ipynb Examples -------- >>> from cogve import cogve >>> y = np.cumsum(np.random.randn(3000))/50 >>> cogv = cogve(y, freq=100, mass=70, height=170, show=True) """ from scipy.signal._arraytools import odd_ext import scipy.fftpack COP = np.asarray(COP) height = height / 100 # cm to m g = 9.8 # gravity acceleration in m/s2 # height of the COG w.r.t. ankle (McGinnis, 2005; Winter, 2005) hcog = 0.56 * height - 0.039 * height # body moment of inertia around the ankle # (Breniere, 1996), (0.0572 for the ml direction) I = mass * 0.0533 * height ** 2 + mass * hcog ** 2 # Newton-Euler equation of motion for the inverted pendulum # COGv'' = w02*(COGv - COP) # where w02 is the squared pendulum natural frequency w02 = mass * g * hcog / I # add (pad) data and remove mean to avoid problems at the extremities COP = odd_ext(COP, n=freq) COPm = np.mean(COP) COP = COP - COPm # COGv is estimated by filtering the COP data in the frequency domain # using the transfer function for the inverted pendulum equation of motion N = COP.size COPfft = scipy.fftpack.fft(COP, n=N) / N # COP fft w = 2 * np.pi * scipy.fftpack.fftfreq(n=N, d=1 / freq) # angular frequency # transfer function TF = w02 / (w02 + w ** 2) COGv = np.real(scipy.fftpack.ifft(TF * COPfft) * N) COGv = COGv[0: N] # get back the mean and pad off data COP, COGv = COP + COPm, COGv + COPm COP, COGv = COP[freq: -freq], COGv[freq: -freq] if show: _plot(COP, COGv, freq, ax) return COGv