控制论：从经典反馈到现代状态空间（最优控制）

第四部分：最优控制

第十章：线性二次型调节器（LQR）

10.1 最优控制问题

10.1.1 问题形式化

给定线性系统：

$$\dot{x}=Ax+Bu$$

寻找控制律 $u(t)$，使得性能指标最小化：

$$J={\int }_0^{\infty }[x^T(t)Qx(t)+u^T(t)Ru(t)]dt+x^T(T_f)Sx(T_f)$$

其中：

• $Q\ge 0$：状态权重矩阵（惩罚状态偏差）
• $R>0$：控制权重矩阵（惩罚控制能量）
• $S\ge 0$：终端状态权重
• $T_f$：终端时间（$T_f=\infty$ 为无限时间问题）

10.1.2 物理意义

性能指标 $J$ 是两部分的权衡：

• $x^{T}Qx$：惩罚状态偏离零（调节性能）
• $u^{T}Ru$：惩罚控制能量（执行成本）

$Q$ 和 $R$ 的选择反映了设计者对"调节精度"和"控制能量"的偏好。

10.2 LQR 的解

10.2.1 有限时间 LQR

定理 10.1：有限时间 LQR 问题的最优控制律为状态反馈：

$$u^{\ast }(t)=-K(t)x(t)$$

其中反馈增益为：

$$K(t)=R^{-1}{B}^{T}P(t)$$

$P(t)$ 是以下微分 Riccati 方程的解：

$$-\dot{P}=A^{T}P+PA-PB{R}^{-1}{B}^{T}P+Q,P(T_f)=S$$

10.2.2 无限时间 LQR

定理 10.2：当 $T_f\to \infty$ 且系统 $(A,B)$ 能控、$(A,Q^{1/2})$ 能观时，$P(t)$ 收敛到常数矩阵 $P$，它是以下**代数 Riccati 方程（ARE）**的唯一正定解：

$$A^{T}P+PA-PB{R}^{-1}{B}^{T}P+Q=0$$

最优控制律为时不变的状态反馈：

$$u^{\ast }(t)=-Kx(t),K=R^{-1}{B}^{T}P$$

10.2.3 LQR 的稳定性

定理 10.3：LQR 闭环系统 $\dot{x}=(A-BK)x$ 是渐近稳定的。

证明：取 Lyapunov 函数 $V(x)=x^{T}Px$（$P>0$），则：

$$\dot{V}=x^T[(A-BK{)}^{T}P+P(A-BK)]x=-x^T(Q+K^{T}RK)x<0$$

由 Lyapunov 定理，闭环系统渐近稳定。$\square$

10.3 LQR 的性质

10.3.1 最优性条件

LQR 的最优性由 Hamilton-Jacobi-Bellman (HJB) 方程保证：

$$\underset{u}{\min }\left[x^{T}Qx+u^{T}Ru+\frac{\partial V}{\partial x}(Ax+Bu)\right]=0$$

代入 $V(x)=x^{T}Px$，可以推导出 ARE。

10.3.2 频域解释

定理 10.4（LQR 的回路传递恢复）：LQR 闭环系统在每个输入端口的开环传递函数满足：

$$|1+K(j\omega I-A{)}^{-1}B|\ge 1\forall \omega$$

这意味着 LQR 保证了至少 60° 的相位裕度和无穷大的增益裕度——LQR 具有内在的鲁棒性。

10.3.3 $Q$ 和 $R$ 的选择

• $Q=I$：等权惩罚所有状态
• $Q=C^{T}C$：惩罚输出（物理上可测量的量）
• $R=\rho I$：$\rho$ 大意味着更注重节省控制能量，$\rho$ 小意味着更注重调节精度

Bryson 法则：令 $Q_{ii}=1/x_{i,\max }^2$，$R_{jj}=1/u_{j,\max }^2$，其中 $x_{i,\max }$ 和 $u_{j,\max }$ 是可接受的最大值。

10.4 LQR 的计算

10.4.1 直接求解 ARE

对于小规模系统，可以直接求解 ARE：

$$A^{T}P+PA-PB{R}^{-1}{B}^{T}P+Q=0$$

方法：

• Schur 分解法
• 矩阵符号函数法
• 迭代法

10.4.2 Python 实现

使用 scipy.linalg.solve_continuous_are 可以直接求解。

第十一章：线性二次高斯控制（LQG）

11.1 从 LQR 到 LQG

LQR 假设所有状态都可直接测量，但实际中并非如此。LQG（Linear Quadratic Gaussian） 将 LQR 与 Kalman 滤波结合：

11.1.1 问题设置

$$\dot{x}=Ax+Bu+w,w\sim \mathcal{N}(0,Q_n)$$
$$y=Cx+v,v\sim \mathcal{N}(0,R_n)$$

最小化：

$$J=\underset{T\to \infty }{\lim }\frac{1}{T}\mathbb{E}\left[{\int }_0^T(x^{T}Qx+u^{T}Ru)dt\right]$$

11.2 分离原理

定理 11.1（LQG 分离原理）：LQG 的最优控制律为：

$$u^{\ast }=-K\hat{x}$$

其中 $K$ 是 LQR 最优增益，$\hat{x}$ 是 Kalman 滤波器的状态估计。

控制器设计和估计器设计完全解耦——这就是分离原理。

11.3 LQG 的性质

11.3.1 确定等价原理

