Localization目标是确定自动驾驶车辆在全局坐标系内的位置(Position)和方向(Orientation)，精确的Localization系统是任何自动驾驶汽车的关键组成部分。为了实现精确的Localization系统，需要使用State Estimation，从不精确的各种传感器的测量结果中，找到最优解作为车辆的定位位置。
本文主要介绍State Estimation中一种常用的基础技术:Ordinary Least Squares Method。
1.Least Squares的历史
1801年，意大利天文学家Giuseppe Piazzi发现了一颗小行星-谷神星，经过40多天的跟踪观测后，由于谷神星运行至太阳背后，使得Piazzi失去了谷神星的位置。由于谷神星的直径只有900公里，再次定位它非常困难。为了帮助再次定位谷神星，高斯（carl friedrich gauss）发明了Least Squares，根据piazza公布的测量数据，精确估计出谷神星轨道参数。奥地利天文学家海因里希·奥尔伯斯根据高斯计算出来的轨道重新发现了谷神星。
2.Oridinary Least Squares Method
普通最小二乘法(OLS)是一种用于在线性回归模型中估计未知参数的线性最小二乘法。
2.1 线性回归的一般形式:
其中：
是观测测量值，m是观测测量值的数目。
是待估计参数, n是未知参数的个数。一般情况下m>n。
2.2 最小二乘的矩阵解:
写成矩阵形式:
根据高等数学的知识，我们知道，函数极值点出现在偏导数为0的位置。矩阵求导过程中用到矩阵迹的知识参见附录一。
在极点数，导数为0，令:
当
可逆时，得到最小二乘的矩阵解形式:
2.3 最小二乘的概率性解释
为什么目标函数的形式是：
流程分析:
1、假设目标变量
和输入变量
的关系如下:
是测量误差项。
2、
服从独立同分布。
独立同分布的定义：随机过程中，任何时刻取值均为随机变量，如果这些随机变量服从同一分布，并且相互独立，那么这些随机变量是独立同分布。
3、假设
服从正态分布
即概率密度函数:
将
带入，得到:
4、最大似然估计
联合概率密度函数为:
要使上述函数取得最大值，只需要:
取最小值即可。这也解释了线性回归要选用最小二乘作为衡量指标的原因。
3. Oridinary Least Squares使用举例
image
真实的无人车定位是一个非常复杂的系统，为了简化问题，我们假设定位系统是一维的。由于卫星定位系统存在误差，导致每颗卫星定位的位置都有一定的差异。无人车的真实的位置只有一个，但这个位置数值我们并不知道。
注意：真实的情况下，一颗卫星是不能实现定位的，为了举例方便，这里也做了简化。假设有5颗卫星，测量的车辆位置分别如下:
卫星编号 | 车辆位置 |
|---|---|
s1 | 1.85 |
s2 | 1.90 |
s3 | 1.82 |
s4 | 1.84 |
s5 | 1.87 |
记五次测量结果:
，车辆位置的真值为:
，
是每次测量的噪声项，这些噪声项独立同分布。每次测量项可以表示为真值和噪声项的和。
每一次真值与测量值之间的误差为:
，而真值就是使得Square Error最小的
：
将误差该式写成矩阵的形式:
其中:
m是测量的次数，n是待估计的未知参数的个数。
根据上面的推导，
时，取得极值
Oridinary Least Squares中各个测量结果的值的权重(weight)是相等的。但是在实际的应用中，我们明确知道，有些设备的测量结果比其它设备要好，它的权重就比其它测量结果高，这就是:Weighted Least Squares.
附录一: 扩展数学知识
矩阵迹的定义:
矩阵
的迹是指A主对角线上所有元素的和，记为tr(A),即：
定理一: tr(AB) = tr(BA)
证明:
定理二: tr(ABC) = tr(CAB) = tr(BCA)
证明: 把AB或者BC当做一个整体，可知显然成立。
定理三:
其中A是mxn矩阵，B是nxm矩阵。
证明: 由
可得:
因此:
定理四:
证明： 证明过程与定理三相同。
定理五:
证明很简单，忽略。
定理六:
证明很简单，忽略。
定理七:
证明：根据变量多次出现的求导法则:
参考链接
https://www.jianshu.com/p/edaf949bcaeb
https://blog.csdn.net/sddfsAv/article/details/88804644
https://www.coursera.org/learn/state-estimation-localization-self-driving-cars
