状态反馈：LQR、LQI 与平衡步兵

对象：平衡步兵的拉格朗日模型#

先说我是怎么避免写错符号的，因为这一节是整篇里最容易默默写错的地方：我从拉格朗日量把两条运动方程完整推了一遍，再用三个数值检查核对（能量守恒、功率平衡、对中心差分的线性化），检查的结果放在“结果”那一节。

对象是平面里的两轮倒立摆：两个轮子合起来当一个轮子（质量 $m_w=2$ kg，转动惯量 $I_w=0.01$ kg·m²，半径 $r=0.1$ m），车身质量 $M=12$ kg，质心在轮轴上方 $l=0.2$ m，绕质心的转动惯量 $I_b=0.4$ kg·m²，电机力矩上限 10 N·m（这些都是我取的量级，不是实测）。广义坐标取两个：轮子滚过的距离 $x=r\theta_w$，和车身从竖直方向向前倾的角度 $\varphi$（$\varphi>0$ 表示质心跑到 $+x$ 一侧）。

车身质心的位置是 $(x+l\sin\varphi,\ l\cos\varphi)$，速度分量 $\dot x+l\cos\varphi\,\dot\varphi$ 和 $-l\sin\varphi\,\dot\varphi$。动能是轮子的平动加转动、车身质心的平动、车身绕质心的转动：

记 $a=m_w+I_w/r^2+M$（数值 15 kg）和 $c=I_b+Ml^2$（0.88 kg·m²）。$L=T-V$ 对 $x$ 不显含，所以 $x$ 方程的左边只有 $\frac{d}{dt}\frac{\partial L}{\partial\dot x}$：

$\varphi$ 方程里 $\dot x\dot\varphi\sin\varphi$ 这一项在 $\frac{d}{dt}\frac{\partial L}{\partial\dot\varphi}$ 和 $\frac{\partial L}{\partial\varphi}$ 里各出现一次，抵消了：

广义力是符号最容易出错的地方，我用虚功算。电机的力矩 $\tau$ 作用在轮子上，同时有一个反作用力矩 $-\tau$ 作用在车身上，所以虚功是 $\tau\,\delta\theta_w-\tau\,\delta\varphi=\tfrac{\tau}{r}\delta x-\tau\,\delta\varphi$，于是 $Q_x=\tau/r$，$Q_\varphi=-\tau$。$\tau>0$ 的意思是轮子向前滚，车身被向后推，和“加速时身体后仰”的日常经验一致。再加一个外力：水平方向的力 $F$ 作用在车身质心上（后面的 push 算例要用），虚功是 $F\,\delta(x+l\sin\varphi)=F\,\delta x+Fl\cos\varphi\,\delta\varphi$，所以 $Q_x$ 多一个 $F$，$Q_\varphi$ 多一个 $Fl\cos\varphi$。完整的模型是：

每次求导数的时候解一个 2×2：$\begin{bmatrix}a&Ml\cos\varphi\\Ml\cos\varphi&c\end{bmatrix}$ 乘 $(\ddot x,\ddot\varphi)$ 等于右边，行列式 $ac-(Ml\cos\varphi)^2$ 在 $\varphi=0$ 是 7.44，永远不会是 0（$a c - M^2l^2\cos^2\varphi\ge ac-M^2l^2$），直接用克莱姆法则。plants.py 里的 Wip.deriv 就是这几行：

1
    def deriv(self, x, u):
2
        a, c = self._coefs()
3
        ml = self.m * self.l
4
        s, co = math.sin(x[2]), math.cos(x[2])
5
        r1 = u[0] / self.r + u[1] + ml * s * x[3] * x[3]
6
        r2 = -u[0] + u[1] * self.l * co + ml * self.g * s
7
        b = ml * co
8
        det = a * c - b * b
9
        return [
10
            x[1],
11
            (c * r1 - b * r2) / det,
12
            x[3],
13
            (a * r2 - b * r1) / det,
14
        ]

r1 和 r2 是两个方程的右边（把 $\dot\varphi^2$ 项和重力项移了过去），u 是 $[\tau,\ F]$，所以外力是对象的第二个输入，和前几篇“扰动就是多出来的输入”的约定一致。同一个文件里还有 energy，是后面自检要用的动能加势能。