LQG 的最优控制律与确定性 LQR 的形式相同，只是用估计状态代替真实状态——这称为确定等价原理（Certainty Equivalence）。

11.3.2 鲁棒性问题

虽然 LQG 在数学上是最优的，但它的鲁棒性不如 LQR。这是因为 Kalman 滤波器的引入可能降低系统的稳定裕度。

这促使了 $H_{\infty }$ 控制理论的发展——在设计时直接考虑模型不确定性。

第十二章：模型预测控制（MPC）

12.1 MPC 的基本思想

12.1.1 滚动优化

模型预测控制（Model Predictive Control, MPC） 的核心思想是：

在每个时刻，基于当前状态和系统模型，求解一个有限时域的最优控制问题，只执行第一步控制，然后在下一时刻重复这个过程。

这就是"滚动优化"（receding horizon）。

12.1.2 MPC 的数学形式

在时刻 $k$，求解：

$$\underset{u_k,\dots ,u_{k+N-1}}{\min }\sum _{i=0}^{N-1}[x_{k+i}^{T}Q{x}_{k+i}+u_{k+i}^{T}R{u}_{k+i}]+x_{k+N}^{T}P{x}_{k+N}$$

约束条件：

$$x_{k+i+1}=A{x}_{k+i}+B{u}_{k+i}$$
$$x_{\min }\le x_{k+i}\le x_{\max }$$
$$u_{\min }\le u_{k+i}\le u_{\max }$$

只执行 $u_k^{\ast }$，在 $k+1$ 时刻重新求解。

12.2 MPC 的优势

12.2.1 处理约束

MPC 是唯一能系统地处理约束的控制方法。这在实际工程中极为重要：

• 执行器有物理限制（阀门开度、电机转速）
• 状态有安全限制（温度不能过高、位置不能超出范围）

12.2.2 多变量处理

MPC 自然地处理多输入多输出（MIMO）系统，不需要解耦。

12.2.3 前馈能力

如果可以预测未来的参考信号或干扰，MPC 可以提前做出反应。

12.3 MPC 的计算

12.3.1 无约束 MPC

无约束 MPC 可以解析求解，等价于一个时变状态反馈律。

12.3.2 有约束 MPC

有约束 MPC 需要求解二次规划（QP）：

$$\underset{z}{\min }\frac{1}{2}{z}^{T}Hz+f^{T}z$$
$$\text{s.t.}Gz\le b$$

现代 QP 求解器可以在毫秒级时间内求解中等规模的问题。

12.3.3 显式 MPC

对于小规模系统，可以离线计算所有可能状态区域的最优控制律，得到分段仿射（PWA）的显式解。

12.4 MPC 与最优控制的关系

MPC 可以看作有限时域的 LQR 加约束。当预测时域 $N\to \infty$ 且无约束时，MPC 收敛到 LQR。

MPC 的"滚动优化"思想也与强化学习中的"模型预测"方法有深刻的联系。

第五部分：与机器学习的联系

第十三章：控制论视角下的序列建模

13.1 序列建模作为控制问题

从控制论的角度，序列建模可以被重新表述为：

给定输入序列 $x_0,x_1,\dots ,x_T$，设计一个动态系统（控制器），使得输出 $y_t$ 尽可能接近期望的目标 $y_t^{\ast }$。

这个视角揭示了序列建模与控制论之间的深层联系：

13.2 SSM 的控制论解释

13.2.1 稳定性与记忆

SSM 的状态转移矩阵 $\bar{A}$ 的特征值决定了系统的"记忆"行为：

• $|\bar{\lambda }|<1$：模态衰减，信息随时间流失
• $|\bar{\lambda }|\approx 1$：模态持久，信息长期保持
• $|\bar{\lambda }|>1$：模态发散，系统不稳定

HiPPO 理论的本质是：选择最优的 $A$ 矩阵，使得系统在"记住重要信息"和"忘掉无关信息"之间达到最优平衡。

这与控制论中的稳定性-性能权衡一脉相承：

• 太稳定（衰减太快）：丢失长程信息
• 太不稳定（衰减太慢）：无法区分新旧信息

13.2.2 能控性与表达能力

SSM 的能控性决定了模型能否将任意输入映射到任意隐状态：

• 如果 SSM 完全能控，它可以将输入信息编码到隐状态的任意方向
• 能控性矩阵的秩反映了模型可以影响的状态空间维度

定理 13.1：对于 SSM $h_t=\bar{A}{h}_{t-1}+\bar{B}{x}_t$，如果 $(\bar{A},\bar{B})$ 完全能控，则模型可以将任意长度为 $n$ 的输入序列映射到 ${\mathbb{R}}^N$ 中的任意状态。

13.2.3 能观性与信息提取

SSM 的能观性决定了模型能否从输出中提取所有隐状态的信息：

• 如果 SSM 完全能观，输出 $y_t=C{h}_t$ 包含了隐状态的全部信息
• 能观性矩阵的秩反映了可以从输出中恢复的状态空间维度

13.3 选择性机制的控制论解释

13.3.1 自适应控制的视角

Mamba 的选择性机制（输入依赖的 $\Delta$, $B$, $C$）可以理解为一种自适应控制：

• 系统参数根据输入内容实时调整
• 这使得模型能够根据上下文动态调整"记忆策略"

