企业官网建设流程全解析

1. 这不是数学考试，而是给真实传感器数据“把脉”的手艺

贝叶斯滤波这个词，一听到就容易让人联想到黑板上密密麻麻的积分符号、条件概率公式和一堆希腊字母。但我在某高校实验室带学生做移动机器人定位项目时发现，真正卡住大家的从来不是公式本身，而是根本不知道这些符号在现实世界里对应着什么——那个不断抖动的激光雷达读数，那个偶尔跳变的IMU角速度，那个在走廊拐角处突然失准的里程计，它们才是贝叶斯滤波要真正打交道的对象。贝叶斯滤波（三）贝叶斯滤波算法推导，这个标题里的“推导”二字，绝不是为了炫技或应付考试，而是为了搞清楚：当传感器数据像潮水一样涌来时，我们手里的状态估计值，到底是怎么一帧一帧被“冲刷”、被“重塑”、又被“锚定”下来的。它解决的核心问题非常朴素：如何让一个系统在持续接收噪声干扰的观测数据的同时，始终对自身状态（比如位置、速度、姿态）保持一份既不过分保守、也不盲目轻信的判断？适合谁来学？如果你正在调试无人机悬停、开发扫地机建图、优化车载ADAS的融合定位，或者只是想弄明白手机GPS为什么在隧道里还能短暂维持方向感——那你不是在学一个抽象算法，而是在掌握一种处理不确定性的底层思维。我试过直接扔给学生一整页贝叶斯递推公式，结果他们盯着P(xₖ|z₁:ₖ)发呆；后来我改用一个真实的AGV小车轨迹数据集，让他们亲手把每一帧激光扫描点、每一毫秒轮速编码器脉冲，一步步代入到状态更新和预测步骤中，两小时后，有人脱口而出：“哦，原来‘预测’就是靠运动模型猜下一步在哪，‘更新’就是用新看到的墙来校正这个猜测！”——这种顿悟，比背下十个公式都管用。推导的过程，本质上是一次对物理世界因果链条的严谨复盘：前一时刻我大概在哪（先验），我打算怎么动（运动模型），我实际看到了什么（观测模型），这些信息如何加权融合（似然函数），最终给出一个更靠谱的位置（后验）。它不承诺消除噪声，而是教会系统如何与噪声共处，并从中提取出最可能的真相。

2. 从直觉到公式的四步拆解：为什么必须是这个结构？

2.1 核心思路的本质：一个闭环的“猜-看-校-存”循环

贝叶斯滤波的整个框架，可以被压缩成四个字：“猜、看、校、存”。这不是比喻，而是对算法每一步物理意义的精准还原。很多初学者误以为推导是为了得到一个终极闭式解，其实恰恰相反——推导的核心价值在于确认这个四步循环结构的不可替代性和必然性。我们来看为什么：

• “猜”（Prediction）：系统不会凭空知道下一刻的状态。它必须依赖一个运动模型（Motion Model），比如小车匀速直线运动、无人机受重力和推力影响的六自由度动力学。这个模型描述了“如果我现在状态是xₖ₋₁，且控制输入是uₖ（比如电机PWM值），那么理论上我下一刻最可能的状态xₖ会是什么？”数学上，这表现为一个状态转移概率分布P(xₖ|xₖ₋₁, uₖ)。注意，这里已经引入了不确定性：模型本身有误差（轮胎打滑、空气阻力建模不准），控制输入也有噪声（电压波动导致推力微变），所以“猜”的结果不是一个确定的点，而是一个以理论值为中心、呈高斯分布（或其他形式）的“可能性云团”。

• “看”（Measurement / Observation）：与此同时，传感器睁开了眼。激光雷达扫到了一堵墙的距离，摄像头识别出了一个路标，IMU测得了角加速度。这些原始数据zₖ，本身也裹挟着噪声（激光的散射、图像的像素量化、IMU的零偏漂移）。因此，“看”到的结果，不能直接等同于真实状态。它需要通过一个观测模型（Observation Model）来翻译，即建立“如果真实状态是xₖ，那么我理论上应该观测到什么样的zₖ？”这个关系，其数学表达就是似然函数P(zₖ|xₖ)。它回答的是：在所有可能的真实状态中，哪一个最能“解释”我刚刚看到的这个观测值？