线性化：A 和 B 是怎么出来的#

在 $\varphi=0$ 附近取 $\sin\varphi\approx\varphi$，$\cos\varphi\approx1$，丢掉 $\dot\varphi^2$，令 $F=0$：

记 $\Delta=ac-(Ml)^2$，把 2×2 求逆：

代入 $Ml=2.4$，$Mgl=23.544$，$\Delta=15\times0.88-2.4^2=7.44$，状态取 $\mathbf x=(x,\dot x,\varphi,\dot\varphi)$：

代码把这四个分式原样写出来（只给 $\tau$ 这一列，外力 $F$ 只出现在非线性模型里）：

1
    def linearize(self) -> tuple[list[list[float]], list[list[float]]]:
2
        """A, B of x' = A x + B tau at phi = 0 (hand derivation, ch09)."""
3
        a, c = self._coefs()
4
        ml = self.m * self.l
5
        mgl = ml * self.g
6
        det = a * c - ml * ml
7
        amat = [
8
            [0.0, 1.0, 0.0, 0.0],
9
            [0.0, 0.0, -ml * mgl / det, 0.0],
10
            [0.0, 0.0, 0.0, 1.0],
11
            [0.0, 0.0, a * mgl / det, 0.0],
12
        ]
13
        bmat = [
14
            [0.0],
15
            [(c / self.r + ml) / det],
16
            [0.0],
17
            [-(a + ml / self.r) / det],
18
        ]
19
        return amat, bmat

两个观察。第一，$A$ 的特征值是 $0,0,\pm\sqrt{47.468}=\pm6.89$ rad/s：一个不稳定极点，倾倒的时间常数 $1/6.89=0.145$ s，所以控制器必须比这快得多（1 kHz 对这个来说足够）。第二，$B$ 的第二项和第四项符号相反：$\tau>0$ 让轮子向前加速，同时把车身向后推。要让车向前走，一开始得先向后“退”，让车身前倾，再追上去。这是一个非最小相位的系统，阶跃响应里会有一个先反向的小凹坑，后面的图里能看见，凹坑有多深我后面给数。

我不信手推的结果，所以 report 把 $A$ 和 $B$ 和 Wip.deriv 的中心差分（步长 $10^{-6}$）比了一次：最大误差 $4.47\times10^{-11}$，是差分本身的截断误差量级。

离散化：矩阵指数和 ZOH#

控制器每 1 ms 跑一次，输入保持不变（第 4 篇的 ZOH），对象 $\dot x=Ax+B\tau$ 的精确离散化是

两个量可以用一个矩阵指数同时得到：

验证很短：右边那个矩阵的幂级数里，第二列块是 $\sum_{k\ge1}\frac{t_s^k}{k!}A^{k-1}B=\int_0^{t_s}e^{A\sigma}d\sigma\,B$。

矩阵指数我自己写，不调库：缩放平方。先把矩阵除以 $2^s$，直到无穷范数不超过 0.5，对缩小后的矩阵做 12 阶泰勒（截断误差大约是 $0.5^{13}/13!\approx2\times10^{-14}$），再平方 $s$ 次。Python 版和 scipy.linalg.expm 在 4 个量级（$0.1$ 到 $30$ 倍的随机 5×5 矩阵）上的最大相对误差是 $2.51\times10^{-13}$，在我设的 $10^{-12}$ 容差之内。

pidlab/mat.py 是 LQR 要用的那几个矩阵运算（zeros、identity、mul、add、sub、scale、transpose、max_abs）。矩阵是行列表组成的列表（list[list[float]]），没有用 numpy，因为这是原生版本要逐位对上的参考实现，numpy 和 scipy 只出现在 report 里做核对。里面最重要的是矩阵乘，一个朴素的三重循环，累加顺序后面三种语言都照抄：

1
def mul(a: Matrix, b: Matrix) -> Matrix:
2
    """a (n x k) times b (k x m)."""
3
    n, k, m = len(a), len(b), len(b[0])
4
    out = zeros(n, m)
5
    for i in range(n):
6
        for j in range(m):
7
            s = 0.0
8
            for p in range(k):
9
                s += a[i][p] * b[p][j]
