4、卡尔曼滤波基础：离散与连续卡尔曼滤波、噪声统计、协方差更新、最优估计
卡尔曼滤波（Kalman Filter）是现代控制理论、导航制导、信号处理与状态观测领域最核心的算法之一。与经典龙伯格观测器（Luenberger Observer）不同，卡尔曼滤波并非仅仅追求状态误差的极点配置，而是在系统存在随机过程噪声与测量噪声时，以最小均方误差（MMSE）为准则，递推地给出状态的最优估计。本章将从概率与随机过程的视角出发，系统讲解离散卡尔曼滤波的完整递推流程、连续系统的卡尔曼-布西滤波（Kalman-Bucy Filter）形式、噪声统计特性的工程辨识方法，以及协方差矩阵在预测与更新中的传播规律。通过本章学习，读者将能够理解“最优估计”的数学本质，并具备将卡尔曼滤波应用于无传感器观测器设计的基础能力。
上图展示了离散卡尔曼滤波的核心递推闭环。整个算法由“时间更新”（预测）与“测量更新”（校正）两个步骤交替构成。时间更新利用系统模型（状态转移矩阵 A、输入矩阵 B）向前推算状态先验估计 x̂ₖ⁻ 与误差协方差先验 Pₖ⁻；测量更新则利用当前时刻的测量值 zₖ 与测量矩阵 H，通过卡尔曼增益 Kₖ 对先验估计进行修正，得到后验估计 x̂ₖ 与后验协方差 Pₖ。这一循环从初始条件 x̂₀ 与 P₀ 出发，随着 k 的递增不断递归，从而实现实时最优估计。
4.1 离散卡尔曼滤波的数学基础与递推方程
考虑如下线性离散随机系统：
xₖ = A·xₖ₋₁ + B·uₖ₋₁ + wₖ₋₁
zₖ = H·xₖ + vₖ其中，xₖ ∈ ℝⁿ 为系统状态向量，uₖ₋₁ 为已知控制输入，zₖ ∈ ℝᵐ 为测量向量。wₖ₋₁ 为过程噪声（Process Noise），vₖ 为测量噪声（Measurement Noise）。卡尔曼滤波的基本假设是：wₖ 与 vₖ 均为零均值的高斯白噪声，且互不相关，即满足：
- E[wₖ] = 0，E[wₖ·wⱼᵀ] = Q·δₖⱼ，其中 Q 为过程噪声协方差矩阵（n×n 对称半正定）；
- E[vₖ] = 0，E[vₖ·vⱼᵀ] = R·δₖⱼ，其中 R 为测量噪声协方差矩阵（m×m 对称正定）；
- E[wₖ·vⱼᵀ] = 0，即过程噪声与测量噪声不相关。
在上述假设下，卡尔曼滤波给出了状态估计的最小均方误差解。其递推方程可归纳为以下五个核心公式：
| 步骤 | 公式 | 物理含义 |
|---|---|---|
| ① 状态预测 | x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ₋₁ | 利用系统模型推算先验状态 |
| ② 协方差预测 | Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q | 先验误差协方差的传播 |
| ③ 卡尔曼增益 | Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹ | 权衡预测与测量的可信度 |
| ④ 状态校正 | x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ − H·x̂ₖ⁻) | 用新息修正先验估计 |
| ⑤ 协方差校正 | Pₖ = (I − Kₖ·H)·Pₖ⁻ | 更新后验误差协方差 |
其中，新息（Innovation）定义为 eₖ = zₖ − H·x̂ₖ⁻，它反映了测量值与预测值之间的偏差。卡尔曼增益 Kₖ 的取值在 0 与某个上界之间动态调整：当测量噪声 R 较小（测量可信度高）时，Kₖ 增大，滤波更信任测量值；当过程噪声 Q 较小（模型精确）时，Pₖ⁻ 较小，Kₖ 减小，滤波更信任模型预测。
核心原理：卡尔曼滤波的本质是贝叶斯估计在高斯假设下的解析解。先验分布 p(xₖ|z₁:ₖ₋₁) ~ N(x̂ₖ⁻, Pₖ⁻) 与似然 p(zₖ|xₖ) ~ N(H·xₖ, R) 相乘后，后验分布 p(xₖ|z₁:ₖ) 仍为高斯分布，其均值即为 x̂ₖ，协方差即为 Pₖ。这一性质保证了滤波递推的闭合性，无需存储历史数据。
4.2 连续卡尔曼滤波（卡尔曼-布西滤波）
当系统以连续时间形式描述时，需要将离散卡尔曼滤波推广为连续形式。考虑线性连续随机系统：
ẋ(t) = F·x(t) + G·u(t) + w(t)
z(t) = H·x(t) + v(t)其中，w(t) 与 v(t) 为连续时间白噪声过程，其协方差满足 E[w(t)·w(τ)ᵀ] = Q_c·δ(t−τ)，E[v(t)·v(τ)ᵀ] = R_c·δ(t−τ)。连续卡尔曼滤波（又称卡尔曼-布西滤波）的递推方程为：
- 状态估计方程： x̂̇(t) = F·x̂(t) + G·u(t) + K(t)·[z(t) − H·x̂(t)]
- 协方差黎卡提方程： Ṗ(t) = F·P(t) + P(t)·Fᵀ + Q_c − P(t)·Hᵀ·R_c⁻¹·H·P(t)
- 增益计算： K(t) = P(t)·Hᵀ·R_c⁻¹
连续卡尔曼滤波与离散形式的核心区别在于：协方差更新由差分方程变为微分方程（黎卡提方程），增益 K(t) 随时间连续变化。在工程实现中，连续卡尔曼滤波通常用于理论分析或模拟仿真，而实际数字控制器中仍以离散形式为主。若需将连续系统离散化，可采用零阶保持（ZOH）或双线性变换（Tustin）等方法，将 F、G 矩阵转换为 A、B 矩阵，同时将 Q_c 转换为离散 Q 矩阵：
A = e^(F·Ts)
Q = ∫₀ᵀˢ e^(F·τ)·Q_c·e^(Fᵀ·τ) dτ其中 Ts 为采样周期。工程中常用近似公式 Q ≈ Q_c·Ts（当 Ts 足够小时）。
4.3 噪声统计特性的辨识与工程调参
卡尔曼滤波的性能高度依赖于噪声协方差矩阵 Q 与 R 的准确性。若 Q 设置过小，滤波器将过度信任模型，导致估计滞后或发散；若 Q 设置过大，滤波器将过度信任测量，导致估计噪声增大。同样，R 设置不当也会引起类似问题。工程中常用的噪声统计辨识方法包括：
- 离线测量法：在系统静止或已知工况下，采集传感器输出数据，计算测量噪声方差 R。例如，对静止的加速度计输出求样本方差，即可得到 R 的估计值。
- Allan 方差法：用于辨识陀螺仪、加速度计等惯性传感器的噪声特性，可分离量化噪声、角度随机游走、零偏不稳定性、速率随机游走等误差源，从而为 Q 矩阵的选取提供依据。
- 新息序列分析法：在滤波运行过程中，统计新息序列 eₖ 的样本协方差，并与理论值 Sₖ = H·Pₖ⁻·Hᵀ + R 进行比较。若实际协方差远大于理论值，说明 Q 或 R 设置偏小；反之则偏大。
- 自适应滤波法：采用 Sage-Husa 自适应卡尔曼滤波，在线估计 Q 与 R。该方法通过指数加权移动平均（EWMA）实时更新噪声统计，但需注意算法的收敛性与稳定性。
风险提示：Q 与 R 并非物理量，而是滤波器设计参数。在实际工程中，Q 与 R 的相对比值（而非值）决定了滤波器的动态响应与噪声抑制能力。调参时应先固定 R（通过传感器标定获得），再调整 Q 以平衡响应速度与平滑度。切勿盲目增大 Q 以追求快速响应，否则可能导致估计值剧烈抖动。
4.4 协方差更新的数值稳定性与工程实现
协方差矩阵 Pₖ 的更新是卡尔曼滤波的“心脏”。在实际计算中，由于舍入误差与模型不确定性，Pₖ 可能失去对称正定性，导致滤波发散。为此，工程中常采用以下措施：
- 对称化处理：每次更新后执行 Pₖ = (Pₖ + Pₖᵀ)/2，强制对称。
- Joseph 形式：使用 Pₖ = (I − Kₖ·H)·Pₖ⁻·(I − Kₖ·H)ᵀ + Kₖ·R·Kₖᵀ 替代简化形式，该形式在数值上更稳定，即使增益计算存在误差也能保证 Pₖ 半正定。
- 平方根滤波：将 Pₖ 分解为 Pₖ = Sₖ·Sₖᵀ（Cholesky 分解），直接递推 Sₖ，避免直接计算 Pₖ 的平方，显著提高数值精度。典型算法包括 Potter 平方根滤波与 UD 分解滤波。
- 遗忘因子：在协方差预测中引入遗忘因子 α（略大于 1），即 Pₖ⁻ = α·A·Pₖ₋₁·Aᵀ + Q，以限制 Pₖ 过小，防止滤波器“自满”而失去对突变的跟踪能力。
4.5 案例拆解：永磁同步电机无位置传感器控制中的卡尔曼滤波设计
以表贴式永磁同步电机（SPMSM）的无位置传感器控制为例，说明卡尔曼滤波的实际应用流程。电机在 α-β 静止坐标系下的电压方程为：
di_α/dt = (−R_s/L_s)·i_α + (ω_e·ψ_f/L_s)·sin(θ_e) + u_α/L_s
di_β/dt = (−R_s/L_s)·i_β − (ω_e·ψ_f/L_s)·cos(θ_e) + u_β/L_s其中，i_α、i_β 为定子电流，u_α、u_β 为定子电压，R_s 为定子电阻，L_s 为定子电感，ψ_f 为永磁体磁链，ω_e 为电角速度，θ_e 为转子电角度。选取状态向量 x = [i_α, i_β, ω_e, θ_e]ᵀ，测量向量 z = [i_α, i_β]ᵀ。由于方程中存在 sin(θ_e) 与 cos(θ_e) 的非线性项，需采用扩展卡尔曼滤波（EKF）进行线性化。
关键参数设置：
| 参数 | 数值 | 说明 |
|---|---|---|
| 采样周期 Ts | 100 μs | PWM 载波周期 |
| R_s | 0.36 Ω | 定子电阻 |
| L_s | 0.85 mH | 定子电感 |
| ψ_f | 0.071 Wb | 永磁体磁链 |
| Q 矩阵 | diag(1e-3, 1e-3, 10, 0.01) | 电流噪声小，转速与角度噪声较大 |
| R 矩阵 | diag(0.01, 0.01) | 电流传感器噪声方差 |
| 初始 P₀ | diag(0.1, 0.1, 1, 0.01) | 初始协方差 |
实施步骤：
- 在 MATLAB/Simulink 中搭建电机模型与 EKF 算法，离线仿真验证收敛性；
- 将 EKF 算法转化为 C 代码，嵌入 DSP（如 TMS320F28379D）中，注意使用浮点运算单元（FPU）加速矩阵运算；
- 在电机台架上进行空载与带载实验，记录估计角度与实际编码器角度的误差；
- 若稳态误差偏大，优先调整 Q 中 ω_e 与 θ_e 对应的元素；若动态响应慢，适当增大 Q 或减小 R；
- 在低速区（< 5% 额定转速）观察滤波发散情况，必要时注入高频信号辅助辨识。
结果对比：在额定转速 3000 rpm 下，EKF 估计角度误差在 ±2° 电角度以内，转速估计误差小于 10 rpm。与传统的滑模观测器（SMO）相比，EKF 在负载突变时的动态误差更小，但计算量约为 SMO 的 3 倍。
经验复盘：卡尔曼滤波的调参过程本质上是“模型信任度”与“测量信任度”的权衡。建议先通过离线仿真确定 Q/R 的大致量级，再在实验中微调。若滤波发散，首先检查系统模型是否正确（尤其是矩阵 A 与 H 的维度和数值），其次检查 Q 是否过小导致协方差 P 收敛到零，最后检查传感器数据是否存在异常跳变。
4.6 常见误区与总结复盘
在卡尔曼滤波的学习与工程应用中，以下误区值得特别关注：
- 误区一：将 Q 与 R 视为物理噪声的精确测量。实际上，Q 与 R 是设计参数，反映的是模型误差与测量误差的综合不确定性。即使物理噪声已知，模型失配也会导致 Q 需要人为增大。
- 误区二：忽略系统可观性。卡尔曼滤波要求系统满足可观性条件。若某些状态不可观，P 矩阵中对应元素将无法收敛，滤波结果不可信。在设计前应进行可观性秩检验。
- 误区三：直接使用简化协方差更新公式。在嵌入式系统中，由于字长限制，简化公式 Pₖ = (I − Kₖ·H)·Pₖ⁻ 容易导致 Pₖ 失去对称正定性。建议使用 Joseph 形式或平方根滤波。
- 误区四：忽视初始值 P₀ 的影响。P₀ 设置过小会导致滤波初期收敛缓慢；设置过大则可能引起数值振荡。一般取 P₀ 为对角阵，对角线元素为状态量量级平方的 10~100 倍。
- 误区五：将卡尔曼滤波与龙伯格观测器对立。实际上，当 Q → 0 且 R → ∞ 时，卡尔曼滤波退化为确定性系统的龙伯格观测器；当 R → 0 时，滤波完全跟随测量。理解这一连续谱系有助于根据工程需求选择合适的观测器结构。