• “校”（Update / Correction）：这是贝叶斯滤波的灵魂所在。它把“猜”的结果（先验分布P(xₖ|z₁:ₖ₋₁)）和“看”的结果（似然P(zₖ|xₖ)）进行融合。融合的规则，就是贝叶斯定理：后验 = 先验 × 似然 / 归一化常数。这里的“×”不是简单的数字相乘，而是两个概率分布的逐点相乘（Point-wise multiplication）。想象一下，你有一张“猜”的地图（先验，显示你可能在A区、B区、C区，概率各不相同），又有一张“看”的地图（似然，显示只有在B区和C区才最有可能看到那堵特定的墙），两张地图叠在一起，“校”出来的结果，就是只保留A、B、C三个区域中重叠度最高的那块——也就是B区和C区，且B区的概率会因为“看”的证据更强而被进一步放大。这个过程天然地实现了加权融合：当“猜”很准（先验方差小）、“看”很模糊（似然方差大）时，后验就更偏向“猜”；反之，当“看”很清晰（比如GPS在开阔地带）、“猜”很飘（比如长时间无里程计更新），后验就更听“看”的话。

• “存”（Storage / Recursion）：最后一步看似简单，却至关重要。上一步得到的后验分布P(xₖ|z₁:ₖ)，立刻成为下一时刻的先验分布P(xₖ|xₖ₋₁, uₖ)的起点。这个“存”，不是把一个数字存进变量，而是把整个概率分布（通常是它的均值和协方差）作为下一循环的初始状态。它保证了信息的时序连续性和记忆性。没有这一步，每一次更新都是孤立的，系统永远无法从历史数据中学习和积累置信度。

这个“猜-看-校-存”的闭环，是任何在线状态估计算法都无法绕开的逻辑骨架。卡尔曼滤波、粒子滤波、无迹卡尔曼滤波，它们的区别，仅仅在于如何具体实现这四步——比如用高斯分布近似所有概率，还是用一堆粒子来采样；用线性化处理非线性，还是用确定性采样点。但骨架本身，由贝叶斯定理所决定，是铁律。

2.2 方案选型背后的硬逻辑：为什么非得用概率分布，而不是单个数值？

一个尖锐的问题常被提出：“既然最终我们只用一个均值（比如位置坐标）去控制机器人，那为什么费这么大劲去维护整个概率分布（均值+协方差），而不是直接维护一个‘最优估计值’？”这个问题直指核心，答案也异常硬核：因为单个数值无法承载‘不确定性’这一关键信息，而不确定性，是决策的基石。

举个真实案例。某次调试室内服务机器人导航时，我们发现它在靠近玻璃幕墙时频繁发生“幻觉”——明明前方是透明的，它却坚称那里有一堵墙。问题排查到最后，根源在于激光雷达的观测模型。当激光束以接近垂直的角度打在玻璃上时，大部分光会穿透过去，只有极少量被反射回来，导致测距值随机跳变，有时是无穷远（没收到回波），有时是几厘米（表面反射）。如果我们只用一个“最优距离”来代表观测，这个值要么是无穷远（导致规划器认为前方畅通无阻），要么是几厘米（导致规划器认为前方有障碍物），系统会在两种极端间疯狂震荡。而当我们用一个宽泛的、多峰的似然分布P(zₖ|xₖ)来建模这个观测时，情况就完全不同了：这个分布会明确地告诉状态估计算法——“对于任何可能的真实距离xₖ，我观测到这个zₖ的可能性都很低，但尤其低在那些‘正常’的距离上；而在‘无穷远’和‘几厘米’这两个点上，可能性略高一点”。当这个“虚弱”的似然与一个相对集中的先验（比如机器人刚从走廊过来，位置比较确定）相乘时，后验分布虽然也会变宽、变模糊，但它会清晰地显示出“我不确定前方有没有墙，但我的不确定性主要集中在两个极端上”。这个信息，会被下游的路径规划器直接读取：它会据此生成一条“谨慎靠近、边走边探”的策略，而不是一条“全速撞向玻璃”或“原地不动”的傻瓜指令。