10
            out[i][j] = s
11
    return out

pidlab/lqr.py 里的 expm 是上面那段话的直译：

1
def expm(a: Matrix) -> Matrix:
2
    """exp(a): scale until ||a||_inf <= 0.5, Taylor to order 12, square."""
3
    n = len(a)
4
    nrm = 0.0
5
    for row in a:
6
        s = 0.0
7
        for x in row:
8
            s += abs(x)
9
        nrm = max(nrm, s)
10
    squarings = 0
11
    while nrm > 0.5:
12
        nrm *= 0.5
13
        squarings += 1
14
    h = scale(a, 0.5**squarings)
15
    term = identity(n)
16
    out = identity(n)
17
    for k in range(1, TAYLOR_ORDER + 1):
18
        term = scale(mul(term, h), 1.0 / k)
19
        out = add(out, term)
20
    for _ in range(squarings):
21
        out = mul(out, out)
22
    return out

求范数用显式的循环累加，不用 sum()：Python 3.12 起 sum() 对浮点数用补偿求和，和 C 的朴素累加在最后一位上会不同，虽然在这里只影响“要缩放几次”这个判断，我还是把它对齐了。有了 expm，ZOH 就是把 $A$、$B$ 拼成 $(n+m)\times(n+m)$ 的块矩阵，乘 $t_s$，取指数，再把左上角和右上角切出来：

1
def c2d_zoh(a: Matrix, b: Matrix, ts: float) -> tuple[Matrix, Matrix]:
2
    """Zero-order-hold discretization via exp([[A, B], [0, 0]] ts)."""
3
    n, m = len(a), len(b[0])
4
    big = zeros(n + m, n + m)
5
    for i in range(n):
6
        for j in range(n):
7
            big[i][j] = a[i][j] * ts
8
        for j in range(m):
9
            big[i][n + j] = b[i][j] * ts
10
    e = expm(big)
11
    ad = [row[:n] for row in e[:n]]
12
    bd = [row[n:] for row in e[:n]]
13
    return ad, bd

LQR：代价函数、Riccati 方程和 Bryson 权重#

LQR 要的是在无穷时域里最小化

的状态反馈 $u_k=-Kx_k$。Kalman 在 1960 年的 “Contributions to the theory of optimal control” 里讨论的就是这一类问题（摘要的原话是设计线性控制系统来最小化某个二次函数的积分）；我这里用的是离散时间的形式，推导我没有去对原文，下面的递推式是标准结果，由 scipy.linalg.solve_discrete_are 核对。

最优的 $K$ 由离散代数 Riccati 方程（DARE）的解 $P$ 给出：

单输入的时候 $R+B_d^TPB_d$ 是标量，求逆就是一次除法，所以增益函数很短：

1
def lqr_gain(ad: Matrix, bd: Matrix, p: Matrix, r: float) -> Matrix:
2
    """K = (R + B'PB)^-1 B'PA as a 1 x n matrix."""
3
    pb = mul(p, bd)
4
    s = r + mul(transpose(bd), pb)[0][0]
5
    return scale(mul(transpose(pb), ad), 1.0 / s)

权重怎么取。 我用的是 Bryson 法则：每个状态（和输入）的权重取“允许的最大值”的平方的倒数，见 UCSB 的一份讲义（我没有去翻 Bryson 和 Ho 的原书）。允许值我取：位置 0.5 m、速度 1 m/s、倾角 0.1 rad、倾角速度 1 rad/s、力矩 10 N·m，于是

怎么解 DARE：一个我踩了的坑。 最直接的做法是从 $P=Q$ 出发反复套上面那个式子，直到不再变化。我先这么写了，结果它不收敛：最大变化量 $\max|\Delta P|$ 在第 4454 步降到最低的 $4.2\times10^{-2}$，然后开始变大，到第 20000 步是 $4.9\times10^{44}$（report 里把这个原始版本留着，单独跑一遍打印出来）。原因我猜是舍入：$A_d^TPA_d$ 和 $A_d^TPB_d(\cdot)^{-1}B_d^TPA_d$ 是两个很大、很接近的量相减，而对象有一个不稳定模态，把相减后剩下的舍入误差一步步放大。

