Skip to content

卡尔曼滤波 (Kalman Filter)#

卡尔曼滤波是一种递归最优估计方法，在已知系统动态模型和测量模型的条件下，能够从带噪声的观测序列中估计出系统的最优状态。

算法思路#

卡尔曼滤波的核心思想是预测-更新（Predict-Update）循环：

  1. 预测步骤 (Predict)：基于系统动力学模型，预测下一时刻的状态及其不确定性（先验估计）。
  2. 更新步骤 (Update)：利用当前时刻的最新观测值校正预测状态，得到更准确的状态估计（后验估计）。

这两个步骤交替进行，通过卡尔曼增益 (Kalman Gain) 动态平衡“对预测的信任程度”与“对观测的信任程度”。

线性卡尔曼滤波 (KF)#

数学推导#

1. 系统模型#

假设系统为线性离散系统：

状态方程：

\[x_k = F_k x_{k-1} + B_k u_k + w_k\]

观测方程：

\[z_k = H_k x_k + v_k\]
变量 含义 变量 含义
\(x_k\) 状态向量 \(z_k\) 观测向量
\(F_k\) 状态转移矩阵 \(H_k\) 观测矩阵
\(u_k\) 控制输入 \(w_k\) 过程噪声，满足 \(w_k \sim \mathcal{N}(0, Q_k)\)
\(B_k\) 控制输入矩阵 \(v_k\) 测量噪声，满足 \(v_k \sim \mathcal{N}(0, R_k)\)

2. 预测步骤 (Predict)#

状态预测（先验状态估计）：

\[\hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k\]

协方差预测（先验误差协方差）：

\[P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k\]

3. 更新步骤 (Update)#

计算卡尔曼增益：

\[K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}\]

状态更新（后验状态估计）：

\[\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1})\]

协方差更新（后验误差协方差）：

\[P_{k|k} = (I - K_k H_k) P_{k|k-1}\]

注：公式中的 \(y_k = z_k - H_k \hat{x}_{k|k-1}\) 被称为 Innovation（创新/残差），反映了实际观测与预测观测之间的差异。

4. 核心公式汇总#

为方便工程实现，通常将核心状态更新公式总结如下（假设矩阵不随时间变化，省略下标 \(k\)）：

\[\boxed{K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}}\]
\[\boxed{\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1})}\]
\[\boxed{P_{k|k} = (I - K_k H) P_{k|k-1}}\]

参数说明与调优指南#

参数 含义 调整建议
\(Q\) 过程噪声协方差 反映系统内部模型的不确定性。\(Q\) 越大，表示系统动态变化越不可靠，滤波器更倾向于信任观测值。
\(R\) 测量噪声协方差 反映外部传感器的测量精度。\(R\) 越大，表示测量越不准，滤波器更倾向于信任模型预测值。
\(P_0\) 初始估计协方差 初始状态的不确定度。若对初始状态无把握，可设较大的值以加速收敛。

工程调参原则：

  • 响应太慢（跟随不上真实变化）：减小 \(R\) 或增大 \(Q\)。
  • 波动太大（噪声过滤不干净）：增大 \(R\) 或减小 \(Q\)。
  • 滤波器的稳态性能和跟踪误差最终由 \(Q/R\) 的比值决定，绝对值大小影响收敛速度。

典型应用：1D 常量估计#

对于估计某个静止恒定值 \(x\)（如测量静止物体的重量）的极简场景：

  • \(F = 1\)（状态不随时间变化）
  • \(H = 1\)（传感器直接观测该状态）
  • \(Q \approx 0\)（系统极其稳定）
  • \(R\) 根据传感器手册的白噪声方差设置

此时卡尔曼增益退化为标量形式：

\[k = \frac{p}{p + r}\]

其中 \(p\) 是估计方差。增益会自适应收敛：初始时 \(p\) 大，\(k\) 接近 1，完全采信观测；随着迭代估计收敛，\(p \to 0\)，\(k \to 0\)，不再受新观测噪声干扰。