在控制论中，自适应控制器根据系统运行状况调整参数，以应对未知或时变的系统特性。Mamba 的选择性机制做的是同样的事情——根据输入内容调整"系统参数"（$\Delta$, $B$, $C$），以最优地压缩历史信息。

13.3.2 切换系统的视角

选择性 SSM 可以被理解为一个切换系统（switched system）——在每个时间步，系统在不同的"模式"之间切换：

$$h_t={\bar{A}}_t{h}_{t-1}+{\bar{B}}_t{x}_t$$

其中 ${\bar{A}}_t$, ${\bar{B}}_t$ 由输入 $x_t$ 决定。

切换系统的稳定性分析比 LTI 系统复杂得多——即使每个单独的模式都是稳定的，切换也可能导致不稳定。这解释了为什么选择性 SSM 需要精心设计（如 $A$ 保持固定，只有 $\Delta$, $B$, $C$ 变化）。

13.3.3 最优控制的视角

从最优控制的角度，选择性机制可以理解为在每个时刻求解一个"局部最优控制问题"：

给定当前状态 $h_{t-1}$ 和输入 $x_t$，选择 ${\Delta }_t$, $B_t$, $C_t$ 使得输出 $y_t$ 最接近期望值。

这类似于 MPC 的滚动优化思想——在每个时刻做出局部最优决策。

13.4 Kalman 滤波器与 SSM 训练

13.4.1 EM 算法

SSM 的训练可以与 Kalman 滤波器的期望最大化（EM）算法类比：

• E 步（前向传播）：给定当前参数，用 Kalman 滤波器计算状态的后验估计
• M 步（参数更新）：给定状态估计，更新系统参数 $(A,B,C)$ 以最大化似然

在深度 SSM 中，前向传播类似于 E 步（用当前参数计算隐状态），反向传播类似于 M 步（根据梯度更新参数）。

13.4.2 信息滤波器

Kalman 滤波器的对偶形式——信息滤波器——使用信息矩阵 $\Lambda =P^{-1}$ 和信息向量 $\eta =\Lambda x$：

$${\Lambda }_{k|k}={\Lambda }_{k|k-1}+C^T{R}^{-1}C$$
$${\eta }_{k|k}={\eta }_{k|k-1}+C^T{R}^{-1}{y}_k$$

这与 Transformer 的注意力机制有相似之处——信息矩阵的更新类似于注意力权重的累积。

第十四章：自适应控制与在线学习

14.1 自适应控制的基本问题

14.1.1 问题设置

当被控对象的参数未知或时变时，需要控制器在线调整以适应变化：

$$\dot{x}=A(\theta )x+B(\theta )u$$

其中 $\theta$ 是未知参数。

14.1.2 自适应控制的方法

14.2 在线学习与自适应控制

14.2.1 在线凸优化

自适应控制可以被形式化为在线凸优化问题：

在每一轮 $t$：

目标：最小化遗憾（regret）：

$$R_T=\sum _{t=1}^T{\ell }_t(w_t)-\underset{w}{\min }\sum _{t=1}^T{\ell }_t(w)$$

在线梯度下降（OGD）实现了 $O(\sqrt{T})$ 的遗憾界。

14.2.2 SSM 的在线学习

SSM 的递推更新 $h_t=\bar{A}{h}_{t-1}+\bar{B}{x}_t$ 可以被看作一种特殊的在线学习算法：

• 隐状态 $h_t$ 是"模型参数"
• 输入 $x_t$ 是"数据"
• 状态转移 $\bar{A}$ 是"正则化"（防止参数变化太快）
• 输入矩阵 $\bar{B}$ 是"学习率"（控制新数据的影响）

选择性机制中的 ${\Delta }_t$ 控制了"学习率"的大小——大 $\Delta$ 意味着快速学习（忘掉旧信息，关注新输入），小 $\Delta$ 意味着保守学习（保持旧信息，忽略新输入）。

14.3 鲁棒自适应控制

14.3.1 参数不确定性的处理

实际系统中，参数估计总有误差。鲁棒自适应控制的目标是：

即使参数估计不完美，也要保证系统的稳定性和性能。

方法：

• σ-修正：在自适应律中加入泄漏项，防止参数漂移
• 死区（dead-zone）：当误差很小时停止自适应
• 投影算法：将参数约束在已知的有界集合内

14.3.2 与 SSM 训练的联系

SSM 训练中的梯度下降可以被看作一种自适应控制：

• 参数 $\theta =(A,B,C,\Delta )$ 是被控量
• 损失函数 $\mathcal{L}$ 是性能指标
• 梯度 ${\nabla }_{\theta }\mathcal{L}$ 是控制信号
• 学习率 $\eta$ 是控制增益

训练的稳定性（不发散）对应于控制系统的稳定性。学习率调度对应于增益调度。

第十五章：强化学习与最优控制的统一

15.1 最优控制与强化学习的共同框架

15.1.1 MDP 框架

最优控制和强化学习都可以用**马尔可夫决策过程（MDP）**来统一描述：

MDP 由 $(S,A,P,R,\gamma )$ 定义：

• $S$：状态空间
• $A$：动作空间
• $P(s^{\prime }|s,a)$：状态转移概率
• $R(s,a)$：奖励函数
• $\gamma$：折扣因子

15.1.2 最优控制作为 MDP

确定性 LQR 可以写成 MDP 的形式：