修法是把递推换成一个等价但全是“正半定的项相加”的形式：先用当前的 $P$ 算出 $K$，再

等价性两行就能看出来：展开右边得 $Q+A_d^TPA_d-2K^TB_d^TPA_d+K^T(R+B_d^TPB_d)K$，而 $K^T(R+B_d^TPB_d)K=K^TB_d^TPA_d$（把 $K$ 的定义代进去），合并后正是 DARE 的右边。收敛判据是 $\max|\Delta P|<10^{-12}(1+\max|P|)$，上限 200000 步，超过返回错误：

1
def dare_iterate(
2
    ad: Matrix, bd: Matrix, q: Matrix, r: float
3
) -> tuple[Matrix, int]:
4
    # ...
5
    p = q
6
    for it in range(1, MAX_ITER + 1):
7
        k = lqr_gain(ad, bd, p, r)
8
        acl = sub(ad, mul(bd, k))
9
        kt = transpose(k)
10
        pn = add(q, scale(mul(kt, k), r))
11
        pn = add(pn, mul(transpose(acl), mul(p, acl)))
12
        done = max_abs(sub(pn, p)) < 1e-12 * (1.0 + max_abs(pn))
13
        p = pn
14
        if done:
15
            return p, it
16
    raise RuntimeError("DARE iteration did not converge")

注意 K 在每一步都重新算，这就是上面数学里的稳定形式。1 kHz 下 LQR 要 13067 步，LQI 要 15178 步（慢的原因是最慢的闭环极点在 $1-0.00084$，也就是 1.2 s 的时间常数，对 1 ms 的步长来说要迭代上万次）。

这个判据有一个代价：停下来的时候，$P$ 离真解还差大约 $\Delta P/(1-\rho)$，$\rho$ 是收敛率，接近 1。实测 Python 的 $P$ 和 solve_discrete_are 的相对差（除以 $\max|P|$）是 LQR $6.53\times10^{-10}$、LQI $4.53\times10^{-10}$，$K$ 是 $3.07\times10^{-10}$ 和 $1.99\times10^{-10}$。都在 $10^{-9}$ 的容差内，但只有一倍多的余量，这是停止判据决定的，不是 bug。

LQI：给位置误差加一个积分状态#

LQR 的 $u=-Kx$ 没有积分：外力持续作用的时候，平衡点会偏。第 1 篇说过，积分项迫使任何稳态的误差为零，LQI 把同一件事放进状态里：增广一个状态 $z$，让它积分位置误差，

$c=(1,0,0,0)$ 取出位置。对这个五阶系统用同一个 DARE 求解，$Q$ 多一项 $q_z=4$，得到 $K$ 的五个分量，最后一个是 $k_z$。增广本身只有几行，c 是参数，换成别的输出行也能用：

1
def lqi_augment(
2
    ad: Matrix, bd: Matrix, c: Matrix, ts: float
3
) -> tuple[Matrix, Matrix]:
4
    """Append z+ = z + ts (x_ref - c x) to the state. c is 1 x n."""
5
    n = len(ad)
6
    aa = zeros(n + 1, n + 1)
7
    ba = zeros(n + 1, 1)
8
    for i in range(n):
9
        for j in range(n):
10
            aa[i][j] = ad[i][j]
11
        ba[i][0] = bd[i][0]
12
        aa[n][i] = -ts * c[0][i]
13
    aa[n][n] = 1.0
14
    return aa, ba

我把参考也放进了比例项：$u=-K(x-x_{ref}e_0)-k_zz$，等价于把状态平移到参考点（平衡步兵在任何位置都是平衡点，平移不改变动力学）。力矩饱和时冻结 $z$（和第 5 篇的 CLAMP 同一个思路）。设计的入口在驱动 ch09_lqr.py 的 design() 里，把上面几个函数串起来：线性化，离散化，LQR 的 4 阶 DARE，LQI 的 5 阶 DARE：

1
def design() -> dict:
2
    """Discretize the linearization and solve LQR and LQI."""
3
    a, b = Wip().linearize()
4
    ad, bd = c2d_zoh(a, b, TS)
5
    p4, it4 = dare_iterate(ad, bd, q_matrix(4), R)
6
    k4 = lqr_gain(ad, bd, p4, R)
