3、数学基础（下）：最小二乘估计、加权最小二乘、递推最小二乘

好，咱们接着聊数学基础。上一节我们把概率论和矩阵论过了一遍，那些是理解卡尔曼滤波的“语法”。今天要讲的这三个工具——最小二乘、加权最小二乘、递推最小二乘——才是真正干活的“动词”。

我个人习惯把最小二乘看作一切估计问题的起点。你想想看，卡尔曼滤波本质上不就是一种递推的最小二乘吗？只不过它加上了系统模型和噪声统计特性。所以把这几个概念吃透，后面看卡尔曼滤波的公式就不会觉得那么突兀了。

3.1 最小二乘估计（Least Squares, LS）

先说最朴素的版本。假设你有一堆测量数据，想拟合出一条直线。比如我测了某个传感器的输出电压和实际物理量的关系，数据点有误差，但我知道它们大致呈线性。

最小二乘的思想很简单：让预测值和测量值的误差平方和最小。为什么是平方？因为平方能放大大误差的惩罚，而且数学上方便求导。

数学形式是这样的：

设有测量方程：z = Hx + v
其中 z 是测量向量，H 是观测矩阵，x 是待估计的状态，v 是噪声。

最小二乘的目标：min ||z - Hx||²

解为：x̂ = (HᵀH)⁻¹Hᵀz

这个公式你肯定见过。但我要提醒一点：这里没有用到任何噪声的统计信息。它假设所有测量值的“可信度”是一样的。这在现实中很少成立。

我在项目中遇到过这样的情况：用最小二乘做IMU的零偏标定，结果总是不太对。后来发现是因为不同姿态下的测量噪声方差不一样，但最小二乘把它们一视同仁了。嗯，这就是它的局限性。

3.2 加权最小二乘（Weighted Least Squares, WLS）

加权最小二乘就聪明多了。它给每个测量值分配一个权重，权重越大，这个测量在估计中的“话语权”就越大。

说白了，就是告诉算法：这个测量靠谱，多信它一点；那个测量噪声大，少信它一点。

数学上，权重矩阵 W 通常取测量噪声协方差矩阵的逆：W = R⁻¹。为什么？因为协方差大的测量，权重小；协方差小的测量，权重大。很直观对吧？

加权最小二乘的目标：min (z - Hx)ᵀW(z - Hx)

解为：x̂ = (HᵀWH)⁻¹HᵀWz

你对比一下普通最小二乘的公式，其实就是多了个 W。但效果天差地别。

加权最小二乘还有一个重要性质：当权重取为噪声协方差的逆时，它是最小方差无偏估计（MVU）。也就是说，在所有线性无偏估计中，它的估计误差方差最小。卡尔曼滤波的测量更新步骤，本质上就是在做这件事。

3.3 递推最小二乘（Recursive Least Squares, RLS）

好，现在问题来了。前面两种方法都是批处理的——你得把所有数据收集齐了，然后一次性算。但在导航系统中，数据是源源不断来的。你总不能每来一个新测量，就把历史数据全部重算一遍吧？

递推最小二乘就是来解决这个问题的。它的核心思想是：用上一时刻的估计值，加上当前测量的修正，得到新的估计值。是不是听起来很耳熟？对，卡尔曼滤波也是这个套路。

递推公式如下：

初始化：
x̂₀ = 0, P₀ = αI (α取一个较大的数)

对于每个新测量 (zₖ, Hₖ)：
Kₖ = Pₖ₋₁Hₖᵀ(HₖPₖ₋₁Hₖᵀ + Rₖ)⁻¹
x̂ₖ = x̂ₖ₋₁ + Kₖ(zₖ - Hₖx̂ₖ₋₁)
Pₖ = (I - KₖHₖ)Pₖ₋₁

你看这个 Kₖ，它叫增益矩阵。它决定了新测量对估计值的修正力度。如果测量噪声 Rₖ 很大，Kₖ 就小，新测量基本被忽略；如果 Rₖ 很小，Kₖ 就大，新测量被充分信任。

遗忘因子 λ（0 < λ ≤ 1）的引入，让RLS能跟踪时变系统：

Kₖ = Pₖ₋₁Hₖᵀ(HₖPₖ₋₁Hₖᵀ + λRₖ)⁻¹
Pₖ = (I - KₖHₖ)Pₖ₋₁ / λ

λ 越小，算法对旧数据的遗忘越快，跟踪能力越强，但估计噪声也越大。我一般取 λ = 0.95 ~ 0.99，具体看系统变化快慢。

3.4 三者关系与导航应用

最后，我把这三个方法放在一起对比一下：

方法	数据使用方式	噪声统计需求	计算量	适用场景
最小二乘	批处理	不需要	低	离线标定、静态拟合
加权最小二乘	批处理	需要噪声协方差	中	传感器融合、精度要求高
递推最小二乘	在线递推	需要噪声协方差	低（每步）	实时导航、在线参数辨识

在导航融合中，我个人的经验是：

• 离线标定用最小二乘就够了，简单粗暴。
• 多传感器静态融合用加权最小二乘，精度更高。
• 实时导航必须用递推最小二乘或卡尔曼滤波，因为数据是流式到达的。

其实，卡尔曼滤波可以看作是递推最小二乘在动态系统上的推广。它多了状态预测这一步，用系统模型来“预判”下一时刻的状态，然后再用测量来修正。这个我们后面会详细讲。

好，数学基础部分就到这里。下一节我们开始进入卡尔曼滤波的核心内容。到时候你会发现，今天讲的这些东西，每一个都会反复出现。