• 状态 $s_t=x_t$
• 动作 $a_t=u_t$
• 转移 $s_{t+1}=A{s}_t+B{a}_t$（确定性）
• 奖励 $r_t=-(x_t^{T}Q{x}_t+u_t^{T}R{u}_t)$
• 折扣因子 $\gamma =1$（无限时域）或 $\gamma <1$

15.2 动态规划与 HJB 方程

15.2.1 值函数

定义值函数 $V(x)$：从状态 $x$ 出发，遵循最优策略所能获得的累积奖励。

Bellman 最优方程：

$$V(x)=\underset{a}{\max }[R(x,a)+\gamma \mathbb{E}[V(x^{\prime })|x,a]]$$

15.2.2 HJB 方程

对于连续时间系统 $\dot{x}=f(x,u)$，HJB 方程为：

$$0=\underset{u}{\min }[r(x,u)+{\nabla }_{x}V(x)\cdot f(x,u)]$$

对于 LQR（$f=Ax+Bu$, $r=x^{T}Qx+u^{T}Ru$），解为 $V(x)=x^{T}Px$，推导出 ARE。

15.2.3 策略梯度

强化学习中的策略梯度方法：

$${\nabla }_{\theta }J(\theta )=\mathbb{E}\left[\sum _{t=0}^T{\nabla }_{\theta }\log {\pi }_{\theta }(a_t|s_t)\cdot A^{\pi }(s_t,a_t)\right]$$

其中 $A^{\pi }$ 是优势函数。这与最优控制中的协态方程（costate equation）有深刻联系。

15.3 神经网络用于控制

15.3.1 神经网络作为函数逼近器

深度强化学习用神经网络逼近值函数或策略：

• 值函数逼近：$V_{\theta }(s)\approx V^{\ast }(s)$
• 策略逼近：${\pi }_{\theta }(a|s)\approx {\pi }^{\ast }(a|s)$

15.3.2 神经 ODE 与控制

神经常微分方程（Neural ODE）：

$$\dot{x}=f_{\theta }(x,t)$$

将神经网络作为连续时间动态系统，这直接连接了深度学习和控制论。训练 Neural ODE 等价于求解一个最优控制问题。

15.3.3 SSM 与控制的统一视角

SSM（如 Mamba）可以被理解为一种学习到的动态系统：

• 参数 $(A,B,C)$ 通过学习得到
• 选择性机制是学习到的自适应控制律
• 训练过程是最优控制问题的数值求解

这个统一视角揭示了：深度学习和控制论不是两个独立的领域，而是同一个数学框架的不同表现形式。

15.4 世界模型与控制

15.4.1 世界模型的概念

世界模型（World Model） 是对环境动态的学习到的模型：

$$s_{t+1}=f_{\theta }(s_t,a_t)$$

它允许智能体在"想象"中规划和决策，而不需要与真实环境交互。

15.4.2 SSM 作为世界模型

SSM 天然适合作为世界模型的核心组件：

• 状态表示：$h_t$ 编码了环境的历史信息
• 动态预测：$h_{t+1}=\bar{A}{h}_t+\bar{B}{a}_t$ 预测下一个状态
• 观测生成：$y_t=C{h}_t$ 生成观测

Dreamer（Hafner et al., 2020）等模型已经在使用类似 SSM 的结构作为世界模型。

第十六章：完整可运行代码实现

16.1 经典控制系统仿真

"""
经典控制系统的完整仿真。
包含: 传递函数、阶跃响应、Bode 图、根轨迹。
"""

import numpy as np
from scipy import signal
import matplotlib.pyplot as plt

class TransferFunction:
"""传递函数的简单表示和仿真。"""

def __init__(self, num, den):
"""
Args:
num: 分子多项式系数 (高次到低次)
den: 分母多项式系数 (高次到低次)
"""
self.num = np.array(num, dtype=float)
self.den = np.array(den, dtype=float)
self.system = signal.TransferFunction(self.num, self.den)

def poles(self):
"""返回极点。"""
return np.roots(self.den)

def zeros(self):
"""返回零点。"""
return np.roots(self.num)

def is_stable(self):
"""判断是否稳定（所有极点在左半平面）。"""
return all(np.real(self.poles()) < 0)

def step_response(self, T=None):
"""计算阶跃响应。"""
t, y = signal.step(self.system, T=T)
return t, y

def impulse_response(self, T=None):
"""计算脉冲响应。"""
t, y = signal.impulse(self.system, T=T)
return t, y

def bode(self, omega=None):
"""计算 Bode 图数据。"""
if omega is None:
omega = np.logspace(–2, 2, 1000)
w, mag, phase = signal.bode(self.system, omega)
return w, mag, phase

def __mul__(self, other):
"""串联。"""
num = np.convolve(self.num, other.num)
den = np.convolve(self.den, other.den)
return TransferFunction(num, den)

def feedback(self, H=None, sign=–1):
"""闭环反馈。"""
if H is None:
H = TransferFunction([1], [1]) # 单位反馈

# T = G / (1 + G*H)
GH_num = np.convolve(self.num, H.num)
GH_den = np.convolve(self.den, H.den)

if sign == –1:
den_new = np.polyadd(GH_den, GH_num)
else:
den_new = np.polysub(GH_den, GH_num)

return TransferFunction(GH_num, den_new)

def demonstrate_second_order():
"""演示二阶系统的阶跃响应。"""
print("=" * 60)
print("二阶系统阶跃响应演示")
print("=" * 60)