7
    aa, ba = lqi_augment(ad, bd, [[1.0, 0.0, 0.0, 0.0]], TS)
8
    p5, it5 = dare_iterate(aa, ba, q_matrix(5), R)
9
    k5 = lqr_gain(aa, ba, p5, R)
10
    # ...

串级 PID 是一个受限的状态反馈#

现在回到开头的问题。串级的三个环，用第 5 篇的 Pid 搭：

直立环的符号要说一下。从上面线性化的结果看，倾角 $\varphi>0$（质心在前）的时候要让轮子向前滚（$\tau>0$）去追质心，也就是 $\tau=+P\varphi+D\dot\varphi$，$P,D>0$。第 5 篇的 Pid 算的是 $k_p(r-y)$，所以直立环的 kp、kd 取负数：$-P$ 和 $-D$（Pid 允许负增益，第 8 篇 Wood–Berry 的回路 2 就用过）。D 项作用在测量上（c = 0），所以 $D$ 项是 $-D\cdot\frac{d}{dt}(-\varphi)=+D\dot\varphi$。速度环输出倾角设定值：想前进更快，就把车身往前倾，$\varphi_{ref}=k_v(v_{ref}-\dot x)+w$，$\dot w=k_i(v_{ref}-\dot x)$，位置环 $v_{ref}=k_x(x_{ref}-x)$。接线和第 7 篇一样：外环的输出是内环的设定值，速度环的 hold_integral 是直立环上一拍的 saturated：

1
class Cascade:
2
    """Position P -> velocity PI -> pitch PD, all from the ch05 Pid."""
3

4
    def __init__(self, g: dict) -> None:
5
        lim = Wip().tau_max
6
        self.pos = Pid(PidConfig(g["kx"], 0.0, 0.0, TS))
7
        self.vel = Pid(PidConfig(g["kv"], g["ki"], 0.0, TS))
8
        # u = P phi + D phid: the pitch loop has negative gains
9
        self.pit = Pid(
10
            PidConfig(-g["p"], 0.0, -g["d"], TS, u_min=-lim, u_max=lim)
11
        )
12

13
    def update(self, x: list[float], xref: float) -> float:
14
        vref = self.pos.update(xref, x[0])
15
        phiref = self.vel.update(vref, x[1], 0.0, self.pit.saturated)
16
        return self.pit.update(phiref, x[2])

把三层展开，$\tau=P\varphi+D\dot\varphi-P\varphi_{ref}$：

积分 $w=k_i\int(k_x(x_{ref}-x)-\dot x)\,dt=k_i\bigl(k_xz-(x-x_0)\bigr)$（$z$ 就是上一节 LQI 的 $z$）。整理成 $\tau=-K\,[x-x_{ref},\ \dot x,\ \varphi,\ \dot\varphi,\ z]^T$：

（差一个只取决于 $x_{ref}-x_0$ 的常值力矩，tilt 和 push 里它是 0，阶跃里它等价于积分器的初值不同。）这几个等式在代码里就是 cascade_equivalent，五个增益换成 $K$ 的五个分量：

1
def cascade_equivalent(g: dict) -> list[float]:
2
    # ...
3
    p, d = g["p"], g["d"]
4
    return [
5
        -p * (g["kv"] * g["kx"] + g["ki"]),
6
        -p * g["kv"],
7
        -p,
8
        -d,
9
        p * g["ki"] * g["kx"],
10
    ]

一串 PID 就是一个状态反馈，五个增益对应 $K$ 的五个分量。 这是“PID 是 LQI 的一个受限子集”这句话的准确含义。受限在哪？五个参数 $(P,D,k_v,k_x,k_i)$ 对五个分量，看起来够，但中间有一个二次方程：已知一个目标 $K$，$P=-K_\varphi$，$D=-K_{\dot\varphi}$，$k_v=K_{\dot x}/K_\varphi$ 直接得到；剩下 $K_x$ 和 $K_z$ 要求

