|
1 | 1 | import numpy as np |
2 | 2 | import matplotlib.pyplot as plt |
| 3 | +from scipy.interpolate import interp1d |
3 | 4 |
|
4 | 5 | import ibllib.dsp.fourier as ft |
5 | 6 |
|
@@ -77,6 +78,141 @@ def rolling_window(x, window_len=11, window='blackman'): |
77 | 78 | return y[round((window_len / 2 - 1)):round(-(window_len / 2))] |
78 | 79 |
|
79 | 80 |
|
| 81 | +def non_uniform_savgol(x, y, window, polynom): |
| 82 | + """Applies a Savitzky-Golay filter to y with non-uniform spacing as defined in x. |
| 83 | + This is based on |
| 84 | + https://dsp.stackexchange.com/questions/1676/savitzky-golay-smoothing-filter-for-not-equally-spaced-data |
| 85 | + The borders are interpolated like scipy.signal.savgol_filter would do |
| 86 | + https://dsp.stackexchange.com/a/64313 |
| 87 | + Parameters |
| 88 | + ---------- |
| 89 | + x : array_like |
| 90 | + List of floats representing the x values of the data |
| 91 | + y : array_like |
| 92 | + List of floats representing the y values. Must have same length as x |
| 93 | + window : int (odd) |
| 94 | + Window length of datapoints. Must be odd and smaller than x |
| 95 | + polynom : int |
| 96 | + The order of polynom used. Must be smaller than the window size |
| 97 | + Returns |
| 98 | + ------- |
| 99 | + np.array |
| 100 | + The smoothed y values |
| 101 | + """ |
| 102 | + |
| 103 | + if len(x) != len(y): |
| 104 | + raise ValueError('"x" and "y" must be of the same size') |
| 105 | + if len(x) < window: |
| 106 | + raise ValueError('The data size must be larger than the window size') |
| 107 | + if type(window) is not int: |
| 108 | + raise TypeError('"window" must be an integer') |
| 109 | + if window % 2 == 0: |
| 110 | + raise ValueError('The "window" must be an odd integer') |
| 111 | + if type(polynom) is not int: |
| 112 | + raise TypeError('"polynom" must be an integer') |
| 113 | + if polynom >= window: |
| 114 | + raise ValueError('"polynom" must be less than "window"') |
| 115 | + |
| 116 | + half_window = window // 2 |
| 117 | + polynom += 1 |
| 118 | + |
| 119 | + # Initialize variables |
| 120 | + A = np.empty((window, polynom)) # Matrix |
| 121 | + tA = np.empty((polynom, window)) # Transposed matrix |
| 122 | + t = np.empty(window) # Local x variables |
| 123 | + y_smoothed = np.full(len(y), np.nan) |
| 124 | + |
| 125 | + # Start smoothing |
| 126 | + for i in range(half_window, len(x) - half_window, 1): |
| 127 | + # Center a window of x values on x[i] |
| 128 | + for j in range(0, window, 1): |
| 129 | + t[j] = x[i + j - half_window] - x[i] |
| 130 | + |
| 131 | + # Create the initial matrix A and its transposed form tA |
| 132 | + for j in range(0, window, 1): |
| 133 | + r = 1.0 |
| 134 | + for k in range(0, polynom, 1): |
| 135 | + A[j, k] = r |
| 136 | + tA[k, j] = r |
| 137 | + r *= t[j] |
| 138 | + |
| 139 | + # Multiply the two matrices |
| 140 | + tAA = np.matmul(tA, A) |
| 141 | + # Invert the product of the matrices |
| 142 | + tAA = np.linalg.inv(tAA) |
| 143 | + # Calculate the pseudoinverse of the design matrix |
| 144 | + coeffs = np.matmul(tAA, tA) |
| 145 | + # Calculate c0 which is also the y value for y[i] |
| 146 | + y_smoothed[i] = 0 |
| 147 | + for j in range(0, window, 1): |
| 148 | + y_smoothed[i] += coeffs[0, j] * y[i + j - half_window] |
| 149 | + |
| 150 | + # If at the end or beginning, store all coefficients for the polynom |
| 151 | + if i == half_window: |
| 152 | + first_coeffs = np.zeros(polynom) |
| 153 | + for j in range(0, window, 1): |
| 154 | + for k in range(polynom): |
| 155 | + first_coeffs[k] += coeffs[k, j] * y[j] |
| 156 | + elif i == len(x) - half_window - 1: |
| 157 | + last_coeffs = np.zeros(polynom) |
| 158 | + for j in range(0, window, 1): |
| 159 | + for k in range(polynom): |
| 160 | + last_coeffs[k] += coeffs[k, j] * y[len(y) - window + j] |
| 161 | + |
| 162 | + # Interpolate the result at the left border |
| 163 | + for i in range(0, half_window, 1): |
| 164 | + y_smoothed[i] = 0 |
| 165 | + x_i = 1 |
| 166 | + for j in range(0, polynom, 1): |
| 167 | + y_smoothed[i] += first_coeffs[j] * x_i |
| 168 | + x_i *= x[i] - x[half_window] |
| 169 | + |
| 170 | + # Interpolate the result at the right border |
| 171 | + for i in range(len(x) - half_window, len(x), 1): |
| 172 | + y_smoothed[i] = 0 |
| 173 | + x_i = 1 |
| 174 | + for j in range(0, polynom, 1): |
| 175 | + y_smoothed[i] += last_coeffs[j] * x_i |
| 176 | + x_i *= x[i] - x[-half_window - 1] |
| 177 | + |
| 178 | + return y_smoothed |
| 179 | + |
| 180 | + |
| 181 | +def smooth_interpolate_savgol(signal, window=31, order=3, interp_kind='cubic'): |
| 182 | + """Run savitzy-golay filter on signal, interpolate through nan points. |
| 183 | +
|
| 184 | + Parameters |
| 185 | + ---------- |
| 186 | + signal : np.ndarray |
| 187 | + original noisy signal of shape (t,), may contain nans |
| 188 | + window : int |
| 189 | + window of polynomial fit for savitzy-golay filter |
| 190 | + order : int |
| 191 | + order of polynomial for savitzy-golay filter |
| 192 | + interp_kind : str |
| 193 | + type of interpolation for nans, e.g. 'linear', 'quadratic', 'cubic' |
| 194 | + Returns |
| 195 | + ------- |
| 196 | + np.array |
| 197 | + smoothed, interpolated signal for each time point, shape (t,) |
| 198 | + """ |
| 199 | + |
| 200 | + signal_noisy_w_nans = np.copy(signal) |
| 201 | + timestamps = np.arange(signal_noisy_w_nans.shape[0]) |
| 202 | + good_idxs = np.where(~np.isnan(signal_noisy_w_nans))[0] |
| 203 | + # perform savitzky-golay filtering on non-nan points |
| 204 | + signal_smooth_nonans = non_uniform_savgol( |
| 205 | + timestamps[good_idxs], signal_noisy_w_nans[good_idxs], window=window, polynom=order) |
| 206 | + signal_smooth_w_nans = np.copy(signal_noisy_w_nans) |
| 207 | + signal_smooth_w_nans[good_idxs] = signal_smooth_nonans |
| 208 | + # interpolate nan points |
| 209 | + interpolater = interp1d( |
| 210 | + timestamps[good_idxs], signal_smooth_nonans, kind=interp_kind, fill_value='extrapolate') |
| 211 | + signal = interpolater(timestamps) |
| 212 | + |
| 213 | + return signal |
| 214 | + |
| 215 | + |
80 | 216 | def smooth_demo(): |
81 | 217 |
|
82 | 218 | t = np.linspace(-4, 4, 100) |
|
0 commit comments