另一个例子是协方差矩阵的作用。在卡尔曼滤波中，状态协方差Pₖ不仅告诉你估计有多准，更关键的是，它参与了卡尔曼增益Kₖ的计算：Kₖ = Pₖ⁻Hᵀ(H Pₖ⁻Hᵀ + R)⁻¹。这里的R是观测噪声协方差。Kₖ本质上是一个动态权重：当Pₖ⁻很大（预测很不准），R很小（观测很准）时，Kₖ趋近于1，意味着几乎完全相信新观测；当Pₖ⁻很小（预测很准），R很大（观测很噪）时，Kₖ趋近于0，意味着几乎忽略新观测。这个精妙的自适应机制，完全依赖于对“不确定性”的量化。一个单点估计，永远无法计算出Kₖ，也就永远无法实现这种智能的、情境感知的融合。

因此，推导贝叶斯滤波，本质上是在推导一套如何严谨地量化、传播和利用不确定性的方法论。它不是数学家的智力游戏，而是工程师为机器赋予“审慎”这一人类高级认知能力的必经之路。

2.3 避坑指南：推导中三个最容易被忽略的“魔鬼细节”

在带领不同背景的学生（从数学系到机械系）推导贝叶斯滤波时，我发现有三个细节，教科书常常一笔带过，但实操中一旦出错，后果就是整个滤波器发散、估计值乱飞。它们不是公式里的主角，却是支撑整个大厦的地基。

提示：第一个魔鬼细节——“条件独立性”假设的物理含义与失效场景
贝叶斯滤波推导中，一个关键的简化步骤是假设“当前观测zₖ只与当前状态xₖ有关，而与之前的所有状态和观测无关”，即P(zₖ|xₖ, z₁:ₖ₋₁, x₁:ₖ₋₁) = P(zₖ|xₖ)。这个“观测马尔可夫性”假设，在绝大多数标准传感器（激光、IMU、GPS）下是成立的。但它的失效场景极具欺骗性。例如，当使用视觉里程计（VO）时，当前帧的特征匹配结果，不仅取决于当前相机位姿xₖ，还强烈依赖于上一帧的特征点位置和质量（即xₖ₋₁）。如果强行套用P(zₖ|xₖ)模型，滤波器会将VO的累积误差误认为是状态本身的剧烈跳变，从而过度修正，导致轨迹抖动。解决方案不是抛弃贝叶斯框架，而是扩展状态向量，把上一帧的关键特征点坐标也纳入xₖ中，使P(zₖ|xₖ)重新成立。这提醒我们：推导不是为了套公式，而是为了理解公式的适用边界。

提示：第二个魔鬼细节——“归一化常数”的计算陷阱
在贝叶斯定理P(xₖ|z₁:ₖ) ∝ P(zₖ|xₖ) P(xₖ|z₁:ₖ₋₁)中，那个“∝”（正比于）后面的归一化常数∫P(zₖ|xₖ) P(xₖ|z₁:ₖ₋₁)dxₖ，理论上需要在整个状态空间上积分。对于高斯分布，这个积分有解析解；但对于粒子滤波，它就是一个巨大的计算黑洞。很多初学者会试图去精确计算它，结果发现粒子权重和为1的代码永远跑不完。正确的做法是：接受它只是一个比例因子，只计算权重的相对大小。粒子滤波中，我们为每个粒子i计算未归一化的权重wᵢ⁽ᵏ⁾ ∝ P(zₖ|xᵢ⁽ᵏ⁾)，然后用所有wᵢ⁽ᵏ⁾的和来归一化。这个“和”就是归一化常数的蒙特卡洛近似。推导时理解这一点，能让你在选择算法时，一眼看穿其计算复杂度的本质。

提示：第三个魔鬼细节——“预测步”中运动模型噪声的注入时机
运动模型通常写作xₖ = f(xₖ₋₁, uₖ) + wₖ，其中wₖ是过程噪声。推导预测步P(xₖ|z₁:ₖ₋₁) = ∫P(xₖ|xₖ₋₁, uₖ) P(xₖ₋₁|z₁:ₖ₋₁) dxₖ₋₁时，噪声wₖ是内嵌在P(xₖ|xₖ₋₁, uₖ)里的。但实操中，很多人会错误地先用f(xₖ₋₁, uₖ)算出一个确定性预测，再单独加一个噪声样本。这在EKF/UKF中会导致线性化点错误；在粒子滤波中，则会破坏粒子的统计一致性。正确的方式是：在生成每个粒子的预测时，必须对每个粒子独立地采样一次wₖ。这意味着，即使所有粒子起始于同一个xₖ₋₁，经过预测步后，它们也会因为不同的wₖ而散开，形成一个自然的、符合物理规律的“可能性云团”。这个细节，决定了你的滤波器是“有温度的”（能体现真实物理扰动），还是“冷冰冰的”（只是一个数学玩具）。

