def find_delay_gain(y: np.array,
u: np.array,
fs: float,
name: str = '',
plot: bool = False):
# type: (np.array, np.array, float, str, bool) -> Dict
"""Find delay and gain to match y to u"""
# pylint: disable=too-many-locals
costs = []
delays = []
gains = []
# noinspection PyTypeChecker
assert np.all(y.index == u.index)
# noinspection PyShadowingNames
def f_cost(y, u, i, k):
"""Signal sum sq error cost"""
e = y - k * u.shift(i).bfill()
return e.dot(e)
max_delay = 0.3
n_delay = int(max_delay * fs)
for i in range(n_delay):
# pylint: disable=cell-var-from-loop
# noinspection PyTypeChecker
res = scipy.optimize.minimize(
lambda k_: f_cost(y, u, i, k_),
x0=50,
method='SLSQP',
bounds=[(0, 1000)],
)
k = res['x'][0]
# if it failed for anything but a line search, print result
if res['success'] != True and res['status'] != 8:
print(res)
costs.append(f_cost(y, u, i, k))
gains.append(k)
delays.append(i)
# noinspection PyTypeChecker
sample_delay = delays[np.argmin(costs)]
# noinspection PyTypeChecker
gain = gains[np.argmin(costs)]
if plot:
plt.figure(figsize=(10, 5))
costs = np.array(costs)
delays = np.array(delays)
plt.plot(1e3 * delays / fs, costs, '-')
plt.xlabel('delay [msec]')
plt.ylabel('sum sq error')
plt.title('{:s} delay'.format(name))
plt.grid()
plt.figure(figsize=(10, 5))
(gain * u.shift(sample_delay)).plot(
label='{:s} acc cmd filtered'.format(name), alpha=0.5)
y.plot(label='{:s} acc filtered'.format(name), alpha=0.5)
plt.legend()
plt.grid()
plt.ylabel('$rad / s^2$')
e = y - gain * u.shift(sample_delay).bfill()
np.var(e)
fit = 1 - np.var(e) / np.var(y)
return {
'sample_delay': sample_delay,
'delay': sample_delay / fs,
'gain': gain,
'fit': fit,
'f_s': fs,
}
