第二章 基础数学工具：概率论与随机过程回顾、矩阵论基础、最小二乘估计
各位同学，欢迎来到第二章。说实话，这一章看起来有点「干」，但它是整个组合导航滤波算法的地基。我当年刚入行时，觉得这些数学工具太理论，结果第一次做卡尔曼滤波调参，就被协方差矩阵的奇异性问题折腾了一周。嗯，从那以后，我再也不敢小看这些基础了。
咱们今天要聊三块内容：概率论与随机过程、矩阵论、最小二乘估计。别急，我会用实际项目中的例子帮你串起来。
2.1 概率论与随机过程：导航中的不确定性
组合导航的核心是什么？说白了，就是处理不确定性。传感器有噪声，模型有误差，我们得用概率的语言来描述这些「不确定」。
2.1.1 随机变量与分布
我个人习惯把传感器的测量值看作随机变量。比如一个GPS位置输出，它并不是一个确定的点，而是服从某个概率分布。最常见的假设是高斯分布：
p(x) = (1 / sqrt(2πσ²)) * exp(-(x-μ)² / (2σ²))为什么大家都爱用高斯？两个原因：一是中心极限定理告诉我们，很多独立随机因素叠加的结果趋近于高斯；二是高斯分布数学性质好，线性变换后还是高斯。我在做IMU零偏标定时，就利用了这个性质——多个测量值平均后，噪声分布更接近高斯。
2.1.2 随机过程：时间上的不确定性
导航问题中，状态是随时间变化的。这时候单个随机变量不够用了，得用随机过程。比如陀螺仪的零偏漂移，它不是一个固定值，而是随时间缓慢变化——这就是一个随机过程。
最常用的模型是高斯-马尔可夫过程。为什么？因为它既描述了时间相关性，又保持了数学上的可操作性。我曾在某款低成本IMU上做过测试，用一阶高斯-马尔可夫模型拟合零偏漂移，效果比白噪声模型好了一个数量级。
核心概念：在组合导航中，我们通常假设过程噪声和测量噪声都是高斯白噪声。白噪声意味着不同时刻的噪声不相关，这大大简化了滤波器的设计。
2.1.3 协方差与相关性
这里有个容易踩的坑。两个传感器误差可能是相关的——比如GPS和IMU都受温度影响。如果你忽略了这种相关性，滤波器会过于自信，导致发散。
避坑指南：我曾经在一个项目中，直接假设GPS和气压计测量噪声不相关，结果高度估计误差越来越大。后来发现，两者都受大气压力变化影响，存在弱相关性。加入协方差项后，问题立刻解决了。
2.2 矩阵论基础：导航算法的「语言」
你想想看，组合导航的状态向量可能有15维甚至更高（位置、速度、姿态、零偏...）。不用矩阵，根本没法写公式。矩阵就是导航算法的「母语」。
2.2.1 矩阵运算与性质
最基本的几个操作：
- 转置：把行和列互换，用上标T表示
- 逆矩阵：A⁻¹，相当于矩阵的「除法」
- 迹：对角线元素之和，常用于优化问题
我个人最常用的是矩阵的迹。在卡尔曼滤波中，我们经常用迹来评估估计误差的大小——迹越小，说明整体估计越准。
2.2.2 特征值与特征向量
这个在导航中有什么用？可太有用了。协方差矩阵的特征值代表了各个方向上的不确定性大小。如果某个特征值特别大，说明那个方向上的估计很不确定——这往往是滤波器发散的早期信号。
我记得有一次调试车载组合导航系统，发现东向位置误差突然增大。一查协方差矩阵的特征值，果然东向对应的特征值在快速增大。这就是特征值分析给我们的预警。
2.2.3 矩阵求逆引理
这个公式在卡尔曼滤波中频繁出现：
(A + BCD)⁻¹ = A⁻¹ - A⁻¹B(C⁻¹ + DA⁻¹B)⁻¹DA⁻¹看着复杂，但它的实际意义是：当我们要更新一个高维矩阵的逆时，可以用低维矩阵的逆来「修正」，大大减少计算量。在嵌入式系统上做实时导航，这个技巧能省下宝贵的CPU时间。
实用技巧：在实际代码中，永远不要直接计算矩阵的逆。用QR分解或Cholesky分解来求解线性方程组，数值稳定性好得多。我见过太多因为直接求逆导致滤波器发散的案例了。
2.3 最小二乘估计：从数据中「提炼」真相
最小二乘估计是导航中最基础的估计方法。它的思想很简单：找到一组状态量，使得预测值与测量值的误差平方和最小。
2.3.1 线性最小二乘
假设我们有测量方程：z = Hx + v，其中v是噪声。最小二乘解为：
x̂ = (HᵀH)⁻¹Hᵀz这个公式看起来简单，但有个前提：HᵀH必须可逆。如果不可逆，说明你的测量信息不足，无法唯一确定状态。这就是「可观测性」问题——在组合导航中，我们经常需要检查系统是否可观测。
2.3.2 加权最小二乘
不同传感器的精度不一样。GPS精度可能是米级，而IMU短时精度很高。这时候要给不同测量值分配不同的权重：
x̂ = (HᵀW⁻¹H)⁻¹HᵀW⁻¹z其中W是测量噪声协方差矩阵。精度高的传感器，对应的权重就大。我在做GPS/INS紧组合时，就利用加权最小二乘来融合伪距和载波相位测量——效果比简单平均好得多。
2.3.3 递推最小二乘
导航是实时系统，数据是一点一点来的。递推最小二乘允许我们每来一个新测量，就更新一次估计，而不需要重新处理所有历史数据。
K = Pₖ₋₁Hᵀ(HPₖ₋₁Hᵀ + R)⁻¹
x̂ₖ = x̂ₖ₋₁ + K(z - Hx̂ₖ₋₁)
Pₖ = (I - KH)Pₖ₋₁你看，这个形式和卡尔曼滤波已经很像了。实际上，卡尔曼滤波就是递推最小二乘在动态系统上的推广。理解了最小二乘，你就已经一只脚踏进了卡尔曼滤波的大门。
本章核心逻辑：概率论描述不确定性，矩阵论提供计算工具，最小二乘给出估计方法。三者结合，就构成了组合导航滤波算法的数学基础。后面的卡尔曼滤波、粒子滤波，都是在这个地基上盖起来的高楼。
好了，这一章的内容就到这里。数学工具是枯燥的，但它们是后续所有算法的「燃料」。下一章我们会进入真正的滤波算法世界——相信我，有了这些基础，你会学得轻松很多。
公众号:蓝海资料掘金营，微信deep3321