1. 卡尔曼滤波入门：状态估计问题、高斯分布、贝叶斯滤波思想、卡尔曼滤波的5个核心公式
1.1 状态估计问题——我们到底在解决什么问题？
大家好，我是你们的老朋友。今天咱们正式开始聊卡尔曼滤波。
先问一个问题：你手上有传感器数据，比如GPS、IMU、激光雷达。这些数据都有噪声。你想要的，是系统真实的状态——比如位置、速度、姿态。但真实状态你永远测不到，只能靠估计。
这就是状态估计问题。
说白了，就是「从带噪声的观测中，猜出系统内部的状态」。我当年刚入行时，总觉得这玩意儿玄乎。后来做了一次无人机定位，GPS信号被高楼遮挡，IMU又飘得厉害……嗯，那时候我才真正理解：没有好的状态估计，传感器再多也是废铁。
状态估计的核心假设是：系统状态随时间变化，而且变化是有规律的。我们可以用数学模型来描述这个规律。同时，传感器观测到的数据，也跟状态有数学关系。
所以，我们有两个模型：
- 运动模型：描述状态怎么随时间演变
- 观测模型：描述状态怎么产生观测数据
卡尔曼滤波，就是在这两个模型之间做最优折中。
1.2 高斯分布——为什么它无处不在？
你想想看，自然界里很多噪声都服从高斯分布。中心极限定理告诉我们：大量独立随机变量的和，趋近于高斯分布。传感器噪声、过程噪声，基本都是高斯。
高斯分布有两个参数：均值 μ 和方差 σ²。均值是「最可能的值」，方差是「不确定度」。
我个人习惯把高斯分布想象成一个钟形曲线。曲线越瘦高，说明我越确定；越扁平，说明我越没底。
卡尔曼滤波之所以好用，就是因为它假设所有噪声都是高斯的。这样一来，状态估计就变成了「两个高斯分布相乘」的问题——结果还是高斯分布。这就很舒服了，因为高斯分布只需要均值和协方差就能完全描述。
重要性质：高斯分布经过线性变换后，仍然是高斯分布。这是卡尔曼滤波能保持「线性最优」的数学基础。
我在项目中遇到过一件事：有一次用激光雷达做障碍物跟踪，观测噪声明显不是高斯——有离群点。结果卡尔曼滤波直接崩了，估计值乱跳。后来加了鲁棒性处理，才稳住。所以，高斯假设不是万能的，但大部分场景下够用。
1.3 贝叶斯滤波思想——从概率角度看估计
贝叶斯滤波，是卡尔曼滤波的理论源头。它的核心思想就一句话：
后验概率 ∝ 似然 × 先验概率
什么意思呢？
- 先验：根据运动模型，我「猜」当前状态大概在哪
- 似然：根据观测模型，当前观测数据「支持」哪些状态
- 后验：把两者乘起来，得到「最合理」的状态估计
贝叶斯滤波是一个递归过程。每一时刻，我先用运动模型预测（先验），再用观测数据更新（后验）。然后下一时刻，后验变成新的先验，继续循环。
你想想看，这不就是「预测-更新」的循环吗？卡尔曼滤波就是贝叶斯滤波在高斯假设下的具体实现。
个人经验：理解贝叶斯滤波，比死记卡尔曼公式更重要。我面试新人时，只要能把贝叶斯思想讲清楚，代码写得差点我也愿意要。因为思想对了，公式只是工具。
1.4 卡尔曼滤波的5个核心公式
好，终于到正题了。卡尔曼滤波就5个公式，分两组：
预测阶段（2个公式）
1. 状态预测：x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ
2. 协方差预测：Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q- x̂ₖ⁻：先验状态估计（预测值）
- Pₖ⁻：先验协方差（预测的不确定度）
- A：状态转移矩阵
- B：控制输入矩阵
- uₖ：控制输入
- Q：过程噪声协方差
更新阶段（3个公式）
3. 卡尔曼增益：Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹
4. 状态更新：x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻)
5. 协方差更新：Pₖ = (I - Kₖ·H)·Pₖ⁻- Kₖ：卡尔曼增益（权重，决定信预测还是信观测）
- H：观测矩阵
- zₖ：实际观测值
- R：观测噪声协方差
- I：单位矩阵
这5个公式，我建议你手推一遍。当年我学的时候，推了整整三遍才真正理解每个矩阵的物理意义。
避坑指南：我曾经在项目中把Q和R设反了——过程噪声设得特别小，观测噪声设得特别大。结果滤波器几乎不更新，状态一直跟着预测走，定位误差越积越大。调了三天才发现是参数反了。所以，Q和R的物理意义一定要搞清楚：Q是「模型不可靠程度」，R是「传感器不可靠程度」。
1.5 一个直观的例子——一维位置估计
假设你用一个测距传感器测位置。传感器有噪声，方差R=1。你有一个运动模型，但模型也有噪声，方差Q=0.1。初始位置估计为0，不确定度P₀=100（非常不确定）。
第一次观测，z₁=1.2。我们来算一下：
预测：x̂₁⁻ = 0（假设没有控制输入）
P₁⁻ = 100 + 0.1 = 100.1
卡尔曼增益：K₁ = 100.1 / (100.1 + 1) ≈ 0.99
更新：x̂₁ = 0 + 0.99 × (1.2 - 0) ≈ 1.188
P₁ = (1 - 0.99) × 100.1 ≈ 1.001你看，因为初始不确定度很大（P₀=100），卡尔曼增益接近1，几乎完全相信观测。更新后，不确定度骤降到1左右。
第二次观测，z₂=1.5：
预测：x̂₂⁻ = 1.188
P₂⁻ = 1.001 + 0.1 = 1.101
卡尔曼增益：K₂ = 1.101 / (1.101 + 1) ≈ 0.524
更新：x̂₂ = 1.188 + 0.524 × (1.5 - 1.188) ≈ 1.351
P₂ = (1 - 0.524) × 1.101 ≈ 0.524这次，不确定度变小了，卡尔曼增益约0.5，预测和观测各信一半。这就是卡尔曼滤波的精髓——动态调整权重。
核心思想总结：卡尔曼滤波不是「信预测」或「信观测」二选一，而是根据当前的不确定度，自动计算最优权重。不确定度大，就多信观测；不确定度小，就多信预测。
1.6 本章小结
这一章我们聊了：
- 状态估计问题：从带噪声观测中猜真实状态
- 高斯分布：为什么它是卡尔曼滤波的数学基础
- 贝叶斯滤波思想：预测-更新的递归框架
- 5个核心公式：预测2个，更新3个
- 一维例子：直观理解卡尔曼增益的作用
下一章，我们会深入推导这5个公式，从数学上理解「为什么卡尔曼增益长那个样子」。到时候我会带大家手撕矩阵求导，别怕，跟着我一步步来。
嗯，今天就到这儿。有问题随时交流。