卡尔曼滤波器介绍、源码

“本在充满不确定性的世界里，如何从嘈杂的观测数据中提取出系统的真实状态？无论是自动驾驶汽车的精准定位，还是无人机的稳定悬停，背后都离不开一个强大的算法 —— 卡尔曼滤波器。本文将带你从零开始，系统地了解卡尔曼滤波器的核心思想、数学原理，并通过一个直观的实践案例，揭开它神秘的面纱”

一、引言：什么是卡尔曼滤波器？

想象一下，你在一个漆黑的房间里，用一个不稳定的手电筒去追踪一个移动的小球。手电筒的光束（测量值）会晃动（噪声），小球的运动轨迹（系统状态）也不完全确定。卡尔曼滤波器（Kalman Filter）就像是一位经验丰富的观察者，它通过一个 **“预测 - 校正”** 的不断循环，来解决这个问题。

1. 预测 (Predict)
   ：它会根据上一次看到的位置，来预测小球下一步最有可能在哪里。
2. 校正 (Update)
   ：它会结合当前晃动的手电筒光束（新的观测），来修正这个预测。

最终，它会给出一个比单独依赖预测或观测都更准确的小球位置估计。因此，卡尔曼滤波器本质上是一个最优估计算法，旨在从一系列包含噪声的、不确定的测量数据中，估计出系统的真实状态。

自 1960 年由鲁道夫・卡尔曼（Rudolf E. Kálmán）提出以来，因其最优性（在特定条件下）、递归性（计算高效）和普适性，已成为导航、控制、信号处理等领域不可或缺的工具。

二、离散卡尔曼滤波器（KALMAN Filter）详解

卡尔曼滤波器适用于线性系统。我们首先需要用数学语言来精确描述我们所要估计的系统。

2.1 问题建模：线性系统的数学描述

我们需要定义两个核心方程：

1. 状态方程 (Process Model)这个方程描述了系统状态是如何随时间演变的。xk=Axk−1+Buk−1+wk−1

• xk
  : k 时刻的系统状态向量（例如，汽车的位置和速度）。
• A
  : 状态转移矩阵，描述状态如何自然演变。
• B
  : 控制输入矩阵，描述外部控制（如油门）如何影响状态。
• uk−1
  : k−1 时刻的控制向量。
• wk−1
  : 过程噪声，代表模型未描述的不确定性（如路面颠簸），通常假设其服从均值为 0，协方差为 Q 的正态分布。

2. 观测方程 (Measurement Model)这个方程描述了我们如何通过传感器测量系统状态。zk=Hxk+vk

• zk
  : k 时刻的观测向量（例如，雷达测量的距离）。
• H
  : 观测矩阵，将系统状态映射到观测空间。
• vk
  : 观测噪声，代表传感器误差，通常假设其服从均值为 0，协方差为 R 的正态分布。

核心假设：过程噪声 wk 和观测噪声 vk 是相互独立的白色高斯噪声。

2.2 卡尔曼滤波算法：预测 - 校正循环

卡尔曼滤波的计算过程就是不断重复以下两个阶段的循环。

阶段一：时间更新（预测）

这一步利用系统模型，从 k−1 时刻的状态预测出 k 时刻的先验状态。

1. 状态预测方程:x^k−=Ax^k−1+Buk−1含义：根据上一时刻的最优估计和控制输入，预测当前时刻的系统状态。

2. 误差协方差预测方程:Pk−=APk−1AT+Q含义：预测当前状态估计的不确定性。它由上一时刻的不确定性传播和过程噪声引入的新不确定性共同构成。

阶段二：测量更新（校正）

这一步利用新的观测值 zk 来修正预测结果，得到更精确的后验估计。

1. 计算卡尔曼增益 (Kk):Kk=Pk−HT(HPk−HT+R)−1含义：卡尔曼增益是算法的核心，它决定了我们应该在多大程度上相信 “预测”（x^k−），又在多大程度上相信 “观测”（zk）。如果传感器很准（R 小），Kk 就大，更相信观测；如果预测很准（Pk− 小），Kk 就小，更相信预测

2. 更新状态估计

x^k=x^k−+Kk(zk−Hx^k−)含义：这是最终的最优估计。它将先验估计与一个由卡尔曼增益和 “残差”（观测值与预测观测值的差异）构成的修正项相加

3.更新误差协方差:Pk=(I−KkH)Pk−含义：更新后验估计的不确定性。由于融入了新的观测信息，理论上不确定性 Pk 会小于先验不确定性 Pk−。

三、扩展卡尔曼滤波器（EKF）—— 处理非线性问题

