File size: 5,559 Bytes
573eba1
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
"""Next Balance: hold a vertical lift platform at its target while the payload and friction change.

Simulation only. Plant (semi-implicit Euler, dt = 0.01 s):
    m(t) * (z'' + g) = u(t - delay) - c(t) * z' - F(t) * tanh(z' / 0.01)
Sensor: z + Gaussian noise. Actuator force is limited to [0, U_MAX].
Both controllers use the same alpha-beta state estimator and know the nominal payload only.

fixed PID   : u = Kp e + Ki I - Kd v + m_nominal g
adaptive    : recursive least squares (forgetting factor) estimates [m, c, F] from filtered
              signals; u = m_hat (wn^2 e - 2 zeta wn v + ki I + g) + c_hat v
              (F_hat is estimated to keep m_hat unbiased but not fed back: it chatters with noisy v)
The same code is ported line by line to web/core.js; parity is tested on fixed scenarios.
"""
import math

from nlrt import Mulberry32

DT, G, U_MAX, M_PLATFORM, EPS = 0.01, 9.81, 400.0, 2.0, 0.01
ALPHA, BETA = 0.3, 0.05
Z_REF, T_STEP, T_RAMP = 0.5, 0.5, 1.0


def scenario(seed, stress=False):
    r = Mulberry32(seed)
    s = {"payload0": r.uniform(1, 4), "t_load": r.uniform(3.5, 4.5),
         "dm": r.uniform(-1, 10 if stress else 6), "c0": r.uniform(2, 5), "F0": r.uniform(1, 3),
         "t_fric": r.uniform(6.5, 7.5), "fric_gain": r.uniform(1, 6 if stress else 4),
         "delay": r.randint(8 if stress else 6), "noise": r.uniform(0, 0.006 if stress else 0.003),
         "T": 12.0, "seed": seed}
    return s


def simulate(sc, ctrl, params, record_every=1):
    rng = Mulberry32(sc["seed"] * 31 + 7)
    m_nom = M_PLATFORM + sc["payload0"]
    n = int(round(sc["T"] / DT))
    z = v = 0.0
    zh = vh = 0.0
    integ = 0.0
    queue = [m_nom * G] * (sc["delay"] + 1)  # commands in flight; start at hover force
    # adaptive state: RLS + state-variable filters
    theta = [m_nom, 3.0, 2.0]
    P = [[10.0, 0, 0], [0, 10.0, 0], [0, 0, 10.0]]
    fz = [0.0, 0.0]
    fu = [m_nom * G, 0.0]
    ft = [0.0, 0.0]
    w = params.get("wf", 20.0)
    lam = params.get("lam", 0.995)
    t_hist, z_hist, u_hist, m_hist = [], [], [], []
    energy = 0.0
    for k in range(n):
        t = k * DT
        m = M_PLATFORM + sc["payload0"] + (sc["dm"] if t >= sc["t_load"] else 0.0)
        g_f = sc["fric_gain"] if t >= sc["t_fric"] else 1.0
        c, F = sc["c0"] * g_f, sc["F0"] * g_f
        ref = Z_REF * min(1.0, max(0.0, (t - T_STEP) / T_RAMP))
        # sensor and estimator
        meas = z + sc["noise"] * rng.normal()
        zp = zh + vh * DT
        res = meas - zp
        zh = zp + ALPHA * res
        vh = vh + (BETA / DT) * res
        e = ref - zh
        integ = max(-params.get("imax", 0.5), min(params.get("imax", 0.5), integ + e * DT))
        if ctrl == "pid":
            u = params["kp"] * e + params["ki"] * integ - params["kd"] * vh + m_nom * G
        else:
            u_applied = queue[0]
            tv = math.tanh(vh / EPS)
            # state-variable filters H(s) = w^2 / (s + w)^2 on measured z, applied force and friction regressor
            az = w * w * (meas - fz[0]) - 2 * w * fz[1]
            au = w * w * (u_applied - fu[0]) - 2 * w * fu[1]
            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]
            phi = [az + G, fz[1], ft[0]]
            y = fu[0]
            Pphi = [sum(P[i][j] * phi[j] for j in range(3)) for i in range(3)]
            den = lam + sum(phi[i] * Pphi[i] for i in range(3))
            K = [Pphi[i] / den for i in range(3)]
            err = y - sum(phi[i] * theta[i] for i in range(3))
            theta = [theta[i] + K[i] * err for i in range(3)]
            P = [[(P[i][j] - K[i] * Pphi[j]) / lam for j in range(3)] for i in range(3)]
            theta = [min(30.0, max(0.5, theta[0])), min(60.0, max(0.0, theta[1])), min(60.0, max(0.0, theta[2]))]
            wn, zeta = params["wn"], params["zeta"]
            acmd = wn * wn * e - 2 * zeta * wn * vh + params["ki_a"] * integ
            u = theta[0] * (acmd + G) + theta[1] * vh
        u = max(0.0, min(U_MAX, u))
        queue.append(u)
        u_act = queue.pop(0)
        # plant
        a = (u_act - c * v - F * math.tanh(v / EPS)) / m - G
        v = v + a * DT
        z = z + v * DT
        energy += u_act * u_act * DT
        if k % record_every == 0:
            t_hist.append(round(t, 4))
            z_hist.append(z)
            u_hist.append(u_act)
            m_hist.append(theta[0] if ctrl != "pid" else m_nom)
    return {"t": t_hist, "z": z_hist, "u": u_hist, "m_hat": m_hist, "energy": energy}


def metrics(sc, tr, band=0.01, record_every=1):
    t, z = tr["t"], tr["z"]
    iae = sum(abs(z[i] - Z_REF * min(1.0, max(0.0, (t[i] - T_STEP) / T_RAMP))) for i in range(len(t))) * DT * record_every
    after = [i for i in range(len(t)) if t[i] >= sc["t_load"]]
    max_dev = max(abs(z[i] - Z_REF) for i in after)
    last_out = None
    for i in after:
        if abs(z[i] - Z_REF) > band:
            last_out = t[i]
    settled = last_out is None or last_out < t[-1] - 0.5
    settle = 0.0 if last_out is None else last_out - sc["t_load"]
    step_idx = [i for i in range(len(t)) if T_STEP <= t[i] < sc["t_load"]]
    overshoot = max(0.0, max(z[i] for i in step_idx) - Z_REF) if step_idx else 0.0
    return {"max_deviation_m": max_dev, "settling_time_s": settle, "settled": settled,
            "control_energy_kN2s": tr["energy"] / 1e6, "step_overshoot_m": overshoot,
            "final_error_m": abs(z[-1] - Z_REF), "iae_ms": iae}