从零开始学习自动驾驶系统-State Estimation & Localization(二)

最小二乘法(Least Squares Method)可以在存在测量噪声的情况下，可以最大限度的剥离噪声的影响，求得最优解。

多传感器融合方案(图片来源自网络)

自动驾驶车辆配备了许多的不同的传感器，它们的测量精度各不相同，有的精度高，有的精度低，而高精度设备的测量结果更加值得信任,如何达到这种效果呢。一个直觉的做法，就是赋予高精度的测量更大的权重，同时降低低质量的测量结果的贡献。Weighted Least Square Method 可以帮助达到这一目的。

1. Weighted Least Square Method

1.1 线性回归的一般形式:

其中：

是观测测量值，m 是观测测量值的数目。

是待估计参数, n 是未知参数的个数。一般情况下 m>n。

1.2 Weighted Least Square 的目标函数

我们假设目标变量

和输入变量

存在如下关系:

是测量误差项，它们独立同分布，并且服从标准正态分布

,即:

如果假设

独立，但是有不同的方差(variance)。

则 Weighted Least Squares Method 的目标函数可以定义如下:

1.3 Weighted Least Square 的矩阵解

令导数为 0，求解极值点:

可得到:

2. Weighted Least Squares 的应用举例

仍以前一篇文章提到的测量车辆位置为例，展示 Weighted Least Squares 的用法。

假设存在 m 个测量值和 n 个未知参数:

Weighted Least Squares 的目标函数如下:

其中:

令:

得到:

假设有激光雷达和卫星同时对自动驾驶车辆进行位置测量，测量结果如下:

代入上式:

可以看到，Weighted Least Square 的计算结果更接近于方差较小的激光雷达测量结果。