第3章最小二乘估计

第3章：最小二乘估计——批处理、递推与加权

各位同学，今天我们来聊聊最小二乘估计。说实话，这玩意儿是咱们做组合导航的看家本领。我刚开始接触导航算法时，觉得卡尔曼滤波才是主角，最小二乘就是个配角。后来在项目里摔过几次跟头才明白——没有扎实的最小二乘功底，卡尔曼滤波玩得再花哨也是空中楼阁。

3.1 最小二乘的思想：从一条直线说起

最小二乘的核心思想，说白了就是：找到一组参数，让模型预测值和实际观测值之间的误差平方和最小。

你想想看，我们手里有一堆带噪声的测量数据，想从中提取出真实的状态。比如用GPS测位置，每次测量都有误差，但多测几次取平均，精度就上去了。最小二乘就是这种「取平均」思想的数学推广。

核心公式：

对于线性模型 z = Hx + v，最小二乘估计为：

x̂ = (H^TH)^-1H^Tz

其中 z 是观测向量，H 是观测矩阵，v 是噪声。

这个公式看起来简单，但背后有大学问。我在做车载组合导航时，就遇到过H矩阵奇异的情况——说白了就是观测信息不足，导致没法唯一确定状态。嗯，这里要注意，H^TH必须可逆，这是最小二乘能用的前提。

3.2 批处理最小二乘：一次算完

批处理最小二乘，就是把所有历史数据攒到一起，一次性算出最优估计。它的优点是精度高，缺点是计算量大、不适合实时系统。

我记得在做一个无人机定位项目时，需要离线处理飞行数据。当时我就用了批处理最小二乘，把所有GPS和IMU数据堆在一起算。结果呢？精度确实好，但矩阵求逆那一步，数据量一大，计算时间蹭蹭往上涨。

我的经验：批处理适合事后分析，不适合实时导航。如果你在做实时系统，请直接跳到递推最小二乘。

批处理的步骤其实很清晰：

收集所有观测数据 z₁, z₂, ..., z_k
构建观测矩阵 H（把所有H_i堆叠起来）
计算 H^TH 和 H^Tz
求解 x̂ = (H^TH)^-1H^Tz

代码实现也很直接：

import numpy as np

def batch_least_squares(H, z):
    """
    批处理最小二乘
    H: 观测矩阵 (m x n)
    z: 观测向量 (m x 1)
    """
    # 计算法方程
    HtH = H.T @ H
    Htz = H.T @ z
    
    # 求解
    x_hat = np.linalg.inv(HtH) @ Htz
    
    return x_hat

3.3 递推最小二乘（RLS）：实时更新的利器

递推最小二乘，英文叫Recursive Least Squares，简称RLS。它的核心思想是：来了新数据，不用重新算一遍，而是在旧估计的基础上做修正。

为什么会这样？你想想看，批处理每次都要对全部数据重新计算，这在实时系统里根本跑不动。RLS的做法是：把旧估计当成「先验信息」，新数据来了，用卡尔曼增益类似的思路去更新。

RLS递推公式：

K_k = P_k-1H_k^T(H_kP_k-1H_k^T + R_k)^-1

x̂_k = x̂_k-1 + K_k(z_k - H_kx̂_k-1)

P_k = (I - K_kH_k)P_k-1

看到没？这个形式和卡尔曼滤波的更新步骤几乎一模一样。我当年学到这里时恍然大悟——卡尔曼滤波本质上就是带过程模型的递推最小二乘。

避坑指南：我曾经在RLS初始化时吃过亏。如果初始P矩阵设得太大，前几步估计会剧烈震荡；设得太小，收敛又太慢。我的建议是：P₀取单位阵乘以一个较大的数（比如100），让算法先快速收敛再说。

RLS的代码实现：

class RecursiveLeastSquares:
    def __init__(self, n, P0=None, R=1.0):
        """
        n: 状态维度
        P0: 初始协方差矩阵
        R: 观测噪声方差
        """
        self.x = np.zeros((n, 1))
        self.P = P0 if P0 is not None else np.eye(n) * 100
        self.R = R
    
    def update(self, z, H):
        """
        递推更新
        z: 当前观测值 (标量或向量)
        H: 当前观测矩阵
        """
        # 计算卡尔曼增益
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)
        
        # 更新状态
        innovation = z - H @ self.x
        self.x = self.x + K @ innovation
        
        # 更新协方差
        self.P = (np.eye(len(self.x)) - K @ H) @ self.P
        
        return self.x.copy()

3.4 加权最小二乘：给数据分个三六九等

加权最小二乘，说白了就是给不同质量的观测数据分配不同的权重。精度高的数据权重大，精度低的数据权重小。

我在做多传感器融合时，这个技巧帮了大忙。比如GPS和里程计，GPS在开阔地带精度高，但进了隧道就废了；里程计短时精度好，但长时间会漂移。用加权最小二乘，可以根据当前环境动态调整权重。

加权最小二乘公式：

x̂ = (H^TW^-1H)^-1H^TW^-1z

其中 W 是观测噪声协方差矩阵，权重就是 W^-1。

权重矩阵W怎么选？我的经验是：W取观测噪声的协方差矩阵。噪声大的传感器，W的对应元素就大，权重自然就小。这其实和卡尔曼滤波里的R矩阵是一个道理。

传感器	典型噪声水平	权重建议
GPS（开阔环境）	2-5米	高
GPS（城市峡谷）	10-30米	中
里程计（短时）	0.1-0.5%	高
里程计（长时）	累积漂移	低

3.5 三种方法的对比与选择

这三种方法各有各的用武之地。我给大家画个图，方便理解它们之间的关系：

三种方法的选择其实有规律可循：

离线数据处理：用批处理，精度最高，不关心计算时间
实时导航：用递推最小二乘，每来一个数据更新一次
多传感器融合：用加权最小二乘，或者直接上卡尔曼滤波

我的建议：初学者先吃透批处理最小二乘，理解最小二乘的本质。然后学RLS，理解递推的思想。最后学加权，理解如何融合不同质量的数据。这三步走完，卡尔曼滤波的大门就为你敞开了。

好了，这一章的内容就到这里。最小二乘是组合导航的基石，希望大家多动手写代码，多在实际数据上跑一跑。只有亲手踩过坑，才能真正理解这些算法的精髓。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录