SLAM中的加权最小二乘法
一、数学描述
机器人携带传感器在环境中运动可由 运动方程 和 观测方程 描述。
其中 表示时刻；
表示
时刻的位姿；
是运动传感器的读数或者输入；
为路标点；
表示观测数据。
为运动噪声，例如对机器人下达了前进 1m 的指令，机器人实际前进了 0.9m。
为观测噪声 ，例如前方障碍物距离机器人 1m，但传感器测量得到的值为 1.1m。
这两种噪声也类似于卡尔曼滤波中的噪声，参考 一文详解卡尔曼滤波两处噪声的来源及影响 和 过程噪声和测量噪声。
其中 表示高斯分布，
表示零均值，
为协方差矩阵。在这些噪声的影响下，我们希望通过带噪声的数据
和
推断位姿
和地图
（以及它们的概率分布），这构成了一个状态估计问题。
运动方程和观测方程可以表述为：
其中 运动误差 和 观测误差 可表述为：
构造加权最小二乘问题为：
其中协方差矩阵之逆 和
提供了最小二乘问题的权重分布。
二、 协方差矩阵之逆提供了最小二乘问题的权重分布
1. 协方差的定义
首先，协方差是衡量两个随机变量之间关系的统计量。对于两个变量 和
，它的协方差定义为：
- 如果
，意味着 X 和 Y 趋向于同时增大或减小（正相关）。
- 如果
，意味着 X 增大时 Y 倾向于减小（负相关）。
- 如果
，则 X 和 Y 之间没有线性相关性。
-
的绝对值越大，表示两个变量的同向（正相关）或反向（负相关）关系越强。
2. 协方差矩阵
协方差矩阵 Σ 是一个对称矩阵，用于描述多维随机变量的协方差结构。设一个 n 维随机变量 X=[X1,X2,...,Xn]，其协方差矩阵定义为：
- 对角线上元素是各观测误差的方差 Var(Xi)。
- 非对角线元素是不同观测值误差之间的协方差 Cov(Xi,Xj)，反映了不同观测值误差之间的相关性。
3. 最小二乘法与加权最小二乘法
在经典的最小二乘法中，我们试图通过最小化目标函数来找到一组最优参数：
假设观测数据 yi 存在观测误差（观测噪声）。如果这些误差是均匀的且相互独立，我们可以直接使用普通最小二乘法。但是，当这些误差的方差不同时，或者误差之间存在相关性时，普通最小二乘法不再是最佳选择。此时，使用加权最小二乘法（Weighted Least Squares, WLS），目标函数变为：
其中，wi 是观测值 yi 的权重。观测值的误差越小（更准确），它对结果的贡献越大，所以赋予它的权重 wi 越大。
4. 协方差矩阵的逆与权重分布
在加权最小二乘法中，权重 wi 通常来源于观测值的误差协方差矩阵的逆，也就是说：
是观测误差的协方差矩阵。
是协方差矩阵的逆，它提供了观测数据的权重分布。
为什么协方差矩阵的逆提供权重分布？
-
误差的方差：对角线元素 Var(Xi) 代表观测值 yi 的误差的方差。如果某个观测值的方差较大，它的误差就大。为了降低这些大误差观测对拟合结果的影响，我们在权重中给它分配较小的值。由于权重与方差成反比，方差矩阵的逆就体现了这个“反比”关系：误差大，权重小；误差小，权重大。
-
误差的协方差：非对角线元素 Cov(Xi,Xj)代表观测值 yi 和 yj 之间的误差相关性。如果 yi 和 yj 的误差相关性很强（
5. 直观理解
协方差矩阵之逆 可以看作一个“校正因子”，用来对观测数据进行加权。具体地：
- 如果某些观测值的误差较小（方差小），它们的权重就大，意味着它们对拟合结果贡献更大。
- 如果某些观测值的误差较大（方差大），它们的权重就小，意味着它们对拟合结果的影响被减弱。
- 如果观测值之间存在相关性（非对角线协方差非零），协方差矩阵的逆会根据这些相关性调整每个观测的权重，避免相关性带来的重复信息影响结果。
6. 总结
协方差矩阵的逆在加权最小二乘法中起到了分配观测值权重的作用：
- 它通过对误差方差和相关性的分析，决定每个观测值对最终结果的影响力。
- 协方差矩阵之逆越大的元素，对应的观测值权重越大，意味着该观测值更“可信”或重要。
- 这种权重分布能够优化拟合过程，使得误差小、相互独立的观测值对结果贡献更大，而误差大、相互相关的观测值影响更小。