import matplotlib.pyplot as plt
from scipy.optimize import curve_fit
from scipy.integrate import odeint
# ============================================================
# System Identification from Measured Data
# ============================================================
# Generate "measured" step response data from an unknown system,
# fit first-order and second-order models, validate on new input.
# --- The "real" system (unknown to the identifier) ---
# A second-order underdamped system: motor position control
# Parameters the identifier will try to recover:
TRUE_WN = 4.0 # Natural frequency (rad/s)
TRUE_ZETA = 0.3 # Damping ratio (underdamped)
TRUE_DELAY = 0.15 # Transport delay (seconds)
def true_system_step(t, step_amplitude=1.0):
True system step response (second-order with delay).
This simulates what you would measure in a real experiment.
for i, ti in enumerate(t):
wd = TRUE_WN * np.sqrt(1 - TRUE_ZETA**2)
env = np.exp(-TRUE_ZETA * TRUE_WN * td)
phase = np.arctan2(TRUE_ZETA, np.sqrt(1 - TRUE_ZETA**2))
y[i] = TRUE_K * step_amplitude * (
1 - env / np.sqrt(1 - TRUE_ZETA**2) * np.sin(wd * td + phase)
# --- Generate "measured" data ---
t_meas = np.arange(0, 8.0, dt)
# True response + measurement noise
y_true = true_system_step(t_meas, step_amplitude)
y_meas = y_true + noise_std * np.random.randn(len(t_meas))
print(" System Identification from Measured Step Response")
print(f" Measurement duration: {t_meas[-1]:.1f} s")
print(f" Sample rate: {1/dt:.0f} Hz")
print(f" Noise std: {noise_std}")
print(f" Step amplitude: {step_amplitude}")
print(" True (hidden) system parameters:")
print(f" K = {TRUE_K}, wn = {TRUE_WN} rad/s, "
f"zeta = {TRUE_ZETA}, delay = {TRUE_DELAY} s")
# ============================================================
# Model 1: First-order fit
# ============================================================
def first_order_step(t, K, tau):
"""First-order step response: y = K * (1 - exp(-t/tau))"""
return K * (1 - np.exp(-t / tau))
# Initial guesses from data inspection
K_guess = y_meas[-100:].mean() # Final value estimate
tau_guess = 0.5 # Rough guess
popt_1st, pcov_1st = curve_fit(
first_order_step, t_meas, y_meas,
bounds=([0, 0.01], [10, 10]),
K_1st, tau_1st = popt_1st
# Confidence intervals (1 sigma)
perr_1st = np.sqrt(np.diag(pcov_1st))
y_fit_1st = first_order_step(t_meas, *popt_1st)
rmse_1st = np.sqrt(np.mean((y_meas - y_fit_1st) ** 2))
print("\n First-order fit: G(s) = K / (tau*s + 1)")
print(f" K = {K_1st:.4f} (+/- {perr_1st[0]:.4f})")
print(f" tau = {tau_1st:.4f} s (+/- {perr_1st[1]:.4f})")
print(f" RMSE = {rmse_1st:.4f}")
except RuntimeError as e:
print(f" First-order fit failed: {e}")
y_fit_1st = np.zeros_like(t_meas)
# ============================================================
# Model 2: Second-order fit
# ============================================================
def second_order_step(t, K, wn, zeta):
"""Second-order underdamped step response."""
for i, ti in enumerate(t):
# Overdamped or critically damped
s1 = -wn * (zeta + np.sqrt(zeta**2 - 1))
s2 = -wn * (zeta - np.sqrt(zeta**2 - 1))
y[i] = K * (1 + (s1 * np.exp(s2 * ti) - s2 * np.exp(s1 * ti))
y[i] = K * (1 - (1 + wn * ti) * np.exp(-wn * ti))
wd = wn * np.sqrt(1 - zeta**2)
env = np.exp(-zeta * wn * ti)
phase = np.arctan2(zeta, np.sqrt(1 - zeta**2))
y[i] = K * (1 - env / np.sqrt(1 - zeta**2) * np.sin(wd * ti + phase))
popt_2nd, pcov_2nd = curve_fit(
second_order_step, t_meas, y_meas,
bounds=([0, 0.1, 0.01], [10, 20, 2.0]),
K_2nd, wn_2nd, zeta_2nd = popt_2nd
perr_2nd = np.sqrt(np.diag(pcov_2nd))
y_fit_2nd = second_order_step(t_meas, *popt_2nd)
rmse_2nd = np.sqrt(np.mean((y_meas - y_fit_2nd) ** 2))
print("\n Second-order fit: G(s) = K*wn^2 / (s^2 + 2*zeta*wn*s + wn^2)")
print(f" K = {K_2nd:.4f} (+/- {perr_2nd[0]:.4f})")
print(f" wn = {wn_2nd:.4f} rad/s (+/- {perr_2nd[1]:.4f})")
print(f" zeta = {zeta_2nd:.4f} (+/- {perr_2nd[2]:.4f})")
print(f" RMSE = {rmse_2nd:.4f}")
except RuntimeError as e:
print(f" Second-order fit failed: {e}")
y_fit_2nd = np.zeros_like(t_meas)
# ============================================================
# Model 3: Second-order with delay
# ============================================================
def second_order_delay_step(t, K, wn, zeta, delay):
"""Second-order underdamped step response with transport delay."""
for i, ti in enumerate(t):
s1 = -wn * (zeta + np.sqrt(zeta**2 - 1))
s2 = -wn * (zeta - np.sqrt(zeta**2 - 1))
y[i] = K * (1 + (s1 * np.exp(s2 * td) - s2 * np.exp(s1 * td))
y[i] = K * (1 - (1 + wn * td) * np.exp(-wn * td))
wd = wn * np.sqrt(1 - zeta**2)
env = np.exp(-zeta * wn * td)
phase = np.arctan2(zeta, np.sqrt(1 - zeta**2))
y[i] = K * (1 - env / np.sqrt(1 - zeta**2) * np.sin(wd * td + phase))
popt_delay, pcov_delay = curve_fit(
second_order_delay_step, t_meas, y_meas,
p0=[K_guess, 3.0, 0.5, 0.1],
bounds=([0, 0.1, 0.01, 0.0], [10, 20, 2.0, 2.0]),
K_d, wn_d, zeta_d, delay_d = popt_delay
perr_delay = np.sqrt(np.diag(pcov_delay))
y_fit_delay = second_order_delay_step(t_meas, *popt_delay)
rmse_delay = np.sqrt(np.mean((y_meas - y_fit_delay) ** 2))
print("\n Second-order + delay fit:")
print(f" K = {K_d:.4f} (+/- {perr_delay[0]:.4f})")
print(f" wn = {wn_d:.4f} rad/s (+/- {perr_delay[1]:.4f})")
print(f" zeta = {zeta_d:.4f} (+/- {perr_delay[2]:.4f})")
print(f" delay = {delay_d:.4f} s (+/- {perr_delay[3]:.4f})")
print(f" RMSE = {rmse_delay:.4f}")
except RuntimeError as e:
print(f" Second-order + delay fit failed: {e}")
y_fit_delay = np.zeros_like(t_meas)
rmse_delay = float('inf')
# ============================================================
# ============================================================
print(" Model Comparison")
print(f" {'Model':<30s} {'RMSE':>10s} {'Parameters':>12s}")
print(f" {'-'*30} {'-'*10} {'-'*12}")
print(f" {'First-order':<30s} {rmse_1st:>10.4f} {'2':>12s}")
print(f" {'Second-order':<30s} {rmse_2nd:>10.4f} {'3':>12s}")
print(f" {'Second-order + delay':<30s} {rmse_delay:>10.4f} {'4':>12s}")
print(f" {'Noise floor (theoretical)':<30s} {noise_std:>10.4f} {'':>12s}")
print("\n The best model has RMSE closest to the noise floor.")
print(" If RMSE >> noise, the model structure is insufficient.")
print(" If RMSE ~ noise, the model captures all systematic behavior.")
# ============================================================
# Validation: test on a ramp input
# ============================================================
print("\n--- Validation on Ramp Input ---")
t_val = np.arange(0, 10.0, dt)
ramp_rate = 0.5 # units/second
# True ramp response (numerical simulation via ODE)
def system_ode(state, t, u_func, K, wn, zeta):
"""ODE for second-order system: K*wn^2 / (s^2 + 2*zeta*wn*s + wn^2)"""
ddy = K * wn**2 * u - 2 * zeta * wn * dy - wn**2 * y
"""Ramp starting at t=0"""
return ramp_rate * max(0, t - TRUE_DELAY)
# Simulate the true system with ramp input
system_ode, [0, 0], t_val,
args=(lambda t: ramp_rate * max(0, t - TRUE_DELAY), TRUE_K, TRUE_WN, TRUE_ZETA)
y_val_meas = y_val_true + noise_std * np.random.randn(len(t_val))
# Simulate the identified model with ramp input
if rmse_delay < float('inf'):
system_ode, [0, 0], t_val,
args=(lambda t: ramp_rate * max(0, t - delay_d), K_d, wn_d, zeta_d)
val_rmse = np.sqrt(np.mean((y_val_meas - y_val_model) ** 2))
print(f" Ramp validation RMSE: {val_rmse:.4f}")
print(f" (Compare to noise floor: {noise_std:.4f})")
if val_rmse < 2 * noise_std:
print(" Model validates well on ramp input.")
print(" Model shows degraded performance on ramp input.")
y_val_model = np.zeros_like(t_val)
# ============================================================
# ============================================================
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
# Plot 1: Measured data and fits
ax.plot(t_meas, y_meas, 'k.', markersize=1, alpha=0.3, label='Measured data')
ax.plot(t_meas, y_true, 'k-', linewidth=2, alpha=0.5, label='True (hidden)')
ax.plot(t_meas, y_fit_1st, 'b--', linewidth=2, label=f'1st order (RMSE={rmse_1st:.3f})')
ax.plot(t_meas, y_fit_2nd, 'r-', linewidth=2, label=f'2nd order (RMSE={rmse_2nd:.3f})')
ax.plot(t_meas, y_fit_delay, 'g-', linewidth=2,
label=f'2nd + delay (RMSE={rmse_delay:.3f})')
ax.set_xlabel('Time (s)')
ax.set_title('Step Response: Data vs Fitted Models')
ax.legend(fontsize=8, loc='lower right')
if rmse_delay < float('inf'):
residuals = y_meas - y_fit_delay
ax.plot(t_meas, residuals, 'g.', markersize=1, alpha=0.5)
ax.axhline(0, color='black', linewidth=0.5)
ax.axhline(noise_std, color='red', linestyle='--', alpha=0.5, label=f'+/- noise std')
ax.axhline(-noise_std, color='red', linestyle='--', alpha=0.5)
ax.set_xlabel('Time (s)')
ax.set_ylabel('Residual')
ax.set_title('Residuals (Best Model)')
# Plot 3: Validation on ramp
ax.plot(t_val, y_val_meas, 'k.', markersize=1, alpha=0.3, label='Measured (ramp)')
ax.plot(t_val, y_val_true, 'k-', linewidth=2, alpha=0.5, label='True response')
if rmse_delay < float('inf'):
ax.plot(t_val, y_val_model, 'g-', linewidth=2,
label=f'Identified model (RMSE={val_rmse:.3f})')
ax.plot(t_val, ramp_rate * np.maximum(0, t_val - TRUE_DELAY), 'gray',
linestyle=':', label='Ramp input (scaled)')
ax.set_xlabel('Time (s)')
ax.set_title('Validation: Ramp Input (Not Used for Fitting)')
ax.legend(fontsize=8, loc='upper left')
# Plot 4: Parameter convergence / error bar chart
if rmse_delay < float('inf'):
param_names = ['K', 'wn', 'zeta', 'delay']
true_vals = [TRUE_K, TRUE_WN, TRUE_ZETA, TRUE_DELAY]
est_vals = [K_d, wn_d, zeta_d, delay_d]
est_errs = [perr_delay[i] for i in range(4)]
# Normalize: show percentage error
pct_errors = [100 * abs(e - t) / t for e, t in zip(est_vals, true_vals)]
bars = ax.bar(param_names, pct_errors, color=['steelblue', 'coral', 'green', 'orange'],
alpha=0.7, edgecolor='black')
ax.set_ylabel('Estimation Error (%)')
ax.set_title('Parameter Estimation Accuracy')
ax.grid(True, alpha=0.3, axis='y')
for bar, pct, est, true in zip(bars, pct_errors, est_vals, true_vals):
ax.text(bar.get_x() + bar.get_width()/2, bar.get_height() + 0.2,
f'{est:.3f}\n(true: {true})', ha='center', fontsize=8)
plt.savefig('system_identification.png', dpi=150, bbox_inches='tight')
print("\nPlot saved: system_identification.png")
Comments