wn = 2.0 # 自然频率
zeta_list = [0.1, 0.3, 0.5, 0.707, 1.0, 1.5]

results = []
for zeta in zeta_list:
# G(s) = wn^2 / (s^2 + 2*zeta*wn*s + wn^2)
G = TransferFunction([wn**2], [1, 2*zeta*wn, wn**2])
t, y = G.step_response(T=np.linspace(0, 5, 500))

# 计算性能指标
y_final = y[–1]
overshoot = max(0, (max(y) – y_final) / y_final * 100)

# 调节时间 (2%)
idx = np.where(np.abs(y – y_final) > 0.02 * y_final)[0]
ts = t[idx[–1]] if len(idx) > 0 else 0

results.append({
'zeta': zeta,
'overshoot': overshoot,
'settling_time': ts,
'peak': max(y),
})

print(f" ζ = {zeta:.3f}: 超调 = {overshoot:.1f}%, "
f"调节时间 = {ts:.2f}s, 峰值 = {max(y):.4f}")

return results

def demonstrate_pid_tuning():
"""演示 PID 控制器参数整定。"""
print("\\n" + "=" * 60)
print("PID 控制器整定演示")
print("=" * 60)

# 被控对象: G(s) = 1 / (s^2 + 2s + 1)
G = TransferFunction([1], [1, 2, 1])

# 不同 PID 参数
pid_params = [
{'name': 'P only', 'Kp': 5.0, 'Ki': 0.0, 'Kd': 0.0},
{'name': 'PI', 'Kp': 5.0, 'Ki': 2.0, 'Kd': 0.0},
{'name': 'PD', 'Kp': 5.0, 'Ki': 0.0, 'Kd': 1.0},
{'name': 'PID', 'Kp': 5.0, 'Ki': 2.0, 'Kd': 1.0},
]

for params in pid_params:
Kp, Ki, Kd = params['Kp'], params['Ki'], params['Kd']

# PID 控制器: C(s) = (Kd*s^2 + Kp*s + Ki) / s
if Ki > 0:
C_num = [Kd, Kp, Ki]
C_den = [1, 0]
else:
C_num = [Kd, Kp]
C_den = [1]

C = TransferFunction(C_num, C_den)
T = C.feedback(G) # 闭环

if T.is_stable():
t, y = T.step_response(T=np.linspace(0, 10, 500))
overshoot = max(0, (max(y) – 1) * 100)
print(f" {params['name']:8s}: 稳定, 超调 = {overshoot:.1f}%")
else:
print(f" {params['name']:8s}: 不稳定!")

if __name__ == "__main__":
demonstrate_second_order()
demonstrate_pid_tuning()

16.2 状态空间与 LQR

"""
状态空间系统的完整实现。
包含: 能控性/能观性检验、极点配置、LQR 设计。
"""

import numpy as np
from scipy.linalg import solve_continuous_are, eigvals

class StateSpaceSystem:
"""连续时间状态空间系统。"""

def __init__(self, A, B, C, D=None):
self.A = np.array(A, dtype=float)
self.B = np.array(B, dtype=float)
self.C = np.array(C, dtype=float)
if D is None:
self.D = np.zeros((C.shape[0], B.shape[1]))
else:
self.D = np.array(D, dtype=float)
self.n = A.shape[0]

def controllability_matrix(self):
"""构造能控性矩阵。"""
n = self.n
m = self.B.shape[1]
C_mat = np.zeros((n, n * m))
AB_power = np.eye(n)
for i in range(n):
C_mat[:, i*m:(i+1)*m] = AB_power @ self.B
AB_power = AB_power @ self.A
return C_mat

def observability_matrix(self):
"""构造能观性矩阵。"""
n = self.n
p = self.C.shape[0]
O_mat = np.zeros((p * n, n))
CA_power = self.C.copy()
for i in range(n):
O_mat[i*p:(i+1)*p, :] = CA_power
CA_power = CA_power @ self.A
return O_mat

def is_controllable(self):
"""判断是否完全能控。"""
C_mat = self.controllability_matrix()
return np.linalg.matrix_rank(C_mat) == self.n

def is_observable(self):
"""判断是否完全能观。"""
O_mat = self.observability_matrix()
return np.linalg.matrix_rank(O_mat) == self.n

def is_stable(self):
"""判断是否稳定。"""
return all(np.real(eigvals(self.A)) < 0)

def simulate(self, u_func, x0, T, dt=0.001):
"""仿真系统响应。

Args:
u_func: 输入函数 u(t) -> array
x0: 初始状态
T: 仿真时长
dt: 时间步长

Returns:
t: 时间序列
x: 状态序列
y: 输出序列
"""
steps = int(T / dt)
t = np.linspace(0, T, steps)
x = np.zeros((steps, self.n))
y = np.zeros((steps, self.C.shape[0]))

x[0] = x0
y[0] = self.C @ x0 + self.D @ u_func(0)

for i in range(1, steps):
u = u_func(t[i–1])
dx = self.A @ x[i–1] + self.B @ u
x[i] = x[i–1] + dx * dt
y[i] = self.C @ x[i] + self.D @ u

return t, x, y

def transfer_function(self):
"""计算传递函数 G(s) = C(sI-A)^{-1}B + D（在特定频率点评估）。"""
# 返回 A 的特征值作为极点
return eigvals(self.A)

