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

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颗卫星，测量的车辆位置分别如下:

记五次测量结果:

，车辆位置的真值为:

，

是每次测量的噪声项，这些噪声项独立同分布。每次测量项可以表示为真值和噪声项的和。

每一次真值与测量值之间的误差为:

，而真值就是使得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