第1章卡尔曼滤波概述

1. 卡尔曼滤波概述：状态估计问题、贝叶斯滤波基础、卡尔曼滤波的诞生与历史

做导航融合这么多年，我经常被问到同一个问题：「卡尔曼滤波到底在干嘛？」

说白了，它解决的是状态估计问题。什么叫状态估计？你想想看——

你开着一辆车，想知道自己当前的位置。GPS告诉你一个位置，但可能有5米的误差。轮速计告诉你走了多远，但轮胎打滑会引入偏差。这时候你信谁？

嗯，谁都不能全信。你需要一个方法，把这两个「都不完美」的信息揉在一起，得到一个比任何一个都更准的结果。这就是状态估计。

核心思想：利用系统的动态模型（预测）和传感器观测（修正），在不确定中寻找最优估计。

我在项目中遇到过这样一个场景：一辆AGV在仓库里跑，激光雷达被货架遮挡了3秒钟。如果只用激光雷达定位，车就丢了。但加上轮式里程计的预测，这3秒内位置误差只漂了20厘米。等激光雷达重新看到特征点，卡尔曼滤波又把误差拉回来了。这就是状态估计的魅力。

状态估计问题有三个关键要素：

为什么要强调「随机」？因为如果误差是确定的，比如GPS总是偏北2米，那直接减掉2米就行了。但现实中的误差是随机的、时变的，所以我们才需要概率工具来处理它。

卡尔曼滤波的底层逻辑，其实是贝叶斯滤波的一个特例。

贝叶斯滤波的核心公式很简单：

后验概率 ∝ 似然 × 先验概率

翻译成人话就是：

为什么会这样？因为贝叶斯公式本质上是一个信息融合的过程。你有一个先验信念，然后拿到新的证据，更新你的信念。这个过程在导航中每时每刻都在发生。

个人经验：我刚开始学贝叶斯滤波时，总觉得「先验」「后验」这些词太抽象。后来我把它想象成「猜」和「测」——先猜一下，再测一下，然后取个折中。这样就好理解多了。

贝叶斯滤波的完整流程是：

但这里有个问题——贝叶斯滤波需要计算整个概率分布。对于连续状态空间，这个积分往往没有解析解。所以我们需要一些简化方法。

卡尔曼滤波就是其中一种简化。它假设：

在这三个假设下，贝叶斯滤波的积分问题就变成了一个解析可解的问题。高斯分布经过线性变换还是高斯分布，只需要传递均值和协方差就够了。

卡尔曼滤波是1960年由鲁道夫·卡尔曼（Rudolf Kalman）提出的。那年他刚30岁。

我记得第一次读到卡尔曼的原始论文时，最大的感受是：这哥们太聪明了。他把维纳滤波从频域搬到了时域，用状态空间模型替代了传递函数，使得滤波可以递推计算——不需要存储所有历史数据，只需要记住上一时刻的状态和协方差。

这个特性有多重要？你想想看，1960年的计算机内存有多大？可能还不如你现在手机里的一张照片大。如果每次滤波都要处理所有历史数据，那根本跑不动。卡尔曼滤波的递推形式，让它成了那个年代唯一可行的实时估计算法。

卡尔曼滤波的第一个重大应用是阿波罗登月计划。NASA需要一种方法，把地面雷达的测量和飞船上的惯性导航数据融合起来，实时估计飞船的位置和速度。卡尔曼滤波完美地解决了这个问题。

避坑指南：我曾经以为卡尔曼滤波是万能的，什么场景都能套。后来在项目中吃了亏——系统是非线性的，直接用标准卡尔曼滤波，结果发散得一塌糊涂。记住：标准卡尔曼滤波只适用于线性高斯系统。非线性系统要用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。

为什么卡尔曼滤波能活60年还这么火？我个人觉得有三个原因：

到今天，卡尔曼滤波已经成了导航融合的标配算法。无论是手机里的GPS+IMU融合，还是自动驾驶汽车里的多传感器融合，底层逻辑都是卡尔曼滤波。

嗯，说到这里，我想强调一点：卡尔曼滤波不是一个「黑盒」。你如果只调参数而不理解原理，遇到问题就会束手无策。我见过太多工程师，调了半年参数还是发散，最后发现是模型建错了。