def lqr_design(A, B, Q, R):
"""设计 LQR 控制器。

Args:
A, B: 系统矩阵
Q: 状态权重矩阵
R: 控制权重矩阵

Returns:
K: 反馈增益矩阵
P: Riccati 方程的解
"""
# 求解代数 Riccati 方程: A^T P + PA – PBR^{-1}B^TP + Q = 0
P = solve_continuous_are(A, B, Q, R)

# 计算反馈增益: K = R^{-1} B^T P
K = np.linalg.solve(R, B.T @ P)

return K, P

def demonstrate_state_space():
"""演示状态空间系统的基本操作。"""
print("=" * 60)
print("状态空间系统演示")
print("=" * 60)

# 弹簧-阻尼器-质量系统
m, c, k = 1.0, 0.5, 2.0
A = np.array([[0, 1], [–k/m, –c/m]])
B = np.array([[0], [1/m]])
C = np.array([[1, 0]])
D = np.array([[0]])

sys = StateSpaceSystem(A, B, C, D)

print(f"\\n系统参数: m={m}, c={c}, k={k}")
print(f"状态维度: {sys.n}")
print(f"特征值: {eigvals(A)}")
print(f"稳定: {sys.is_stable()}")
print(f"能控: {sys.is_controllable()}")
print(f"能观: {sys.is_observable()}")

# 仿真阶跃响应
u_func = lambda t: 1.0 if t >= 0 else 0.0
t, x, y = sys.simulate(u_func, np.zeros(2), T=20, dt=0.01)

print(f"\\n阶跃响应:")
print(f" 终值: {y[–1, 0]:.4f}")
print(f" 超调: {max(0, (max(y[:, 0]) – 1) * 100):.1f}%")

return sys

def demonstrate_lqr():
"""演示 LQR 控制器设计。"""
print("\\n" + "=" * 60)
print("LQR 控制器设计演示")
print("=" * 60)

# 倒立摆线性化模型
M, m, l, g = 1.0, 0.1, 0.5, 9.8

# 状态: [x, x_dot, theta, theta_dot]
A = np.array([
[0, 1, 0, 0],
[0, 0, –m*g/M, 0],
[0, 0, 0, 1],
[0, 0, (M+m)*g/(M*l), 0]
])
B = np.array([[0], [1/M], [0], [–1/(M*l)]])
C = np.array([[1, 0, 0, 0]]) # 只测量位置

sys = StateSpaceSystem(A, B, C)

print(f"\\n倒立摆系统:")
print(f" 开环特征值: {np.sort_complex(eigvals(A))}")
print(f" 能控: {sys.is_controllable()}")
print(f" 能观: {sys.is_observable()}")

# LQR 设计
Q = np.diag([10, 1, 100, 1]) # 大权重惩罚角度偏差
R = np.array([[0.1]])

K, P = lqr_design(A, B, Q, R)
print(f"\\nLQR 反馈增益 K: {K.flatten()}")
print(f"闭环特征值: {np.sort_complex(eigvals(A – B @ K))}")

# 闭环系统仿真
A_cl = A – B @ K
sys_cl = StateSpaceSystem(A_cl, B, C)

x0 = np.array([0, 0, 0.1, 0]) # 初始角度 0.1 rad
t, x, y = sys_cl.simulate(lambda t: 0, x0, T=5, dt=0.001)

print(f"\\n闭环响应 (初始角度 0.1 rad):")
print(f" 最终角度: {x[–1, 2]:.6f} rad")
print(f" 角度调节时间: {t[np.where(np.abs(x[:, 2]) > 0.001)[0][–1]]:.2f}s"
if len(np.where(np.abs(x[:, 2]) > 0.001)[0]) > 0 else " 立即稳定")

if __name__ == "__main__":
demonstrate_state_space()
demonstrate_lqr()

16.3 Kalman 滤波器

"""
Kalman 滤波器的完整实现。
包含: 离散时间 Kalman 滤波器、状态估计演示。
"""

import numpy as np

class KalmanFilter:
"""离散时间 Kalman 滤波器。

系统模型:
x[k+1] = A x[k] + B u[k] + w[k], w ~ N(0, Q)
y[k] = C x[k] + v[k], v ~ N(0, R)
"""

def __init__(self, A, B, C, Q, R, x0=None, P0=None):
"""
Args:
A: 状态转移矩阵 (n, n)
B: 输入矩阵 (n, m)
C: 观测矩阵 (p, n)
Q: 过程噪声协方差 (n, n)
R: 测量噪声协方差 (p, p)
x0: 初始状态估计
P0: 初始误差协方差
"""
self.A = np.array(A, dtype=float)
self.B = np.array(B, dtype=float)
self.C = np.array(C, dtype=float)
self.Q = np.array(Q, dtype=float)
self.R = np.array(R, dtype=float)

self.n = A.shape[0] # 状态维度
self.p = C.shape[0] # 观测维度

# 初始估计
self.x_hat = np.zeros(self.n) if x0 is None else np.array(x0, dtype=float)
self.P = np.eye(self.n) * 1000 if P0 is None else np.array(P0, dtype=float)

# 存储历史
self.history = {
'x_hat': [self.x_hat.copy()],
'P': [self.P.copy()],
'K': [],
}

def predict(self, u=None):
"""预测步骤。

Args:
u: 输入 (m,) 或 None
"""
if u is None:
u = np.zeros(self.B.shape[1])