现实世界中的大多数系统都不是线性的。例如，卫星轨道的计算涉及万有引力，其模型是状态的非线性函数。当系统模型 f 或观测模型 h 是非线性时，标准的卡尔曼滤波器不再适用。

3.1 EKF 的核心思想：泰勒展开线性化

扩展卡尔曼滤波器（Extended Kalman Filter, EKF）通过线性化的方法，将卡尔曼滤波的思想推广到了非线性系统。

其基本思想是，在当前的估计点附近，使用泰勒级数展开的一阶项来近似非线性函数。这相当于用一个切平面来近似一个弯曲的曲面。

线性化后，我们得到了新的 “局部” 线性模型，其状态转移矩阵和观测矩阵由原非线性函数的 ** 雅可比矩阵（Jacobian Matrix）** 代替。

• 过程雅可比矩阵 Ak

• Ak=∂f/∂f---x^k-1
• 观测雅可比矩阵 Hk

• Hk=∂h/∂x---x^k

• EKF 的算法流程与标准卡尔曼滤波器几乎完全相同，只是在所有公式中，用雅

  可比矩阵 Ak 和 Hk 替换了原来的线性矩阵 A 和 H，并用非线性函数 f 和 h 进行状态和观测的预测。

  EKF 的缺陷：由于只做了一阶近似，当系统非线性很强时，这种近似会引入较大误差，甚至导致滤波器发散。

• 四、实践案例 —— 估计一个随机常数

• 为了直观理解卡尔曼滤波器的工作原理，我们来看一个最简单的例子：估计一个恒定的电压值。

  4.1 问题描述与模型建立

• 目标
              估计一个真实值为 x=−0.37727 伏特的恒定电压。
• 观测
•            我们使用一个有噪声的电压表进行测量，测量值 zk 等于真实值加上一个服从正态分布的噪声。
• 模型
• 过程模型
  : 电压恒定，xk=xk−1。所以 A=1。
• 观测模型
  : 测量值直接反映状态，zk=xk+vk。所以 H=1。
• 参数设置
  :
• 我们设定观测噪声协方差 R=(0.1)2=0.01，这代表我们对传感器的精度有一个准确的估计。
• 初始估计 x^0=0，初始不确定性 P0=1。

4.2 模拟结果与分析

下图展示了滤波器运行 50 次迭代的结果。

• 噪声测量值 (Noisy Measurements)
  : 图中的加号，波动很大，远离真实值。
• 后验估计 (A Posteriori Estimate)
  : 图中的蓝色曲线，它迅速收敛到真实值（绿色实线），并且非常平滑。

这个结果完美展示了卡尔曼滤波器的 “滤波” 效果。它能够有效去除噪声，给出对真实值的最优估计。同时，代表不确定性的协方差 P 也迅速收敛到一个极小的值，表明滤波器对其估计结果越来越有信心。

参数的影响：如果我们错误地设置了参数，比如将 R 设置得远小于实际值（高估传感器精度），滤波器会过度相信噪声数据，导致估计值波动剧烈，滤波效果变差。反之，如果将 R 设置得过大（低估传感器精度），滤波器会变得过于保守，收敛速度变慢。这说明，正确理解和设置 Q 和 R 矩阵对于滤波器的性能至关重要。

• 优点
• 最优性
  : 在线性高斯假设下，能提供最小均方误差估计。
• 高效率
  : 递归计算，计算量小，适合实时应用。
• 普适性
  : 应用范围极其广泛。
• 局限性
• 线性与高斯假设
  对系统模型和噪声分布的假设较为严格。
• 参数敏感
  性能依赖于对 Q 和 R 矩阵的准确估计。

5.2 衍生与扩展

为了克服卡尔曼滤波器的局限性，研究者们发展出了许多高级变体，例如：

• 无迹卡尔曼滤波器 (UKF)
  使用确定性采样点（Sigma点）来更精确地近似概率分布，性能通常优于 EKF。
• 粒子滤波器 (PF)
  一种基于蒙特卡洛方法的非线性非高斯滤波方法，适用范围最广，但计算量也最大。

卡尔曼滤波理论及其衍生算法至今仍是信号处理、控制理论和人工智能等领域的研究热点，持续为解决复杂世界中的不确定性问题提供强大的工具。

git link: https://github.com/robodhruv/kalmanfilter-cpp

往期精选：

论文: 点云地面、非地面点云提取、源代码-CSF

干货！如何处理激光雷达点云的运动畸变

点云文件基本操作-代码

深度图转换为3D点云

欢迎点赞、收藏、关注、评论区留言呦~

关注后台回复“kalman”获取对应文章~