第4章概率论基础回顾

4. 概率论基础回顾：高斯分布、协方差矩阵、贝叶斯估计思想

各位同学，欢迎来到第四章。

说实话，很多做惯性导航的工程师，一听到「概率论」三个字就头大。我当年刚入行时也一样，觉得搞算法就是解微分方程、算旋转矩阵，跟概率有什么关系？

后来在项目里栽了跟头才明白——卡尔曼滤波的核心，其实就是概率论。你想想看，传感器有噪声，模型有误差，我们永远无法知道真实姿态。那怎么办？用概率来描述它。

这一章，我们就来聊聊三个最基础的概念：高斯分布、协方差矩阵、贝叶斯估计。别怕，我会用我踩过的坑帮你理解。

核心观点：卡尔曼滤波本质上是在做「概率推理」——用高斯分布描述状态的不确定性，用协方差矩阵描述变量之间的关系，用贝叶斯公式融合先验和观测。

高斯分布，也叫正态分布。你肯定见过那个钟形曲线。

为什么卡尔曼滤波里到处都是高斯分布？两个原因：

一维高斯分布的概率密度函数长这样：

p(x) = (1 / sqrt(2πσ²)) * exp(-(x - μ)² / (2σ²))

其中 μ 是均值，σ² 是方差。均值告诉你「最可能的位置」，方差告诉你「有多不确定」。

举个例子。我做一个IMU静态标定实验，采集了1000组加速度计数据。如果传感器是好的，这些数据应该围绕真实值波动，画出来就是一条钟形曲线。方差越小，说明传感器越准。

我的经验：在实际项目中，我经常先画个直方图看看数据分布。如果明显不是高斯分布（比如双峰、长尾），那就要小心了——卡尔曼滤波的假设可能不成立。我曾经在一个振动剧烈的无人机上吃过这个亏，后来加了预处理才搞定。

一维高斯分布只能描述一个变量。但在姿态估计中，我们同时关心多个量——比如四元数的四个分量、角速度的三个轴。它们之间不是独立的。

举个例子。你转动一个IMU，绕X轴旋转时，Y轴和Z轴的读数也会受影响。这就是相关性。

协方差矩阵就是用来描述这种相关性的。它是一个对称矩阵，对角线元素是各个变量的方差，非对角线元素是协方差。

对于一个n维随机向量 x = [x₁, x₂, ..., xₙ]ᵀ，协方差矩阵定义为：

P = E[(x - μ)(x - μ)ᵀ]

展开来看：

对角线越大，说明这个变量越「不确定」。非对角线越大，说明两个变量越「相关」。

注意：协方差矩阵必须是半正定的。什么意思？就是它的特征值都大于等于0。如果你在代码里算出一个负特征值的协方差矩阵，那一定是算错了。我调试卡尔曼滤波时，经常打印协方差矩阵的特征值来检查数值稳定性。

贝叶斯估计的核心思想其实很简单：先猜一个，然后用观测数据修正。

公式长这样：

p(x|z) = p(z|x) * p(x) / p(z)

其中：

说白了，贝叶斯估计就是「用新信息更新旧信念」。

在卡尔曼滤波中，这个过程被完美地实现了：

而且，因为所有分布都是高斯分布，这个更新过程有闭式解——就是卡尔曼滤波的那五个公式。

我的理解：贝叶斯估计就像是一个「学习」过程。你一开始对姿态的估计可能很粗糙（先验方差大），但随着每次观测，不确定性逐渐减小。我做过一个实验：只用加速度计和磁力计做初始对准，然后开启卡尔曼滤波，后验协方差矩阵的对角线元素会迅速下降——这就是「学习」的过程。

下面这张图展示了本章三个核心概念之间的关系：

好了，这一章的内容就这些。我帮你捋一下重点：

说实话，这三个概念你搞懂了，卡尔曼滤波的骨架就有了。后面的章节，我们会在这些基础上一步步搭建完整的姿态估计算法。

嗯，下一章我们开始讲卡尔曼滤波的五个核心公式。到时候你会发现，原来那些公式就是今天这些概念的「数学翻译」。