没有实根的时候，这个 $K$ 就是任何一组串级增益都够不着的。 判别式 $\alpha^2-4k_v\beta$ 是 LQI 的 $K$ 能不能被串级复现的判据。LQI 的 $K$ 是 $(-46.34,\ -45.82,\ -150.39,\ -25.54,\ 19.34)$，算出来 $\alpha=0.3082$，$\beta=0.1286$，$k_v=0.3047$，判别式是 $-0.0618<0$。没有实根。离它最近的串级取判别式的顶点，$k_x=\alpha/(2k_v)=0.5057$，$k_i=\alpha-k_vk_x=0.1541$，这样前四个分量和 LQI 完全一样，$K_z$ 只有 11.72（LQI 是 19.34）。nearest_cascade 就是在做这个判断：

1
def nearest_cascade(k: list[float]) -> dict:
2
    # ...
3
    p, d = -k[2], -k[3]
4
    kv = k[1] / k[2]
5
    alpha = k[0] / k[2]
6
    beta = -k[4] / k[2]
7
    disc = alpha * alpha - 4.0 * kv * beta
8
    kx = (
9
        (alpha + math.sqrt(disc)) / (2.0 * kv)
10
        if disc >= 0.0
11
        else (alpha / (2.0 * kv))
12
    )
13
    return {"kx": kx, "kv": kv, "ki": alpha - kv * kx, "p": p, "d": d}

整定。 五个增益我交给 scipy.optimize.minimize(method="Nelder-Mead")，目标是 $\sum(x^TQx+R\tau^2)\,t_s$ 在 step 算例里的值（和 LQR 同一个代价，状态取相对参考的偏差），从上面这组“最接近 LQI”的增益出发，五个增益都限制非负，maxiter=4000。优化过程中仿真用每个采样一步 RK4（正式的仿真是每个采样 10 步）。关键就是这一段：

1
    x0 = [start[n] for n in names]
2
    res = minimize(
3
        cost,
4
        x0,
5
        method="Nelder-Mead",
6
        bounds=[(0.0, None)] * 5,  # no negative gains: no positive feedback
7
        options={"maxiter": 4000},
8
    )

把算出来的增益写进 data/ch09_cascade_gains.csv，三个原生程序从里面读，不自己重整定。

仿真：同一个循环，三个控制器#

LQR 和 LQI 是同一个类，integral 标志区分：

1
class StateFeedback:
2
    """u = -K (x - x_ref e0) (- k_z z): LQR when k_z is 0, LQI otherwise."""
3

4
    def __init__(self, k: list[float], integral: bool) -> None:
5
        self.k = k
6
        self.integral = integral
7
        self.z = 0.0
8
        self.lim = Wip().tau_max
9

10
    def update(self, x: list[float], xref: float) -> float:
11
        xt = [x[0] - xref, x[1], x[2], x[3]]
12
        s = 0.0
13
        for i in range(4):
14
            s += self.k[i] * xt[i]
15
        if self.integral:
16
            s += self.k[4] * self.z
17
        v = -s
18
        u = min(max(v, -self.lim), self.lim)
19
        if self.integral and u == v:
20
            self.z -= TS * xt[0]
21
        return u

u == v 就是“没有饱和”，只有没饱和的时候才积分，这是冻结 $z$ 的全部实现。仿真函数本身：

1
def simulate(
2
    name: str,
3
    gains: dict,
4
    data: dict,
5
    substeps: int = SUBSTEPS,
6
) -> dict[str, np.ndarray]:
7
    plant = Wip()
8
    plant.m *= float(data["mscale"][0])
9
    ctl = make_controller(name, gains)
10
    x = [0.0, 0.0, float(data["phi0"][0]), 0.0]
11
    rows = []
12
    for k in range(len(data["t"])):
13
        tau = ctl.update(x, data["xref"][k])
14
        rows.append([data["t"][k], *x, tau])
15
        x = advance(plant.deriv, x, [tau, data["force"][k]], TS, substeps)
16
        if abs(x[2]) > 3.0:  # fallen over: freeze (the loop is not a model
17
            x[2] = math.copysign(3.0, x[2])  # of lying on the floor)
18
            x[3] = 0.0
19
    return dict(zip(COLUMNS, np.array(rows, dtype=float).T))