控制流程 (伪代码)#

初始化: 
    x_hat_0 (初始状态), P_0 (初始协方差)
    Q (过程噪声), R (测量噪声)

循环 k=1, 2, ... :
    # ================= 1. 预测 (Predict) =================
    x_hat_prior = F * x_hat_post[k-1] + B * u[k]
    P_prior = F * P_post[k-1] * F.T + Q

    # ================= 2. 更新 (Update)  =================
    # 计算卡尔曼增益
    K = P_prior * H.T * inv(H * P_prior * H.T + R)

    # 计算残差
    y = z[k] - H * x_hat_prior

    # 状态与协方差更新
    x_hat_post[k] = x_hat_prior + K * y
    P_post[k] = (I - K * H) * P_prior

局限性#

  1. 线性假设严格：要求系统状态转移模型和观测模型必须是严格线性的。
  2. 高斯噪声假设：理论上要求过程噪声和测量噪声必须服从零均值高斯白噪声分布。
  3. 最优性条件：只有在满足上述“线性+高斯”条件时，卡尔曼滤波才是最小方差无偏估计（线性最优估计器）。
  4. 计算复杂度：矩阵求逆运算的时间复杂度为 \(O(n^3)\)，在状态向量维度极高时（如 SLAM 地图构建），计算资源开销大。

扩展卡尔曼滤波 (EKF)#

算法思路#

真实世界的系统大多是非线性的。扩展卡尔曼滤波 (Extended Kalman Filter) 的核心思想是：将非线性函数在当前最佳估计点附近进行一阶泰勒展开 (Taylor Expansion) 实现局部线性化，随后即可套用标准卡尔曼滤波的公式框架。

数学公式#

非线性系统模型：

\[x_k = f(x_{k-1}, u_k) + w_k\]
\[z_k = h(x_k) + v_k\]

局部线性化（雅可比矩阵）：

\[F_k \approx \left.\frac{\partial f}{\partial x}\right|_{x=\hat{x}_{k-1|k-1}, u=u_k}\]
\[H_k \approx \left.\frac{\partial h}{\partial x}\right|_{x=\hat{x}_{k|k-1}}\]

求导得到雅可比矩阵 \(F_k\) 和 \(H_k\) 后，将其代入标准 KF 的预测和更新公式中即可。

参数说明#

参数 含义
\(f(\cdot)\) 非线性状态转移函数（如差分驱动机器人的运动学模型）
\(h(\cdot)\) 非线性观测函数（如极坐标下的雷达测距测角模型）
\(F_k\) 状态转移偏导数矩阵（雅可比矩阵 \(\partial f/\partial x\)）
\(H_k\) 观测偏导数矩阵（雅可比矩阵 \(\partial h/\partial x\)）

典型应用场景#

EKF 广泛应用于机器人学与自动驾驶领域：

  • 轮式里程计：差分/阿克曼底盘的非线性运动学状态估计。
  • 组合导航 (GNSS/INS)：GPS 与惯性导航系统的多传感器数据融合。
  • 视觉里程计 (VIO)：视觉特征点追踪与 IMU 的松/紧耦合。

局限性#

  1. 线性化误差：一阶泰勒近似忽略了高阶项。在系统强非线性区域，线性化误差会迅速累积，甚至导致滤波发散（Divergence）。
  2. 雅可比矩阵推导困难：对于复杂的动力学系统，解析求导非常困难且极易出错；若使用数值微分，则会增加计算负担。
  3. 高斯分布畸变：高斯分布经过非线性变换后不再是严格的高斯分布，EKF 仅通过线性化来逼近其均值和方差，在高阶非线性下存在本质缺陷（此缺陷可由无迹卡尔曼滤波 UKF 弥补）。

参考实现#

  • 1D/2D 卡尔曼滤波算法仿真示例：请参见 02.kalman_filter_sim.ipynb
  • 面向对象的底层代码实现：请参见 utils.py 中的 KalmanFilter1D 和 KalmanFilter2D 类定义。