数学基础回顾：高斯分布、协方差矩阵、矩阵运算基础、最小二乘思想
说实话，很多做导航的朋友一上来就啃卡尔曼滤波公式，结果卡在数学推导上。我当年也是这样，看了三天推导，头都大了。后来发现，真正需要掌握的数学基础，其实就那么几块。今天咱们就把这些底子打牢。
我个人习惯，学任何算法前，先把数学工具理清楚。就像修车，你得先认识扳手和螺丝刀，对吧？
1. 高斯分布：噪声的“标准语言”
卡尔曼滤波为什么这么火？因为它假设所有噪声都服从高斯分布。说白了，高斯分布就是描述“随机误差”的数学工具。
一个一维高斯分布由两个参数决定：
- 均值 μ：分布的中心位置，也就是最可能的值
- 方差 σ²：分布的离散程度，值越大，数据越分散
公式长这样：
p(x) = (1 / √(2πσ²)) * exp(-(x-μ)² / (2σ²))嗯，这里要注意：方差越小，曲线越“瘦高”，说明我们对这个值的把握越大。我在做GPS/INS组合导航时，经常用高斯分布来建模传感器的噪声特性。比如GPS的水平定位误差，通常可以近似为均值为0的高斯分布。
核心要点：卡尔曼滤波的“高斯假设”意味着，我们只需要传递均值和协方差，就能完整描述状态的不确定性。这是整个滤波器的数学基础。
2. 协方差矩阵：多维变量的“关系网”
导航问题从来不是一维的。位置、速度、姿态，这些状态变量之间是相互影响的。协方差矩阵就是用来描述这种“相互影响”的数学工具。
举个例子，假设我们有两个变量：位置 x 和速度 v。协方差矩阵长这样：
P = | σ²_x σ_xv |
| σ_xv σ²_v |其中：
- 对角线元素 σ²_x、σ²_v：各自的不确定性（方差）
- 非对角线元素 σ_xv：两者的相关性。正值表示正相关，负值表示负相关
我曾经在调试一个无人机定位系统时，发现位置和速度的协方差突然变得很大。排查了半天，原来是IMU的加速度计零偏没校准好。你看，协方差矩阵不仅能告诉你“误差有多大”，还能告诉你“误差之间怎么互相影响”。
避坑指南：我曾经遇到过协方差矩阵变成非正定的情况，导致滤波器直接发散。后来养成了习惯，每次更新后都检查一下矩阵的正定性。这是血泪教训。
3. 矩阵运算基础：卡尔曼滤波的“算术”
卡尔曼滤波的五个公式，本质上就是矩阵的加减乘除和求逆。如果你矩阵运算不熟，那看公式就像看天书。
需要掌握的核心操作：
- 矩阵乘法：注意维度匹配。A(m×n) × B(n×p) = C(m×p)
- 矩阵转置：把行和列互换，用上标 T 表示
- 矩阵求逆：只有方阵才能求逆。逆矩阵相当于矩阵的“除法”
- 单位矩阵 I：对角线为1，其余为0。任何矩阵乘以单位矩阵等于它自己
举个实际例子，卡尔曼增益的计算：
K = P * H^T * (H * P * H^T + R)^(-1)你看，这里面有乘法、转置、求逆。如果你能把这个公式的维度推一遍，矩阵运算就算过关了。
注意：矩阵乘法不满足交换律！A×B ≠ B×A。我见过不少新手在这里栽跟头，写代码时把顺序搞反了，结果滤波器直接炸了。
4. 最小二乘思想：卡尔曼滤波的“灵魂”
最小二乘，说白了就是“找到一组参数，让预测值和观测值的误差平方和最小”。卡尔曼滤波本质上就是递推的最小二乘。
经典的最小二乘问题：
给定观测值 z = H * x + v
求 x 使得 ||z - H*x||² 最小解是：
x_hat = (H^T * H)^(-1) * H^T * z你想想看，卡尔曼滤波的更新步骤，是不是也在做类似的事？它用新观测值来修正先验估计，本质上就是在线求解最小二乘问题。
我记得有一次做多传感器融合，用了最小二乘做初始对准，效果出奇的好。后来干脆把最小二乘作为卡尔曼滤波的初始化步骤，收敛速度快了不少。
个人经验：如果你觉得卡尔曼滤波难理解，不妨先把它看作“带权重的递推最小二乘”。权重就是协方差矩阵，它告诉滤波器“哪个传感器更可信”。这个视角让我豁然开朗。
知识体系总览
下面这张图，是我自己总结的本章知识结构。建议你保存下来，学完整个课程后再回来看，会有更深的理解。
好了，数学基础就聊到这儿。高斯分布给了我们噪声模型，协方差矩阵描述了不确定性，矩阵运算是计算工具，最小二乘提供了优化思想。这四样东西，就是卡尔曼滤波的“四根柱子”。
下一章，咱们就要开始搭房子了——从贝叶斯滤波开始，一步步推导出卡尔曼滤波的五个公式。到时候你会发现，有了今天的基础，那些公式其实没那么可怕。