3. 手把手推导：从贝叶斯定理到卡尔曼增益的完整链条

3.1 第一步：建立完整的贝叶斯递推框架（通用形式）

一切推导的起点，是写出贝叶斯滤波最本源的递推公式。我们定义：

• xₖ：k时刻的系统状态向量（如[x, y, θ, v]ᵀ，表示位置、朝向、速度）。
• zₖ：k时刻的观测向量（如[laser_dist, camera_feature_x, imu_gyro_z]ᵀ）。
• uₖ：k时刻的控制输入向量（如[left_wheel_pwm, right_wheel_pwm]ᵀ）。
• z₁:ₖ：从时刻1到k的所有观测序列。

我们的目标，是求出后验概率密度函数（Posterior PDF）：P(xₖ|z₁:ₖ)，即在已知所有历史观测的前提下，对当前状态的最佳概率描述。

根据贝叶斯定理，这个后验可以分解为：P(xₖ|z₁:ₖ) = P(zₖ|xₖ, z₁:ₖ₋₁) * P(xₖ|z₁:ₖ₋₁) / P(zₖ|z₁:ₖ₋₁)

这个公式本身已经包含了“猜-看-校-存”的全部基因。我们来逐项解读并简化：

• 分子第一项P(zₖ|xₖ, z₁:ₖ₋₁)：这就是前面提到的观测似然。根据“观测马尔可夫性”假设，它简化为P(zₖ|xₖ)。这是观测模型的核心，它编码了传感器的物理特性。例如，对于一个理想激光雷达，如果真实距离是xₖ，那么观测zₖ服从均值为xₖ、方差为σ²的高斯分布：P(zₖ|xₖ) = N(zₖ; xₖ, σ²)。

• 分子第二项P(xₖ|z₁:ₖ₋₁)：这就是先验概率密度函数，也就是“猜”的结果。它代表了在看到zₖ之前，我们对xₖ的所有知识。它不能凭空产生，必须由上一时刻的后验P(xₖ₋₁|z₁:ₖ₋₁)，通过运动模型（或称状态转移模型）传播而来。其数学表达为全概率公式：P(xₖ|z₁:ₖ₋₁) = ∫ P(xₖ|xₖ₋₁, uₖ) * P(xₖ₋₁|z₁:ₖ₋₁) dxₖ₋₁这里，P(xₖ|xₖ₋₁, uₖ)就是过程模型，它描述了系统动力学。例如，对于一个匀速运动的小车，xₖ = xₖ₋₁ + uₖ*Δt + wₖ，其中wₖ是过程噪声，P(xₖ|xₖ₋₁, uₖ) = N(xₖ; xₖ₋₁ + uₖ*Δt, Q)，Q是过程噪声协方差。

• 分母P(zₖ|z₁:ₖ₋₁)：这是一个归一化常数，确保后验PDF在整个状态空间上的积分为1。它本身没有直接的物理意义，但其计算至关重要。我们可以将其视为“在所有可能的xₖ下，观测到zₖ的总可能性”，即：P(zₖ|z₁:ₖ₋₁) = ∫ P(zₖ|xₖ) * P(xₖ|z₁:ₖ₋₁) dxₖ这个积分，正是我们在粒子滤波中用粒子权重和来近似的对象。

将以上所有代入，我们就得到了贝叶斯滤波的通用递推公式：P(xₖ|z₁:ₖ) ∝ P(zₖ|xₖ) * ∫ P(xₖ|xₖ₋₁, uₖ) * P(xₖ₋₁|z₁:ₖ₋₁) dxₖ₋₁

这个公式，就是所有后续具体算法（KF, EKF, UKF, PF）的共同祖先。它清晰地划定了两个核心计算阶段：

1. 预测步（Predict）：计算P(xₖ|z₁:ₖ₋₁)，即那个积分。
2. 更新步（Update）：用P(zₖ|xₖ)去“校正”预测步的结果，得到最终的P(xₖ|z₁:ₖ)。

3.2 第二步：引入高斯假设，走向卡尔曼滤波（KF）