# 状态预测
self.x_hat = self.A @ self.x_hat + self.B @ u

# 协方差预测
self.P = self.A @ self.P @ self.A.T + self.Q

def update(self, y):
"""更新步骤。

Args:
y: 观测值 (p,)
"""
# 计算 Kalman 增益
S = self.C @ self.P @ self.C.T + self.R # 新息协方差
K = self.P @ self.C.T @ np.linalg.inv(S)

# 状态更新
y_pred = self.C @ self.x_hat
self.x_hat = self.x_hat + K @ (y – y_pred)

# 协方差更新 (Joseph 形式，数值更稳定)
I_KC = np.eye(self.n) – K @ self.C
self.P = I_KC @ self.P @ I_KC.T + K @ self.R @ K.T

# 记录历史
self.history['x_hat'].append(self.x_hat.copy())
self.history['P'].append(self.P.copy())
self.history['K'].append(K.copy())

return self.x_hat, self.P, K

def step(self, y, u=None):
"""一步预测+更新。

Args:
y: 观测值
u: 输入

Returns:
x_hat: 状态估计
P: 误差协方差
"""
self.predict(u)
return self.update(y)

def get_estimates(self):
"""返回所有状态估计。"""
return np.array(self.history['x_hat'])

def get_covariances(self):
"""返回所有协方差矩阵。"""
return self.history['P']

def demonstrate_kalman_filter():
"""演示 Kalman 滤波器用于状态估计。"""
print("=" * 60)
print("Kalman 滤波器演示")
print("=" * 60)

np.random.seed(42)

# 系统: 匀速运动模型
# 状态: [位置, 速度]
dt = 1.0
A = np.array([[1, dt], [0, 1]])
B = np.array([[0], [0]]) # 无控制输入
C = np.array([[1, 0]]) # 只观测位置

# 噪声参数
q = 0.1 # 过程噪声强度
Q = np.array([[q*dt**3/3, q*dt**2/2],
[q*dt**2/2, q*dt]])
R = np.array([[1.0]]) # 测量噪声方差

print(f"\\n系统模型:")
print(f" 状态转移 A:\\n{A}")
print(f" 观测矩阵 C: {C}")
print(f" 过程噪声 Q:\\n{Q}")
print(f" 测量噪声 R: {R}")

# 生成真实轨迹
T = 50
x_true = np.zeros((T, 2))
x_true[0] = [0, 1] # 初始位置 0，速度 1

for k in range(1, T):
w = np.random.multivariate_normal([0, 0], Q)
x_true[k] = A @ x_true[k–1] + w

# 生成观测
y_obs = np.zeros(T)
for k in range(T):
v = np.random.normal(0, np.sqrt(R[0, 0]))
y_obs[k] = C @ x_true[k] + v

# Kalman 滤波
kf = KalmanFilter(A, B, C, Q, R, x0=[0, 0], P0=np.eye(2)*10)

for k in range(T):
kf.predict()
kf.update(y_obs[k])

# 结果分析
x_est = kf.get_estimates()

# 计算误差
pos_error = np.sqrt(np.mean((x_est[1:, 0] – x_true[:, 0])**2))
vel_error = np.sqrt(np.mean((x_est[1:, 1] – x_true[:, 1])**2))

print(f"\\n滤波结果 (T={T}):")
print(f" 位置 RMSE: {pos_error:.4f}")
print(f" 速度 RMSE: {vel_error:.4f}")
print(f" 观测 RMSE: {np.sqrt(np.mean((y_obs – x_true[:, 0])**2)):.4f}")
print(f" 滤波后位置误差减小: {(1 – pos_error/np.std(y_obs – x_true[:, 0]))*100:.1f}%")

# 展示稳态 Kalman 增益
K_ss = kf.history['K'][–1]
print(f"\\n稳态 Kalman 增益: {K_ss.flatten()}")

# 稳态协方差
P_ss = kf.history['P'][–1]
print(f"稳态协方差矩阵:\\n{P_ss}")

return kf, x_true, y_obs

if __name__ == "__main__":
demonstrate_kalman_filter()

16.4 MPC 实现

"""
模型预测控制 (MPC) 的简单实现。
使用二次规划求解有限时域最优控制问题。
"""

import numpy as np
from scipy.linalg import block_diag

class SimpleMPC:
"""简单的线性 MPC 控制器。

求解:
min sum_{i=0}^{N-1} (x_i^T Q x_i + u_i^T R u_i) + x_N^T P x_N
s.t. x_{i+1} = A x_i + B u_i
u_min <= u_i <= u_max
x_min <= x_i <= x_max
"""

def __init__(self, A, B, Q, R, N, u_min=None, u_max=None, x_min=None, x_max=None):
"""
Args:
A, B: 系统矩阵
Q, R: 权重矩阵
N: 预测时域
u_min, u_max: 控制约束
x_min, x_max: 状态约束
"""
self.A = np.array(A, dtype=float)
self.B = np.array(B, dtype=float)
self.Q = np.array(Q, dtype=float)
self.R = np.array(R, dtype=float)
self.N = N
self.n = A.shape[0]
self.m = B.shape[1]

# 终端权重 (用 DARE 的解)
self.P = self._solve_dare()

# 约束
self.u_min = u_min
self.u_max = u_max
self.x_min = x_min
self.x_max = x_max

