3、数学基础回顾（二）：非线性优化基础
好，咱们接着聊。上一节我们把状态估计的数学框架搭起来了，这一节要动真格的了——怎么把那些看起来很美的公式，变成能在嵌入式芯片上跑起来的代码。
说实话，我刚入行那会儿，觉得非线性优化就是个黑盒子。调个参数，跑个迭代，能收敛就行。直到有一次在无人机上做VIO，飞着飞着位置就飘了，排查了一整天，最后发现是LM算法的阻尼因子没处理好。从那以后，我再也不敢轻视这些“基础”了。
3.1 从最小二乘说起
先问个问题：为什么VIO里到处都是最小二乘？
你想想看，我们观测到的像素坐标有噪声，IMU的加速度和角速度也有噪声。每个测量值都不完美，但又都有用。怎么办？折中呗。最小二乘就是干这个的——让所有误差的平方和最小。
数学上长这样：
minimize ∑ ||e_i||²其中 e_i 是第 i 个观测的残差。比如重投影误差，就是特征点投影到图像上的位置，跟实际观测到的位置之间的差。
但这里有个坑：如果某个传感器特别不准，或者某个观测明显是野值，你还把它跟其他误差平等对待，那结果就崩了。所以就有了加权最小二乘：
minimize ∑ e_i^T · W_i · e_i这个 W_i 就是权重矩阵。权重越大，这个观测在优化中的话语权就越大。
3.2 马氏距离：给误差加个“尺子”
说到权重，就不得不提马氏距离。我见过不少新手，上来就用欧氏距离算残差，结果优化出来的轨迹歪歪扭扭的。
为什么？因为欧氏距离假设所有维度是独立同分布的。但现实中，比如IMU的加速度计，三个轴的噪声方差可能不一样，而且轴之间还有相关性。这时候用欧氏距离，相当于无视了这些统计特性。
马氏距离的定义是：
d_M = sqrt( (x - μ)^T · Σ^{-1} · (x - μ) )其中 Σ 是协方差矩阵。说白了，就是用数据的分布特性来“归一化”距离。如果某个方向上的噪声大，那在这个方向上的偏差就会被容忍一些。
核心理解： 马氏距离 = 在数据空间里，用协方差矩阵做了一次“白化”处理，然后再算欧氏距离。
在VIO里，我们通常把马氏距离直接嵌入到最小二乘的代价函数中：
代价 = (z - h(x))^T · Σ^{-1} · (z - h(x))这里的 Σ^{-1} 就是信息矩阵，它告诉优化器：哪些测量值更可信。
我的习惯： 在写代码时，我会把协方差矩阵的逆（信息矩阵）提前算好存起来，避免每次迭代都求逆。尤其是当传感器数量多的时候，这个优化能省不少CPU。
3.3 高斯牛顿法：最朴素的迭代思路
好了，代价函数有了，怎么求最小值？
最直接的想法：对代价函数求导，令导数为零。但代价函数通常是非线性的（比如重投影误差里有个投影矩阵），没法直接解。
那就迭代呗。高斯牛顿法的思路很简单：
- 在当前估计值 x_k 处，把代价函数做一阶泰勒展开（线性化）
- 解这个线性化后的最小二乘问题，得到增量 Δx
- 更新 x_{k+1} = x_k + Δx
- 重复直到收敛
数学推导我就不啰嗦了，直接给核心公式：
J^T · J · Δx = -J^T · r其中 J 是残差对状态量的雅可比矩阵，r 是残差向量。
这个方程叫正规方程。解它，你就得到了增量方向。
我曾经踩过的坑： 高斯牛顿法要求 J^T·J 是可逆的。如果状态量之间有冗余（比如VIO中的尺度模糊），这个矩阵就是奇异的，解出来的增量会乱跳。所以实际中，我一般不会裸用高斯牛顿，至少加个阻尼。
3.4 LM算法：给增量加个“刹车”
LM算法（Levenberg-Marquardt）就是在高斯牛顿的基础上，加了一个阻尼项：
(J^T · J + λ · I) · Δx = -J^T · r这个 λ 就是阻尼因子。当 λ 很大时，LM退化为梯度下降法（步长小，但稳定）；当 λ 很小时，LM退化为高斯牛顿法（收敛快，但可能发散）。
LM的精髓在于动态调整 λ：
- 如果这一步让代价下降了，就减小 λ，让算法更“激进”
- 如果这一步让代价上升了，就增大 λ，让算法更“保守”
判断依据是增益比（gain ratio）：
ρ = (实际下降量) / (预测下降量)如果 ρ 接近1，说明线性化很准，可以减小 λ；如果 ρ 很小甚至为负，说明线性化不准，要增大 λ。
我建议： 在嵌入式平台上，λ 的初始值可以设成 J^T·J 的迹乘以一个小系数（比如1e-6）。这样能自动适应不同量级的雅可比矩阵，省得你手动调参。
3.5 状态估计问题的完整流程
把上面这些串起来，一个典型的VIO状态估计步骤是这样的：
- 预测：用IMU数据做一次状态传播（运动模型）
- 观测：拿到新的图像特征点（观测模型）
- 构建代价函数：把预测残差和观测残差加起来，每个残差都乘上对应的信息矩阵
- 线性化：在当前状态附近，对每个残差求雅可比
- 求解增量：用LM算法解正规方程
- 更新状态：把增量加到当前状态上
- 判断收敛：如果增量足够小，或者迭代次数到了，就停止；否则回到第4步
下面这张图可以帮你理清整个逻辑：
3.6 工程中的几个关键细节
理论说完了，聊点实际的。我在嵌入式平台上做VIO优化时，有几个地方特别容易出问题：
| 问题 | 表现 | 我的做法 |
|---|---|---|
| 雅可比计算错误 | 优化发散，或者收敛到错误值 | 用数值微分做一次校验，确保解析雅可比正确 |
| 信息矩阵未归一化 | 不同传感器量纲差异大，优化被某个传感器主导 | 检查信息矩阵的迹，确保各传感器贡献大致相当 |
| LM阻尼因子初始化不当 | 前期迭代太慢，或者直接发散 | 用 J^T·J 的迹乘以1e-6作为初始λ |
| 浮点精度不足 | 在MCU上跑时，正规方程求解误差大 | 必要时用double，或者用QR分解代替直接求逆 |
特别提醒： 在嵌入式平台上，矩阵求逆是个大坑。我见过有人直接用 cv::Mat::inv()，结果在ARM Cortex-M上跑一次要几十毫秒。建议用Cholesky分解或者QR分解，速度快一个数量级。
3.7 小结
这一节我们聊了：
- 最小二乘的本质：在噪声中找最优折中
- 马氏距离：用协方差矩阵给误差加权
- 高斯牛顿法：线性化+迭代求解
- LM算法：加个阻尼，稳如老狗
- 完整的VIO状态估计流程
这些数学工具，说白了就是帮你把传感器数据“拧”成一条平滑的轨迹。下一节我们会把这些东西串起来，看看一个完整的VIO系统到底是怎么工作的。