通用公式虽然优美，但计算上是不可行的，因为积分和乘法操作在任意分布上都没有解析解。为了让它落地，我们必须做出一个强大而实用的假设：所有相关的概率分布都是高斯分布（正态分布）。这个假设并非凭空而来，它有坚实的物理基础：中心极限定理告诉我们，大量独立微小噪声的叠加，其效果往往近似高斯分布；同时，高斯分布在数学上具有完美的封闭性——高斯分布的线性变换、高斯分布之间的乘法、高斯分布的卷积（即积分），其结果仍然是高斯分布。这为我们提供了强大的计算便利。

因此，我们假设：

• 先验P(xₖ₋₁|z₁:ₖ₋₁) = N(x̂ₖ₋₁, Pₖ₋₁)，其中x̂ₖ₋₁是均值（即我们当前的最佳估计），Pₖ₋₁是协方差（即我们对这个估计的不确定性）。
• 过程模型P(xₖ|xₖ₋₁, uₖ) = N(xₖ; Fₖxₖ₋₁ + Bₖuₖ, Qₖ)，其中Fₖ是状态转移矩阵（线性化后的运动模型），Bₖ是控制输入矩阵，Qₖ是过程噪声协方差。
• 观测模型P(zₖ|xₖ) = N(zₖ; Hₖxₖ, Rₖ)，其中Hₖ是观测矩阵（将状态映射到观测空间），Rₖ是观测噪声协方差。

现在，我们开始分步推导。

预测步（Predict）的高斯化

我们需要计算P(xₖ|z₁:ₖ₋₁) = ∫ N(xₖ; Fₖxₖ₋₁ + Bₖuₖ, Qₖ) * N(xₖ₋₁; x̂ₖ₋₁, Pₖ₋₁) dxₖ₋₁。

这是一个高斯分布对另一个高斯分布的卷积。其结果依然是一个高斯分布，其均值和协方差有标准公式：

• 预测均值（先验估计）：x̂ₖ⁻ = Fₖx̂ₖ₋₁ + Bₖuₖ
• 预测协方差（先验不确定性）：Pₖ⁻ = FₖPₖ₋₁Fₖᵀ + Qₖ

这个结果非常直观：预测的均值，就是用运动模型对上一时刻的均值进行线性变换；预测的协方差，则是上一时刻的不确定性（Pₖ₋₁）经过运动模型（Fₖ）的“拉伸”和“扭曲”后，再加上新的过程噪声（Qₖ）的贡献。Pₖ⁻总是大于或等于Pₖ₋₁，这完美体现了“预测会让不确定性增大”这一物理直觉。

更新步（Update）的高斯化

现在，我们有了先验P(xₖ|z₁:ₖ₋₁) = N(x̂ₖ⁻, Pₖ⁻)，以及似然P(zₖ|xₖ) = N(zₖ; Hₖxₖ, Rₖ)。我们需要计算后验P(xₖ|z₁:ₖ) ∝ N(zₖ; Hₖxₖ, Rₖ) * N(xₖ; x̂ₖ⁻, Pₖ⁻)。

两个高斯分布的乘法，其结果也是一个高斯分布。其均值和协方差的计算，是整个推导中最精华的部分。我们不直接套用公式，而是展示其推导逻辑，因为这能让你真正理解卡尔曼增益Kₖ的来源。

两个高斯PDF的乘积，等价于它们的指数部分相加。一个高斯PDF的指数部分是-1/2 (x - μ)ᵀΣ⁻¹(x - μ)。因此，后验的指数部分为：-1/2 (zₖ - Hₖxₖ)ᵀRₖ⁻¹(zₖ - Hₖxₖ) - 1/2 (xₖ - x̂ₖ⁻)ᵀ(Pₖ⁻)⁻¹(xₖ - x̂ₖ⁻)

我们的目标是把这个二次型整理成-1/2 (xₖ - x̂ₖ)ᵀPₖ⁻¹(xₖ - x̂ₖ)的标准形式，从而直接读出后验的均值x̂ₖ和协方差Pₖ。

展开并合并同类项，可以得到：-1/2 xₖᵀ [HₖᵀRₖ⁻¹Hₖ + (Pₖ⁻)⁻¹] xₖ + xₖᵀ [HₖᵀRₖ⁻¹zₖ + (Pₖ⁻)⁻¹x̂ₖ⁻] + const

