2、数学基础（上）：高斯分布、协方差矩阵、矩阵运算回顾

各位同学，欢迎来到第二讲。

说实话，很多做导航融合的工程师，最后栽跟头的地方往往不是卡尔曼滤波本身，而是数学基础。我见过太多人，公式背得滚瓜烂熟，一上真机就炸。为什么？因为没搞懂背后的数学直觉。

今天这一讲，咱们就把高斯分布、协方差矩阵、还有矩阵运算这几个硬骨头啃下来。别怕，我会用我踩过的坑来帮你铺路。

2.1 高斯分布：为什么它无处不在？

先问个问题：为什么卡尔曼滤波里到处都是高斯分布？

说白了，因为高斯分布是自然界最诚实的噪声模型。你想想看，传感器测量值，比如GPS的位置、IMU的加速度，它们的误差大多服从高斯分布——中间大、两头小，偏离均值越远的概率越低。

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

我在做无人机定位时遇到过一件事：GPS信号受多径效应干扰，误差分布变成了双峰。这时候再用高斯假设去套，卡尔曼滤波直接发散。嗯，这里要注意——高斯分布假设不成立时，你得考虑其他滤波器，比如粒子滤波。

2.2 协方差矩阵：不确定性怎么量化？

一维情况很简单，一个方差 σ² 就搞定了。但导航融合里，状态向量通常是多维的，比如位置 (x, y, z) 加上速度 (vx, vy, vz)。这时候，我们不光要知道每个变量的方差，还得知道它们之间的相关性。

协方差矩阵就是干这个的。

P = [[σx², σxy],
     [σyx, σy²]]

举个例子：你开车时，位置和速度是强相关的。位置误差大，速度误差通常也大。协方差矩阵能捕捉这种关系，让滤波器知道「位置不准时，速度也别太信」。

2.3 矩阵运算回顾：加减乘除与转置

卡尔曼滤波里全是矩阵运算。别慌，你只需要掌握几个基本操作。

2.3.1 矩阵加减法

对应元素相加减。维度必须相同。

A = [[1, 2], [3, 4]]
B = [[5, 6], [7, 8]]
A + B = [[6, 8], [10, 12]]

2.3.2 矩阵乘法

这个容易出错。我刚开始做时，经常把维度搞反。

规则：A (m×n) 乘以 B (n×p)，得到 C (m×p)。C 的 (i, j) 元素是 A 的第 i 行与 B 的第 j 列的点积。

2.3.3 矩阵转置

把行变成列，列变成行。用上标 T 表示。

A = [[1, 2],
     [3, 4]]
A^T = [[1, 3],
       [2, 4]]

2.3.4 矩阵求逆

这是卡尔曼滤波里最耗时的操作。对于方阵 A，如果存在 B 使得 A*B = I，则 B 是 A 的逆。

实际工程中，我很少手动求逆。一般用数值稳定的方法，比如 Cholesky 分解或 QR 分解。你想想看，嵌入式平台资源有限，直接求逆容易数值不稳定。

2.4 协方差矩阵的传播：状态更新时它怎么变？

卡尔曼滤波的核心思想之一：状态更新了，不确定性也要跟着更新。

假设我们有线性系统：

x_k = F * x_{k-1} + B * u_k + w_k

其中 w_k 是过程噪声，协方差为 Q。

那么协方差矩阵的预测公式是：

P_k = F * P_{k-1} * F^T + Q

这个公式我建议你手推一遍。F 乘以 P 再乘以 F 的转置，本质上是在做坐标变换。Q 是新增的不确定性。

我记得有一次做轮式机器人定位，轮子打滑导致过程噪声 Q 设小了。结果滤波器过于自信，位置估计越来越偏。后来我把 Q 调大了一倍，效果立竿见影。

2.5 总结与下节预告

今天咱们聊了：

• 高斯分布：噪声建模的基础
• 协方差矩阵：量化多维不确定性
• 矩阵运算：加减乘除、转置、求逆
• 协方差传播：状态更新时不确定性怎么变

这些是卡尔曼滤波的数学骨架。下一讲，我们会深入「数学基础（下）」，聊聊条件概率、贝叶斯公式，以及它们怎么和卡尔曼滤波联系起来。

嗯，今天就到这里。回去把矩阵乘法练熟，下次课咱们直接上卡尔曼滤波的五个核心公式。