def _solve_dare(self):
"""求解离散代数 Riccati 方程。"""
P = self.Q.copy()
for _ in range(100):
P_new = self.Q + self.A.T @ P @ self.A – \\
self.A.T @ P @ self.B @ \\
np.linalg.solve(self.R + self.B.T @ P @ self.B, self.B.T @ P @ self.A)
if np.max(np.abs(P_new – P)) < 1e-10:
break
P = P_new
return P

def solve(self, x0):
"""求解 MPC 优化问题 (无约束情况，解析解)。

Args:
x0: 当前状态

Returns:
u_opt: 最优控制序列 (N, m)
"""
# 构建预测矩阵
# X = Psi * x0 + Theta * U
n, m, N = self.n, self.m, self.N
A, B = self.A, self.B

Psi = np.zeros((n * N, n))
A_power = np.eye(n)
for i in range(N):
A_power = A_power @ A
Psi[i*n:(i+1)*n, :] = A_power

Theta = np.zeros((n * N, m * N))
for i in range(N):
A_power = np.eye(n)
for j in range(i+1):
Theta[i*n:(i+1)*n, (i–j)*m:(i–j+1)*m] = A_power @ B
A_power = A_power @ A

# 构建成本矩阵
Q_bar = block_diag(*([self.Q] * N))
R_bar = block_diag(*([self.R] * N))

# H = Theta^T (Q_bar + …) Theta + R_bar
# 简化: 忽略终端约束的影响
H = Theta.T @ Q_bar @ Theta + R_bar
f = Theta.T @ Q_bar @ Psi @ x0

# 求解: min 0.5 * U^T H U + f^T U
# 无约束解: U* = -H^{-1} f
try:
U_opt = –np.linalg.solve(H, f)
except np.linalg.LinAlgError:
U_opt = –np.linalg.lstsq(H, f, rcond=None)[0]

# 应用约束 (简单投影)
U_opt = U_opt.reshape(N, m)
if self.u_min is not None:
U_opt = np.maximum(U_opt, self.u_min)
if self.u_max is not None:
U_opt = np.minimum(U_opt, self.u_max)

return U_opt

def control(self, x0):
"""计算当前时刻的控制量。

Args:
x0: 当前状态

Returns:
u: 当前控制量 (m,)
"""
U_opt = self.solve(x0)
return U_opt[0]

def demonstrate_mpc():
"""演示 MPC 控制器。"""
print("=" * 60)
print("模型预测控制 (MPC) 演示")
print("=" * 60)

# 双积分器系统 (位置-速度)
dt = 0.1
A = np.array([[1, dt], [0, 1]])
B = np.array([[0], [dt]])
Q = np.diag([10, 1])
R = np.array([[0.1]])

# MPC 参数
N = 20
u_min = np.array([–2.0])
u_max = np.array([2.0])

mpc = SimpleMPC(A, B, Q, R, N, u_min=u_min, u_max=u_max)

print(f"\\n系统: 双积分器")
print(f" A = \\n{A}")
print(f" B = {B.flatten()}")
print(f"预测时域: {N}")
print(f"控制约束: [{u_min[0]}, {u_max[0]}]")

# 仿真
T = 100
x = np.array([5.0, 0.0]) # 初始位置 5
x_history = [x.copy()]
u_history = []

for k in range(T):
u = mpc.control(x)
x = A @ x + B @ u
x_history.append(x.copy())
u_history.append(u[0])

x_history = np.array(x_history)
u_history = np.array(u_history)

print(f"\\n仿真结果 (T={T*dt:.1f}s):")
print(f" 初始位置: {x_history[0, 0]:.2f}")
print(f" 最终位置: {x_history[–1, 0]:.4f}")
print(f" 最终速度: {x_history[–1, 1]:.4f}")
print(f" 最大控制量: {max(abs(u_history)):.4f}")
print(f" 调节时间: {np.where(np.abs(x_history[:, 0]) > 0.01)[0][–1] * dt:.1f}s"
if len(np.where(np.abs(x_history[:, 0]) > 0.01)[0]) > 0 else " 已收敛")

return mpc, x_history, u_history

if __name__ == "__main__":
demonstrate_mpc()

附录

A. 数学符号表

B. 关键公式速查

传递函数：
$$G(s)=C(sI-A{)}^{-1}B+D$$

ZOH 离散化：
$$\bar{A}=e^{A\Delta },\bar{B}=A^{-1}(\bar{A}-I)B$$

能控性矩阵：
$$\mathcal{C}=[BAB\cdots A^{n-1}B]$$

能观性矩阵：
$$\mathcal{O}=[C^T(CA{)}^T\cdots (C{A}^{n-1}{)}^T{]}^T$$

代数 Riccati 方程：
$$A^{T}P+PA-PB{R}^{-1}{B}^{T}P+Q=0$$

LQR 最优增益：
$$K=R^{-1}{B}^{T}P$$

Kalman 增益：
$$L=P{C}^T{R}^{-1}$$

闭环特征值分离：
$$\text{eig}(A-BK)\cup \text{eig}(A-LC)=\text{闭环极点}$$

C. 参考文献

本文涵盖了控制论从经典反馈到现代最优控制的完整理论体系，并探讨了控制论与机器学习（特别是状态空间模型）的深刻联系。代码均使用 NumPy/SciPy 实现，可直接运行。