对比标准二次型-1/2 xₖᵀPₖ⁻¹xₖ + xₖᵀPₖ⁻¹x̂ₖ + const，我们可以立即得出：

• 后验协方差：Pₖ⁻¹ = HₖᵀRₖ⁻¹Hₖ + (Pₖ⁻)⁻¹对其求逆，得到著名的协方差更新公式：Pₖ = (Pₖ⁻)⁻¹ + HₖᵀRₖ⁻¹Hₖ)⁻¹

• 后验均值：Pₖ⁻¹x̂ₖ = HₖᵀRₖ⁻¹zₖ + (Pₖ⁻)⁻¹x̂ₖ⁻将上面求得的Pₖ⁻¹代入，并稍作整理，即可得到：x̂ₖ = x̂ₖ⁻ + PₖHₖᵀRₖ⁻¹(zₖ - Hₖx̂ₖ⁻)

观察这个均值更新公式，它完美地呈现了“校”的过程：x̂ₖ = x̂ₖ⁻ + Kₖ(zₖ - Hₖx̂ₖ⁻)。其中，(zₖ - Hₖx̂ₖ⁻)是新息（Innovation），即观测值与“基于预测的期望观测值”之间的残差，它代表了新信息的“含量”。而Kₖ = PₖHₖᵀRₖ⁻¹，就是大名鼎鼎的卡尔曼增益。

3.3 第三步：卡尔曼增益Kₖ的物理意义与工程解读

Kₖ = PₖHₖᵀRₖ⁻¹这个公式，是卡尔曼滤波的心脏。它不是一个需要死记硬背的魔咒，而是一个充满物理智慧的决策函数。我们可以从多个角度来解读它。

角度一：维度分析（Dimensional Analysis）
让我们检查Kₖ的单位。假设状态xₖ是位置[m]，观测zₖ是距离[m]，那么：

• Pₖ的单位是[m²]（协方差），
• Hₖ是一个纯数字（将位置映射到距离的系数，比如1），
• Rₖ的单位是[m²]（观测噪声方差）。 所以Kₖ的单位是[m²] / [m²] = 1，即无量纲。这说明Kₖ是一个纯权重，它决定了新息(zₖ - Hₖx̂ₖ⁻)应该以多大的比例被采纳到最终估计中。如果Kₖ=0.1，就意味着我们只采纳了新息的10%来修正预测；如果Kₖ=0.9，就意味着我们采纳了90%。

角度二：不确定性驱动的自适应
Kₖ的大小，完全由Pₖ⁻和Rₖ决定。回忆一下，Pₖ⁻是预测的不确定性，Rₖ是观测的不确定性。Kₖ的公式可以变形为：Kₖ = Pₖ⁻Hₖᵀ(HₖPₖ⁻Hₖᵀ + Rₖ)⁻¹

这个形式更清晰地揭示了其本质：Kₖ是预测不确定性在“预测的观测不确定性”与“真实观测不确定性”之和中所占的比例。HₖPₖ⁻Hₖᵀ是预测状态x̂ₖ⁻经过观测模型Hₖ映射后，在观测空间中产生的不确定性（即我们“猜”的观测值有多不准）；Rₖ是观测本身有多不准。两者相加，就是“总的不确定性”。Kₖ，就是“猜的不准”占“总不准”的比重。因此：

• 当Pₖ⁻很大（预测很飘），Rₖ很小（观测很准）时，Kₖ ≈ 1，我们几乎全信观测。
• 当Pₖ⁻很小（预测很稳），Rₖ很大（观测很噪）时，Kₖ ≈ 0，我们几乎不信观测。
• 当两者相当，Kₖ就取一个中间值，实现完美的加权平均。

角度三：实时计算的工程捷径
在实际嵌入式系统中，我们并不需要每次都计算Pₖ的逆矩阵（计算量大且不稳定）。标准的卡尔曼滤波实现，采用了一种更稳健、更高效的递推方式：

1. 计算新息协方差：Sₖ = HₖPₖ⁻Hₖᵀ + Rₖ
2. 计算卡尔曼增益：Kₖ = Pₖ⁻HₖᵀSₖ⁻¹
3. 更新状态估计：x̂ₖ = x̂ₖ⁻ + Kₖ(zₖ - Hₖx̂ₖ⁻)
4. 更新状态协方差：Pₖ = (I - KₖHₖ) Pₖ⁻

