File size: 4,890 Bytes
4cefcfa
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
/* Next Balance compute core: line-by-line port of balance_core.py. */
(function (root, factory) {
  if (typeof module === "object" && module.exports) module.exports = factory();
  else root.Core = factory();
})(typeof self !== "undefined" ? self : this, function () {
  "use strict";
  const DT = 0.01, G = 9.81, U_MAX = 400.0, M_PLATFORM = 2.0, EPS = 0.01, ALPHA = 0.3, BETA = 0.05, Z_REF = 0.5, T_STEP = 0.5, T_RAMP = 1.0;
  const ASSETS = ["model.json"];

  function init(assets) { return { params: assets["model.json"].meta.params }; }

  function simulate(sc, ctrl, params, recordEvery) {
    recordEvery = recordEvery || 1;
    const rng = NLRT.mulberry32((sc.seed * 31 + 7) >>> 0);
    const mNom = M_PLATFORM + sc.payload0, n = Math.round(sc.T / DT);
    let z = 0, v = 0, zh = 0, vh = 0, integ = 0;
    const queue = []; for (let i = 0; i < sc.delay + 1; i++) queue.push(mNom * G);
    let theta = [mNom, 3.0, 2.0];
    let P = [[10, 0, 0], [0, 10, 0], [0, 0, 10]];
    let fz = [0, 0], fu = [mNom * G, 0], ft = [0, 0];
    const w = params.wf != null ? params.wf : 20.0, lam = params.lam != null ? params.lam : 0.995;
    const imax = params.imax != null ? params.imax : 0.5;
    const out = { t: [], z: [], u: [], m_hat: [], energy: 0 };
    for (let k = 0; k < n; k++) {
      const t = k * DT;
      const m = M_PLATFORM + sc.payload0 + (t >= sc.t_load ? sc.dm : 0);
      const gf = t >= sc.t_fric ? sc.fric_gain : 1.0, c = sc.c0 * gf, F = sc.F0 * gf;
      const ref = Z_REF * Math.min(1.0, Math.max(0.0, (t - T_STEP) / T_RAMP));
      const meas = z + sc.noise * rng.normal();
      const zp = zh + vh * DT, res = meas - zp;
      zh = zp + ALPHA * res; vh = vh + (BETA / DT) * res;
      const e = ref - zh;
      integ = Math.max(-imax, Math.min(imax, integ + e * DT));
      let u;
      if (ctrl === "pid") {
        u = params.kp * e + params.ki * integ - params.kd * vh + mNom * G;
      } else {
        const uApplied = queue[0], tv = Math.tanh(vh / EPS);
        const az = w * w * (meas - fz[0]) - 2 * w * fz[1];
        const au = w * w * (uApplied - fu[0]) - 2 * w * fu[1];
        const at = w * w * (tv - ft[0]) - 2 * w * ft[1];
        fz = [fz[0] + DT * fz[1], fz[1] + DT * az];
        fu = [fu[0] + DT * fu[1], fu[1] + DT * au];
        ft = [ft[0] + DT * ft[1], ft[1] + DT * at];
        const phi = [az + G, fz[1], ft[0]], y = fu[0];
        const Pphi = [0, 1, 2].map(i => { let s = 0; for (let j = 0; j < 3; j++) s += P[i][j] * phi[j]; return s; });
        let s = 0; for (let i = 0; i < 3; i++) s += phi[i] * Pphi[i];
        const den = lam + s, K = Pphi.map(x => x / den);
        let s2 = 0; for (let i = 0; i < 3; i++) s2 += phi[i] * theta[i];
        const err = y - s2;
        theta = theta.map((th, i) => th + K[i] * err);
        P = [0, 1, 2].map(i => [0, 1, 2].map(j => (P[i][j] - K[i] * Pphi[j]) / lam));
        theta = [Math.min(30, Math.max(0.5, theta[0])), Math.min(60, Math.max(0, theta[1])), Math.min(60, Math.max(0, theta[2]))];
        const wn = params.wn, zeta = params.zeta;
        const acmd = wn * wn * e - 2 * zeta * wn * vh + params.ki_a * integ;
        u = theta[0] * (acmd + G) + theta[1] * vh;
      }
      u = Math.max(0, Math.min(U_MAX, u));
      queue.push(u);
      const uAct = queue.shift();
      const a = (uAct - c * v - F * Math.tanh(v / EPS)) / m - G;
      v = v + a * DT; z = z + v * DT;
      out.energy += uAct * uAct * DT;
      if (k % recordEvery === 0) { out.t.push(Math.round(t * 1e4) / 1e4); out.z.push(z); out.u.push(uAct); out.m_hat.push(ctrl !== "pid" ? theta[0] : mNom); }
    }
    return out;
  }

  function metrics(sc, tr, band, recordEvery) {
    band = band || 0.01; recordEvery = recordEvery || 1;
    const { t, z } = tr;
    let iae = 0; for (let i = 0; i < t.length; i++) iae += Math.abs(z[i] - Z_REF * Math.min(1.0, Math.max(0.0, (t[i] - T_STEP) / T_RAMP)));
    iae *= DT * recordEvery;
    let maxDev = 0, lastOut = null, over = 0, anyStep = false;
    for (let i = 0; i < t.length; i++) {
      if (t[i] >= sc.t_load) { maxDev = Math.max(maxDev, Math.abs(z[i] - Z_REF)); if (Math.abs(z[i] - Z_REF) > band) lastOut = t[i]; }
      else if (t[i] >= T_STEP) { anyStep = true; over = Math.max(over, z[i] - Z_REF); }
    }
    return { max_deviation_m: maxDev, settling_time_s: lastOut === null ? 0 : lastOut - sc.t_load,
      settled: lastOut === null || lastOut < t[t.length - 1] - 0.5, control_energy_kN2s: tr.energy / 1e6,
      step_overshoot_m: anyStep ? Math.max(0, over) : 0, final_error_m: Math.abs(z[z.length - 1] - Z_REF), iae_ms: iae };
  }

  function runCase(ctx, input) {
    const p = ctx.params[input.ctrl];
    const tr = simulate(input.scenario, input.ctrl, p, 1);
    const zs = tr.z.filter((_, i) => i % 50 === 0);
    return { metrics: metrics(input.scenario, tr), z_every_50: zs };
  }

  return { ASSETS, init, simulate, metrics, runCase, DT, Z_REF };
});