"""System identification of one joint from a noisy step response. Model: J * theta'' + b * theta' = u (inertia J, viscous damping b, constant torque u). From rest, the position is theta(t) = K * (t - tau * (1 - exp(-t / tau))), K = u / b, tau = J / b. K is the final speed and tau the time constant. We fit (K, tau) to measured positions by least squares (a linear fit for the start values, then Gauss-Newton on the full curve) and convert back to b = u / K and J = tau * b. """ from __future__ import annotations from dataclasses import dataclass import numpy as np def step_position(t: np.ndarray, k: float, tau: float) -> np.ndarray: return k * (t - tau * (1.0 - np.exp(-t / tau))) def simulate_step(j: float, b: float, u: float, dt: float, n: int, noise_std: float, rng: np.random.Generator, counts_per_rev: int | None = 4096) -> tuple[np.ndarray, np.ndarray]: """Return (t, measured position). Adds Gaussian noise, then encoder quantisation.""" t = np.arange(n) * dt theta = step_position(t, u / b, j / b) + rng.normal(0.0, noise_std, n) if counts_per_rev: q = 2.0 * np.pi / counts_per_rev theta = np.round(theta / q) * q return t, theta @dataclass class FitResult: j: float b: float k: float tau: float residual_rms: float b_std: float # one-sigma standard error of b from the linearised fit iterations: int def initial_guess(t: np.ndarray, theta: np.ndarray, tail_fraction: float = 0.5) -> tuple[float, float]: """Linear least squares on the tail, where theta ~ K * t - K * tau.""" m = t >= t[-1] * (1.0 - tail_fraction) a = np.column_stack([t[m], np.ones(m.sum())]) (slope, intercept), *_ = np.linalg.lstsq(a, theta[m], rcond=None) k = float(slope) tau = max(float(-intercept / slope), 1e-6) return k, tau def fit_step(t: np.ndarray, theta: np.ndarray, u: float, max_iter: int = 50, tol: float = 1e-12) -> FitResult: k, tau = initial_guess(t, theta) p = np.array([k, tau]) for it in range(1, max_iter + 1): k, tau = p e = np.exp(-t / tau) r = theta - step_position(t, k, tau) # Jacobian of the model with respect to (K, tau). d_k = t - tau * (1.0 - e) d_tau = k * (-(1.0 - e) + (t / tau) * e) jac = np.column_stack([d_k, d_tau]) step, *_ = np.linalg.lstsq(jac, r, rcond=None) p = p + step p[1] = max(p[1], 1e-6) if np.max(np.abs(step) / np.maximum(np.abs(p), 1e-12)) < tol: break k, tau = (float(v) for v in p) r = theta - step_position(t, k, tau) dof = max(len(t) - 2, 1) sigma2 = float(r @ r) / dof e = np.exp(-t / tau) jac = np.column_stack([t - tau * (1.0 - e), k * (-(1.0 - e) + (t / tau) * e)]) cov = sigma2 * np.linalg.inv(jac.T @ jac) k_std = float(np.sqrt(cov[0, 0])) b = u / k return FitResult(j=tau * b, b=b, k=k, tau=tau, residual_rms=float(np.sqrt(sigma2)), b_std=abs(u / k**2) * k_std, iterations=it) if __name__ == "__main__": # Example values, not measured on a real servo. J_TRUE, B_TRUE, U = 0.01, 0.10, 0.05 # kg m^2, N m s/rad, N m rng = np.random.default_rng(42) t, theta = simulate_step(J_TRUE, B_TRUE, U, dt=0.01, n=100, noise_std=0.003, rng=rng) k0, tau0 = initial_guess(t, theta) fit = fit_step(t, theta, U) print(f"start values from the tail fit: K = {k0:.4f} rad/s, tau = {tau0:.4f} s") print(f"Gauss-Newton ({fit.iterations} iterations): K = {fit.k:.4f} rad/s, tau = {fit.tau:.4f} s") print(f"b = {fit.b:.4f} +/- {fit.b_std:.4f} N m s/rad (true {B_TRUE}), " f"J = {fit.j:.5f} kg m^2 (true {J_TRUE})") print(f"residual RMS = {fit.residual_rms * 1000:.2f} mrad")