循环顺序和第 1 篇定的约定一样：先用当前状态算控制量，写一行记录（tau 是限幅后实际作用的力矩），再保持力矩和外力不变积分 10 个 RK4 步。mscale 只放大对象的质量，控制器仍然按 $M=12$ kg 设计，mass130 算例就靠它。倾角超过 3 rad 的时候把它冻在 3 rad 上，免得倒下去之后的数值没有意义，本章所有算例都没有走到这一步（report 断言了 $|\varphi|<0.5$）。

四个算例（tilt、step、push、mass130）和它们的参考、外力、初始倾角、质量比例都由 Python 的 scenario_table 写进 data/ch09_<case>.csv，三个原生程序只读。

C、C++、Rust 实现#

两个新模块和新对象是 Python 版的逐行翻译，三种语言的 ch09 应用也是 Python 驱动的逐行翻译：同样的算例表、同样的接线，读同一份数据，自己算 LQR 和 LQI 的 $K$，写进 gains_<lang>.csv 和 Python 的对比。区别在这几处：

• 矩阵的存放。 C 是平铺的行优先数组加维数参数：pl_mat_mul(a, b, n, k, m, out)，调用者提供所有数组，上限 PL_MAX_DIM（8），输出不能和输入重叠；超过上限的 pl_expm、pl_c2d_zoh、pl_dare_iterate 返回 -EINVAL，不收敛返回 -ERANGE。C++ 是 Mat<T, R, C>（std::array<std::array<T, C>, R>）加模板，维数在类型里，写错了编译不过。Rust 是 [[Real; C]; R] 加 const 泛型。三种语言都没有堆分配（csv.c 除外）。
• 增广一维。 c2d_zoh 要在 $n$ 阶系统上做 $(n+1)$ 阶的指数，lqi_augment 要把 $n$ 阶变成 $n+1$ 阶。C 的数组长度是运行时的，没有问题。C++ 可以直接写 Mat<T, N + 1, N + 1>。Rust 稳定版不能在类型里写 N + 1（generic_const_exprs 还没稳定），所以这两个函数多一个 const 参数 M，调用的时候写 c2d_zoh::<4, 5>，函数内部检查 M == N + 1，不对就返回 None。我没有找到更干净的写法。
• 失败的处理。 dare_iterate：C 是 -ERANGE，C++ 是 std::optional<DareResult<T, N>>（结果里有 $P$ 和迭代次数），Rust 是 Option<(Mat<N, N>, usize)>，Python 抛 RuntimeError。
• 能量自检。 三种语言的 ch09 应用在做任何别的事之前，先跑 $\tau=0$、$F=0$、没有阻尼的 2 s 自由落体（$\varphi_0=0.3$，RK4，$h=10^{-4}$），相对能量漂移超过 $10^{-6}$ 就退出码 1。每一步都检查，不只是写进文件的那 201 行。
• 标量类型。 这一篇仍是 double（pl_real、T、Real），但 mat.c 和 lqr.c 里的字面量我都写成了跟着 pl_real 走的形式（HALF、ONE、DARE_TOL 三个宏），因为我想看它们在 -DPL_USE_FLOAT -Wdouble-promotion 下过不过。下面“编译和风格检查”里说结果。

1
void pl_mat_mul(const pl_real *a, const pl_real *b, int n, int k, int m,
2
    pl_real *out)
3
{
4
  int i, j, p;
5

6
  for (i = 0; i < n; i++) {
7
    for (j = 0; j < m; j++) {
8
      pl_real s = 0.0;
9

10
      for (p = 0; p < k; p++)
11
        s += a[i * k + p] * b[p * m + j];
12
      out[i * m + j] = s;
13
    }
14
  }
15
}

