在实际控制系统中，不确定性无处不在——传感器噪声、过程干扰、模型误差等都会影响系统性能。本章介绍处理随机系统的核心理论与方法，重点讲解卡尔曼滤波器的设计原理及其在状态估计中的应用，以及如何将估计与控制结合形成完整的随机控制系统。我们还将探讨后向随机微分方程（BSDE）这一前沿工具在控制中的应用。通过GPS/INS组合导航系统案例，展示这些理论在实际工程中的威力。
学习目标：
在确定性系统基础上，我们引入随机性来更准确地描述实际系统：
连续时间随机系统： \(\dot{x}(t) = f(x(t), u(t), t) + G(t)w(t)\) \(y(t) = h(x(t), t) + v(t)\)
其中：
离散时间随机系统（更常用于数字控制）： \(x_{k+1} = f(x_k, u_k) + w_k\) \(y_k = h(x_k) + v_k\)
白噪声假设：
功率谱密度： 连续白噪声的功率谱密度是常数，这在实际中是理想化的，但在离散采样下是合理近似。
功率谱密度图示：
|
PSD |━━━━━━━━━━━━━━ 白噪声（理想）
|
| ╱╲
| ╱ ╲ 有色噪声（实际）
|_╱____╲_________
0 频率
均值传播： 对于线性系统 $x_{k+1} = Ax_k + Bu_k + w_k$： \(\mu_{k+1} = A\mu_k + Bu_k\)
协方差传播： \(P_{k+1} = AP_kA^T + Q\)
这描述了不确定性如何随时间演化——即使初始状态确定（$P_0 = 0$），过程噪声会导致不确定性增长。
给定带噪声的测量，如何最优地估计系统状态？卡尔曼滤波器提供了线性系统下的最优解（最小均方误差意义下）。
系统模型： \(x_{k+1} = A_kx_k + B_ku_k + w_k\) \(y_k = C_kx_k + v_k\)
目标：找到状态估计 $\hat{x}_k$，使得估计误差协方差 $P_k = E[(x_k - \hat{x}_k)(x_k - \hat{x}_k)^T]$ 最小。
卡尔曼滤波包含两个步骤：预测和更新。
预测步骤（时间更新）： \(\hat{x}_{k+1|k} = A_k\hat{x}_{k|k} + B_ku_k\) \(P_{k+1|k} = A_kP_{k|k}A_k^T + Q_k\)
更新步骤（测量更新）： \(K_{k+1} = P_{k+1|k}C_{k+1}^T(C_{k+1}P_{k+1|k}C_{k+1}^T + R_{k+1})^{-1}\) \(\hat{x}_{k+1|k+1} = \hat{x}_{k+1|k} + K_{k+1}(y_{k+1} - C_{k+1}\hat{x}_{k+1|k})\) \(P_{k+1|k+1} = (I - K_{k+1}C_{k+1})P_{k+1|k}\)
其中 $K_{k+1}$ 是卡尔曼增益，它平衡了预测和测量的可信度。
卡尔曼增益的作用：
测量噪声小 (R↓) → K↑ → 更相信测量
过程噪声小 (Q↓) → K↓ → 更相信预测
极限情况：
卡尔曼滤波器是线性最小方差无偏估计器（LMMSE）：
对于高斯噪声，卡尔曼滤波器也是所有估计器（包括非线性）中的最优解。
实际系统往往是非线性的： \(x_{k+1} = f(x_k, u_k) + w_k\) \(y_k = h(x_k) + v_k\)
EKF通过局部线性化来应用卡尔曼滤波框架。
预测步骤： \(\hat{x}_{k+1|k} = f(\hat{x}_{k|k}, u_k)\) \(P_{k+1|k} = F_kP_{k|k}F_k^T + Q_k\)
| 其中 $F_k = \frac{\partial f}{\partial x}\bigg | {x=\hat{x}{k | k}}$ 是状态转移雅可比矩阵。 |
更新步骤： \(K_{k+1} = P_{k+1|k}H_{k+1}^T(H_{k+1}P_{k+1|k}H_{k+1}^T + R_{k+1})^{-1}\) \(\hat{x}_{k+1|k+1} = \hat{x}_{k+1|k} + K_{k+1}(y_{k+1} - h(\hat{x}_{k+1|k}))\) \(P_{k+1|k+1} = (I - K_{k+1}H_{k+1})P_{k+1|k}\)
| 其中 $H_{k+1} = \frac{\partial h}{\partial x}\bigg | {x=\hat{x}{k+1 | k}}$ 是测量雅可比矩阵。 |
线性化误差示意：
真实轨迹
╱
╱ ← 线性化误差
╱
╱━━━ EKF估计
╱
╱ 线性化点
无迹卡尔曼滤波（UKF）： 使用确定性采样点（sigma点）来捕获均值和协方差，避免显式计算雅可比矩阵：
迭代EKF（IEKF）： 在更新步骤中多次迭代，每次在新的线性化点重新计算雅可比矩阵，提高精度。
LQG控制的核心是分离原理：最优控制器可以分解为：
这种分离在线性高斯系统下是最优的。
系统模型： \(x_{k+1} = Ax_k + Bu_k + w_k, \quad w_k \sim \mathcal{N}(0, Q_w)\) \(y_k = Cx_k + v_k, \quad v_k \sim \mathcal{N}(0, R_v)\)
代价函数： \(J = E\left[\sum_{k=0}^{N-1}(x_k^TQ_xx_k + u_k^TR_uu_k) + x_N^TQ_fx_N\right]\)
步骤1：设计卡尔曼滤波器估计状态 $\hat{x}_k$
步骤2：解确定性LQR问题，得到反馈增益 $K$
步骤3：控制律为： \(u_k = -K\hat{x}_k\)
完整的控制系统结构：
┌─────────────────────────────────┐
│ │
w_k →┤ 真实系统 x_{k+1} = Ax_k + Bu_k + w_k │
│ │
└────────┬────────────────────────┘
│ y_k = Cx_k + v_k
↓
┌─────────────────────────────────┐
│ 卡尔曼滤波器 │
│ 估计状态 \hat{x}_k │
└────────┬────────────────────────┘
│ \hat{x}_k
↓
┌─────────────────────────────────┐
│ LQR控制器 │
│ u_k = -K\hat{x}_k │
└────────┬────────────────────────┘
│ u_k
└──────────────┐
↓
系统输入
虽然LQG在理论上是最优的，但它缺乏鲁棒性保证：
这就是为什么实践中常需要加入鲁棒性考虑，发展出H∞控制等理论。
经典的随机微分方程（前向SDE）： \(dx_t = f(t, x_t)dt + \sigma(t, x_t)dW_t, \quad x_0 = x\)
后向随机微分方程（BSDE）： \(dY_t = -f(t, Y_t, Z_t)dt + Z_tdW_t, \quad Y_T = \xi\)
其中终端条件 $\xi$ 给定，需要求解 $(Y_t, Z_t)$ 过程。
随机最优控制问题： 最小化代价函数 \(J(t, x) = E\left[\int_t^T L(s, X_s, u_s)ds + g(X_T) \bigg| X_t = x\right]\)
对应的BSDE： 值函数 $V(t, x)$ 满足 \(Y_t = V(t, X_t)\) \(Z_t = \sigma^T(t, X_t)\nabla_xV(t, X_t)\)
这提供了求解随机控制问题的新途径。
对于LQG问题，相应的BSDE是线性的： \(dY_t = -(A^TY_t + Y_tA + Q - Y_tBR^{-1}B^TY_t)dt + Z_tdW_t\)
这等价于随机Riccati方程，但BSDE形式在某些情况下更便于数值求解。
时间离散化： 将 $[0, T]$ 分成 $N$ 个区间，在每个区间上： \(Y_{t_i} = E[Y_{t_{i+1}}|F_{t_i}] + \Delta t \cdot f(t_i, Y_{t_i}, Z_{t_i})\)
最小二乘蒙特卡洛方法：
GPS（全球定位系统）和INS（惯性导航系统）是现代导航的两大支柱技术：
GPS特点：
INS特点：
组合导航通过卡尔曼滤波融合两者优势，是现代飞机、导弹、自动驾驶车辆的标准配置。
状态向量（15维）： \(x = [r^T, v^T, \psi^T, b_a^T, b_g^T]^T\)
其中：
系统动力学： \(\dot{r} = v\) \(\dot{v} = C_b^n(a_m - b_a - n_a) + g\) \(\dot{\psi} = \Omega(\omega_m - b_g - n_g)\) \(\dot{b}_a = -\frac{1}{\tau_a}b_a + w_a\) \(\dot{b}_g = -\frac{1}{\tau_g}b_g + w_g\)
其中：
GPS测量： \(y_{GPS} = \begin{bmatrix} r \\ v \end{bmatrix} + v_{GPS}\)
磁力计测量（用于航向校正）： \(y_{mag} = C_n^b \cdot m_{ref} + v_{mag}\)
其中 $m_{ref}$ 是地磁场参考向量。
误差状态EKF： 不直接估计状态，而是估计误差状态 $\delta x$： \(\delta x = [δr^T, δv^T, δ\psi^T, δb_a^T, δb_g^T]^T\)
这样可以：
预测步骤（IMU更新，高频）：
每个IMU样本（200Hz）：
1. 积分运动方程更新名义状态
2. 传播误差状态协方差
3. 考虑IMU噪声增加不确定性
更新步骤（GPS更新，低频）：
每个GPS样本（1Hz）：
1. 计算创新（测量残差）
2. 检查创新是否合理（野值剔除）
3. 更新误差状态估计
4. 修正名义状态
5. 重置误差状态
时间同步： GPS和IMU的时间戳必须精确对齐，1ms的误差在高速运动时可导致米级定位误差。
野值检测： 使用马氏距离检验GPS测量： \(d^2 = (y - \hat{y})^T S^{-1} (y - \hat{y})\) 其中 $S = HPH^T + R$ 是创新协方差。若 $d^2 > \chi^2_{threshold}$，拒绝该测量。
自适应滤波： 根据创新序列在线调整噪声参数： \(\hat{R}_k = \alpha \hat{R}_{k-1} + (1-\alpha)(e_ke_k^T - HP_{k|k-1}H^T)\)
多路径效应处理： 城市环境中GPS信号反射严重，可以：
典型的GPS/INS组合导航系统性能：
| 指标 | GPS独立 | INS独立(1分钟) | 组合系统 |
|---|---|---|---|
| 位置精度 | 5-10m | 100m | 2-5m |
| 速度精度 | 0.1m/s | 0.5m/s | 0.05m/s |
| 姿态精度 | - | 0.1° | 0.05° |
| 更新率 | 1-10Hz | 100-1000Hz | 100-1000Hz |
| 可用性 | 90% | 100% | 99.9% |
GPS/INS融合系统架构：
┌─────────────┐ ┌─────────────┐
│ IMU传感器 │ │ GPS接收机 │
│ (200 Hz) │ │ (1 Hz) │
└──────┬──────┘ └──────┬──────┘
│ │
↓ ↓
┌─────────────────────────────────┐
│ 时间同步与对准 │
└──────┬──────────────────┬───────┘
│ │
↓ ↓
┌──────────────┐ ┌──────────────┐
│ IMU预处理 │ │ GPS预处理 │
│ -温度补偿 │ │ -野值剔除 │
│ -标定修正 │ │ -多路径检测 │
└──────┬───────┘ └──────┬───────┘
│ │
↓ ↓
┌─────────────────────────────────┐
│ 误差状态EKF │
│ - 预测（IMU驱动） │
│ - 更新（GPS触发） │
│ - 零速检测（ZUPT） │
└──────┬──────────────────────────┘
│
↓
┌─────────────────────────────────┐
│ 导航解输出 │
│ - 位置、速度、姿态 │
│ - 不确定性估计 │
└─────────────────────────────────┘
Rudolf Emil Kálmán 是现代控制理论的奠基人之一。1960年，他发表了具有里程碑意义的论文”A New Approach to Linear Filtering and Prediction Problems”，提出了卡尔曼滤波器。
主要贡献：
Apollo计划的关键技术： 卡尔曼滤波器是Apollo登月计划导航系统的核心。当时MIT的工程师最初对这个”过于理论化”的方法持怀疑态度，但Stanley Schmidt首先认识到其价值，开发了”Apollo滤波器”——实际上就是卡尔曼滤波器的实现。这使得Apollo飞船能够精确导航38万公里到达月球，误差仅几公里。
轶事：
当系统强非线性或噪声非高斯时，EKF和UKF可能失效。粒子滤波通过蒙特卡洛方法直接逼近后验概率分布。
基本思想： 用一组带权重的粒子 ${x_i^k, w_i^k}_{i=1}^N$ 表示概率分布： \(p(x_k|y_{1:k}) \approx \sum_{i=1}^N w_i^k \delta(x_k - x_i^k)\)
序贯重要性采样（SIS）：
| 采样：$x_i^k \sim q(x_k | x_i^{k-1}, y_k)$ |
| 更新权重：$w_i^k \propto w_i^{k-1} \frac{p(y_k | x_i^k)p(x_i^k | x_i^{k-1})}{q(x_i^k | x_i^{k-1}, y_k)}$ |
重采样： 解决权重退化问题，当有效粒子数 $N_{eff} = 1/\sum_i (w_i^k)^2$ 过小时触发。
在控制中的应用：
高斯过程提供了一种非参数的函数逼近方法，特别适合建模未知动力学。
GP动力学模型： \(x_{k+1} = f(x_k, u_k) + w_k\)
其中 $f \sim GP(m(\cdot), k(\cdot, \cdot))$，均值函数 $m$ 和协方差函数 $k$。
GP-MPC框架：
优势：
结合深度学习和卡尔曼滤波：
可微分卡尔曼滤波： 将卡尔曼滤波嵌入神经网络，端到端学习：
应用案例：
本章系统介绍了随机控制系统的核心理论和方法：
核心概念：
关键公式汇总：
卡尔曼滤波递推：
| 预测：$\hat{x}_{k+1 | k} = A\hat{x}_{k | k} + Bu_k$ |
| 更新：$\hat{x}_{k | k} = \hat{x}_{k | k-1} + K(y_k - C\hat{x}_{k | k-1})$ |
LQG控制律：
实践要点：
习题6.1 一维卡尔曼滤波器 考虑标量系统： \(x_{k+1} = ax_k + w_k, \quad w_k \sim \mathcal{N}(0, q)\) \(y_k = x_k + v_k, \quad v_k \sim \mathcal{N}(0, r)\)
推导卡尔曼增益的稳态值，并分析 $q/r$ 比值对滤波器行为的影响。
| Hint: 稳态时 $P_{k+1 | k} = P_{k | k}$，求解代数Riccati方程。 |
习题6.2 观测器与卡尔曼滤波器 证明：当过程噪声和测量噪声都为零时，卡尔曼滤波器退化为确定性观测器。
Hint: 考虑 $Q = 0, R = 0$ 的极限情况。
习题6.3 EKF线性化点选择 某非线性系统 $x_{k+1} = x_k^2 + u_k$。比较以下两种EKF实现的差异： a) 在预测值 $\hat{x}{k+1|k}$ 处线性化 b) 在更新值 $\hat{x}{k|k}$ 处线性化
Hint: 计算雅可比矩阵 $F = 2x$，分析线性化误差。
习题6.4 多传感器融合 三个传感器测量同一个二维位置，测量噪声协方差分别为： \(R_1 = \begin{bmatrix} 1 & 0 \\ 0 & 4 \end{bmatrix}, R_2 = \begin{bmatrix} 4 & 0 \\ 0 & 1 \end{bmatrix}, R_3 = \begin{bmatrix} 2 & 1 \\ 1 & 2 \end{bmatrix}\)
设计最优融合算法，使得估计误差协方差最小。
Hint: 考虑加权最小二乘或信息滤波形式。
习题6.5 自适应卡尔曼滤波 设计一个自适应算法，在线估计未知的测量噪声协方差 $R$。
Hint: 使用创新序列的统计特性。
习题6.6 BSDE与HJB方程（开放题） 考虑随机LQR问题，其值函数满足随机HJB方程。说明如何用BSDE方法求解，并与确定性Riccati方程对比。
Hint: 考虑值函数的鞅表示。
习题6.7 GPS欺骗检测 设计一个算法检测GPS欺骗攻击（spoofing），攻击者发送虚假GPS信号试图误导接收机。
Hint: 利用INS预测和GPS测量的一致性检验。
习题6.8 分布式卡尔曼滤波 多个节点各自有局部观测，设计分布式算法使每个节点都能估计全局状态，且达到集中式卡尔曼滤波的性能。
Hint: 考虑信息形式的卡尔曼滤波和一致性协议。
问题：协方差矩阵失去正定性
症状：
- 协方差矩阵特征值变负
- 滤波器发散
- 出现NaN或Inf
解决方案：
Joseph形式更新： \(P = (I - KH)P(I - KH)^T + KRK^T\) 保证正定性但计算量大
对称化： \(P = \frac{P + P^T}{2}\)
UD分解或平方根滤波器
问题：初始估计偏差过大导致EKF发散
解决方案：
问题：传感器时间不同步
影响：
- 100ms延迟 → 100km/h车辆产生2.8m误差
- 时变延迟导致滤波器不稳定
解决方案：
问题：实际系统与模型差异大
诊断方法：
解决方案：
问题：传感器偶尔输出错误数据
解决方案：
# 卡方检验
innovation = y - H @ x_pred
S = H @ P_pred @ H.T + R
chi2 = innovation.T @ inv(S) @ innovation
if chi2 > chi2_threshold:
# 拒绝这个测量
return x_pred, P_pred