这个流程，将复杂的矩阵求逆，转化为了对一个通常维度更低的Sₖ矩阵（其维度等于观测向量zₖ的维度）的求逆，大大降低了计算负担。这也是为什么卡尔曼滤波能在资源受限的无人机飞控芯片上实时运行的原因。

4. 实操现场：用Python手写一个二维小车定位滤波器

4.1 场景设定与代码骨架搭建

我们来构建一个极简但真实的场景：一辆在XY平面上运动的小车，它装备了一个伪GPS（提供带噪声的全局位置观测）和一个里程计（提供带噪声的相对位移观测）。我们的目标是，仅用这两套廉价传感器，实时估计小车的精确位置[x, y]ᵀ。

• 状态向量：x = [x, y]ᵀ（2维）
• 控制输入：u = [Δx_odom, Δy_odom]ᵀ（里程计报告的位移）
• 观测向量：z = [x_gps, y_gps]ᵀ（GPS报告的位置）

根据这个设定，我们可以写出：

• 运动模型（里程计）：xₖ = xₖ₋₁ + uₖ + wₖ，其中wₖ ~ N(0, Q)。这里，Fₖ = I₂（2x2单位阵），Bₖ = I₂。
• 观测模型（GPS）：zₖ = I₂ * xₖ + vₖ，其中vₖ ~ N(0, R)。所以Hₖ = I₂。

现在，我们用Python从零开始实现这个滤波器。核心是两个函数：predict()和update()。

import numpy as np import matplotlib.pyplot as plt class KalmanFilter2D: def __init__(self, x0, P0, Q, R): """ 初始化卡尔曼滤波器 :param x0: 初始状态估计 [x, y] :param P0: 初始协方差矩阵 2x2 :param Q: 过程噪声协方差 2x2 :param R: 观测噪声协方差 2x2 """ self.x = np.array(x0).reshape(-1, 1) # 状态向量，列向量 self.P = np.array(P0) # 协方差矩阵 self.Q = np.array(Q) # 过程噪声协方差 self.R = np.array(R) # 观测噪声协方差 # 固定的模型矩阵（因为我们假设是恒等变换） self.F = np.eye(2) self.H = np.eye(2) self.B = np.eye(2) def predict(self, u): """ 预测步：基于里程计输入u，预测下一时刻状态 :param u: 控制输入 [Δx, Δy] """ u = np.array(u).reshape(-1, 1) # x_k^- = F * x_{k-1} + B * u self.x = self.F @ self.x + self.B @ u # P_k^- = F * P_{k-1} * F^T + Q self.P = self.F @ self.P @ self.F.T + self.Q def update(self, z): """ 更新步：基于GPS观测z，校正预测 :param z: 观测向量 [x_gps, y_gps] """ z = np.array(z).reshape(-1, 1) # 1. 计算新息 (Innovation) y = z - self.H @ self.x # 2. 计算新息协方差 S = H * P^- * H^T + R S = self.H @ self.P @ self.H.T + self.R # 3. 计算卡尔曼增益 K = P^- * H^T * S^{-1} K = self.P @ self.H.T @ np.linalg.inv(S) # 4. 更新状态估计 x_k = x_k^- + K * y self.x = self.x + K @ y # 5. 更新协方差 P_k = (I - K * H) * P_k^- I = np.eye(self.P.shape[0]) self.P = (I - K @ self.H) @ self.P

这段代码，就是贝叶斯滤波推导成果的直接物化。它没有调用任何高级库（除了numpy的基础运算），每一行都对应着推导公式中的一个环节。predict()函数实现了x̂ₖ⁻ = Fₖx̂ₖ₋₁ + Bₖuₖ和Pₖ⁻ = FₖPₖ₋₁Fₖᵀ + Qₖ；update()函数则完整实现了从新息计算到协方差更新的五步。

4.2 模拟数据生成与滤波效果可视化

为了验证我们的滤波器，我们需要生成模拟的“真实”轨迹、带噪声的里程计数据和带噪声的GPS数据。

# 生成真实轨迹：一个简单的圆形路径 np.random.seed(42) # 固定随机种子，保证结果可重现 N = 100 # 时间步数 true_x = np.zeros((N, 2))