1
int pl_expm(const pl_real *a, int n, pl_real *out)
2
{
3
  pl_real h[MAT_SIZE], term[MAT_SIZE], tmp[MAT_SIZE];
4
  pl_real nrm = 0.0, f = ONE;
5
  int squarings = 0, i, j, k;
6

7
  if (n < 1 || n > PL_MAX_DIM)
8
    return -EINVAL;
9
  for (i = 0; i < n; i++) {
10
    pl_real row = 0.0;
11

12
    for (j = 0; j < n; j++)
13
      row += PL_FABS(a[i * n + j]);
14
    if (row > nrm)
15
      nrm = row;
16
  }
17
  while (nrm > HALF) {
18
    nrm *= HALF;
19
    f *= HALF;
20
    squarings++;
21
  }
22
  pl_mat_scale(a, f, n, n, h);
23
  pl_mat_identity(n, term);
24
  pl_mat_identity(n, out);
25
  for (k = 1; k <= TAYLOR_ORDER; k++) {
26
    pl_mat_mul(term, h, n, n, n, tmp);
27
    pl_mat_scale(tmp, 1.0 / k, n, n, term);
28
    pl_mat_add(out, term, n, n, tmp);
29
    for (i = 0; i < n * n; i++)
30
      out[i] = tmp[i];
31
  }
32
  for (k = 0; k < squarings; k++) {
33
    pl_mat_mul(out, out, n, n, n, tmp);
34
    for (i = 0; i < n * n; i++)
35
      out[i] = tmp[i];
36
  }
37
  return 0;
38
}

1
/* One step of the stabilized Riccati map; returns max|P+ - P|. */
2
static pl_real dare_step(const pl_real *ad, const pl_real *bd,
3
       const pl_real *q, pl_real r, int n,
4
       const pl_real *cur, pl_real *nxt)
5
{
6
  pl_real k[PL_MAX_DIM], acl[MAT_SIZE], t1[MAT_SIZE], t2[MAT_SIZE];
7
  pl_real t3[MAT_SIZE];
8

9
  pl_lqr_gain(ad, bd, cur, r, n, k);
10
  pl_mat_mul(bd, k, n, 1, n, t1);
11
  pl_mat_sub(ad, t1, n, n, acl); /* A - BK */
12
  pl_mat_transpose(k, 1, n, t1);
13
  pl_mat_mul(t1, k, n, 1, n, t2);
14
  pl_mat_scale(t2, r, n, n, t3);
15
  pl_mat_add(q, t3, n, n, t1); /* Q + K'RK */
16
  pl_mat_mul(cur, acl, n, n, n, t2);
17
  pl_mat_transpose(acl, n, n, t3);
18
  pl_mat_mul(t3, t2, n, n, n, acl);
19
  pl_mat_add(t1, acl, n, n, nxt);
20
  pl_mat_sub(nxt, cur, n, n, t1);
21
  return pl_mat_max_abs(t1, n, n);
22
}

1
int pl_dare_iterate(const pl_real *ad, const pl_real *bd, const pl_real *q,
2
        pl_real r, int n, pl_real *p, int *iters)
3
{
4
  pl_real cur[MAT_SIZE], nxt[MAT_SIZE], diff;
5
  int it, i;
6

7
  if (n < 1 || n > PL_MAX_DIM)
8
    return -EINVAL;
9
  for (i = 0; i < n * n; i++)
10
    cur[i] = q[i];
11
  for (it = 1; it <= PL_DARE_MAX_ITER; it++) {
12
    diff = dare_step(ad, bd, q, r, n, cur, nxt);
13
    for (i = 0; i < n * n; i++)
14
      cur[i] = nxt[i];
15
    if (diff < DARE_TOL * (ONE + pl_mat_max_abs(cur, n, n))) {
16
      for (i = 0; i < n * n; i++)
17
        p[i] = cur[i];
18
      *iters = it;
19
      return 0;
20
    }
21
  }
22
  return -ERANGE;
23
}

1
void pl_wip_deriv(const void *ctx, const pl_real *x, const pl_real *u,
2
      pl_real *dx)
3
{
4
  const struct pl_wip *p = ctx;
5
  pl_real a = p->mw + p->iw / (p->r * p->r) + p->m;
6
  pl_real c = p->ib + p->m * p->l * p->l;
7
  pl_real ml = p->m * p->l;
8
  pl_real s = PL_SIN(x[2]);
9
  pl_real co = PL_COS(x[2]);
10
  pl_real r1 = u[0] / p->r + u[1] + ml * s * x[3] * x[3];
11
  pl_real r2 = -u[0] + u[1] * p->l * co + ml * p->g * s;
12
  pl_real b = ml * co;
13
  pl_real det = a * c - b * b;
14

15
  dx[0] = x[1];
16
  dx[1] = (c * r1 - b * r2) / det;
17
  dx[2] = x[3];
18
  dx[3] = (a * r2 - b * r1) / det;
19
}