自动驾驶定位系统-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 线性回归的一般形式:

$$ \begin{aligned} h_{\theta}(x) =& \theta_1 x_1 + \theta_2 x_2 + ... + \theta_n x_n \\ =& \theta^T X \end{aligned} $$

其中：

$$ x^{(i)} = \left[ \begin{matrix} x_1^{(i)} \\ x_2^{(i)} \\ \vdots \\ x_n^{(i)} \end{matrix} \right] X=\left[ \begin{matrix} (x^{(1)})^T \\ (x^{(2)})^T \\ \vdots \\ (x^{(m)})^T \end{matrix} \right] Y = \left[ \begin{matrix} y^{(1)} \\ y^{(2)} \\ \vdots \\ y^{(m)} \end{matrix} \right] $$

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

$$ \theta=\left[ \begin{matrix} \theta_1 \\ \theta_2 \\ \vdots \\ \theta_n \end{matrix} \right] $$

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

2.2 最小二乘的矩阵解:

$$ \begin{aligned} J(\theta_1, \theta_2, ..., \theta_n) =& \frac{1}{2} \sum_{i=1}^{n}(h_{\theta}(x^{(i)}) - y^{(i)})^2 \\ \end{aligned} $$

写成矩阵形式:

$$ \begin{aligned} J(\theta_1, \theta_2, ..., \theta_n) =& \frac{1}{2} \sum_{i=1}^{n}(h_{\theta}(x^{(i)}) - y^{(i)})^2 \\ =& \frac{1}{2} (X\theta - Y)^T(X\theta - Y) \\ =& \frac{1}{2} (\theta^TX^TX\theta - \theta^TX^TY - Y^TX\theta + Y^TY) \end{aligned} $$

根据高等数学的知识，我们知道，函数极值点出现在偏导数为0的位置。矩阵求导过程中用到矩阵迹的知识参见附录一。

$$ \begin{aligned} \frac{\partial J(\theta_1, \theta_2, ..., \theta_n)}{\partial \theta} =& \frac{\partial}{\partial \theta} \frac{1}{2} [(X \theta - Y)^T (X \theta - Y)] \\ =& \frac{\partial}{\partial \theta} \frac{1}{2} tr[(X \theta - Y)^T (X \theta - Y)] \\ = & \frac{1}{2} \frac{\partial}{\partial \theta} tr(\theta^TX^TX\theta - \theta^TX^TY - Y^TX\theta + Y^TY) \\ =& \frac{1}{2} [\frac{\partial tr(\theta^TX^TX\theta)}{\partial \theta} - \frac{\partial tr(\theta^TX^TY)}{\partial \theta} - \frac{\partial tr(Y^TX\theta)}{\partial \theta}] \\ =& \frac{1}{2} [\frac{\partial tr(\theta I \theta^TX^TX)}{\partial \theta} - \frac{\partial tr(\theta^TX^TY)}{\partial \theta} - \frac{\partial tr(Y^TX\theta)}{\partial \theta}] \\ = & \frac{1}{2} [X^TX\theta I + X^TX\theta I^T - 2X^TY] \\ = & X^TX\theta - X^TY \end{aligned} $$

在极点数，导数为0，令:

$$ \frac{\partial J(\theta_1, \theta_2, ..., \theta_n)}{\partial \theta} = X^TX\theta - X^TY = 0 $$

当$X^TX$可逆时，得到最小二乘的矩阵解形式:

$$ \theta =(X^TX)^{-1}X^TY $$

2.3 最小二乘的概率性解释

为什么目标函数的形式是：

$$ \begin{aligned} J =& \frac{1}{2} \sum_{i=1}^{n}(h_{\theta}(x^{(i)}) - y^{(i)})^2 \\ \end{aligned} $$

流程分析:

1、假设目标变量$y$和输入变量$x$的关系如下:

$$ y^{(i)} = \theta^Tx^{(i)} + \epsilon^{(i)} $$

$\epsilon^{(i)}$是测量误差项。

2、$\epsilon$服从独立同分布。

独立同分布的定义：随机过程中，任何时刻取值均为随机变量，如果这些随机变量服从同一分布，并且相互独立，那么这些随机变量是独立同分布。

3、假设$\epsilon$服从正态分布$\epsilon \sim N(0,\sigma^2)$，即概率密度函数$p(\epsilon^{(i)})=\frac{1}{\sqrt{2\pi} \sigma} exp({-\frac{\epsilon^{(i)^2}}{2\sigma^2}})$.

将$y^{(i)} = \theta^Tx^{(i)} + \epsilon^{(i)}$带入，得到:

$$ p(y^{(i)}|x^{(i)};\theta)=\frac{1}{\sqrt{2\pi} \sigma} exp({-\frac{(y^{(i)}-\theta^T x^{(i)})^2}{2\sigma^2}}) $$

4、最大似然估计

联合概率密度函数为:

$$ L(\theta) = P(Y|X;\theta) = \prod_{i=0}^m \frac{1}{\sqrt{2\pi} \sigma} exp({-\frac{(y^{(i)}-\theta^T x^{(i)})^2}{2\sigma^2}}) $$

$$ \begin{aligned} lnL(\theta)=&ln\prod_{i=1}^m \frac{1}{\sqrt{2\pi} \sigma} exp({-\frac{(y^{(i)}-\theta^T x^{(i)})^2}{2\sigma^2}}) \\ =& \sum_{i=1}^{m} \ln \frac{1}{\sqrt{2\pi} \sigma} - \sum_{i=1}^{m} {\frac{(y^{(i)}-\theta^T x^{(i)})^2}{2\sigma^2}} \\ =& m \ln \frac{1}{\sqrt{2\pi} \sigma} - \frac{1}{2 \sigma^2} \sum_{i=1}^{m} (y^{(i)}-\theta^T x^{(i)})^2 \end{aligned} $$

要使上述函数取得最大值，只需要:

$$ \frac{1}{2} \sum_{i=1}^{m} (y^{(i)}-\theta^T x^{(i)})^2 $$

取最小值即可。这也解释了线性回归要选用最小二乘作为衡量指标的原因。

3. Oridinary Least Squares使用举例

真实的无人车定位是一个非常复杂的系统，为了简化问题，我们假设定位系统是一维的。由于卫星定位系统存在误差，导致每颗卫星定位的位置都有一定的差异。无人车的真实的位置只有一个，但这个位置数值我们并不知道。

注意：真实的情况下，一颗卫星是不能实现定位的，为了举例方便，这里也做了简化。假设有5颗卫星，测量的车辆位置分别如下:

卫星编号	车辆位置
s1	1.85
s2	1.90
s3	1.82
s4	1.84
s5	1.87

记五次测量结果: $\{y_i | i=1,2...5\}$，车辆位置的真值为:$x$，$\{v_i | i=1,2...5\}$是每次测量的噪声项，这些噪声项独立同分布。每次测量项可以表示为真值和噪声项的和。

$y_1 = x + v_1$

$y_2 = x + v_2$

$y_3 = x + v_3$

$y_4 = x + v_4$

$y_5 = x + v_5$

每一次真值与测量值之间的误差为: $e_i = y_i - x$，而真值就是使得Square Error最小的$x$：$\hat{x} = {\arg\min}_w ({e_1}^2 + {e_2}^2 + {e_3}^2 + {e_4}^2 + {e_5}^2)$。

将误差该式写成矩阵的形式:

$$ e= \left[ \begin{matrix} e_1 \\ e_2 \\ e_3 \\ e_4 \\ e_5 \\ \end{matrix} \right] =Y - HX =\left[ \begin{matrix} y_1 \\ y_2 \\ y_3 \\ y_4 \\ y_5 \\ \end{matrix} \right]- \left[ \begin{matrix} 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ \end{matrix} \right] x $$

其中:

$$ H_{m \times n} = \left[ \begin{matrix} 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ \end{matrix} \right] $$

m是测量的次数，n是待估计的未知参数的个数。

$$ \begin{aligned} J =& {e_1}^2 + {e_2}^2 + {e_3}^2 + {e_4}^2 + {e_5}^2 \\ =& e^T e \\ =& (Y - HX)^T (Y - HX) \\ =& Y^T Y - X^T H^TY - Y^THX + X^TH^THX \end{aligned} $$

根据上面的推导，$J^{\prime}(x) = 0$时，取得极值

$$ x =(H^TH)^{-1}H^TY = \frac{1}{5} [1, 1, 1, 1, 1]\left[ \begin{matrix} 1.85 \\ 1.90 \\ 1.82 \\ 1.84 \\ 1.87 \\ \end{matrix} \right] = 1.856 $$

Oridinary Least Squares中各个测量结果的值的权重(weight)是相等的。但是在实际的应用中，我们明确知道，有些设备的测量结果比其它设备要好，它的权重就比其它测量结果高，这就是:Weighted Least Squares.

附录一: 扩展数学知识

矩阵迹的定义:

矩阵A_{n times n}的迹是指A主对角线上所有元素的和，记为tr(A),即：

$$ tr(A) = \sum_{i=1}^{n} a_{ii} $$

定理一: tr(AB) = tr(BA)

证明:

$$ \begin{aligned} tr(AB) =& \sum_{i=1}^{n} ({AB})_{ii} \\ =& \sum_{i=1}^{n}\sum_{j=1}^{n} a_{ij} b{ji} \\ =& \sum_{i=1}^{n}\sum_{j=1}^{n} b{ji} a_{ij} \\ =& \sum_{i=1}^{n} ({BA})_{ii} \\ =& tr(BA) \\ \end{aligned} $$

定理二: tr(ABC) = tr(CAB) = tr(BCA)

证明: 把AB或者BC当做一个整体，可知显然成立。

定理三: $\frac{\partial tr({AB})}{\partial A}=\frac{\partial tr({BA})}{\partial A}=B^T$，其中A是mxn矩阵，B是nom矩阵。

证明:
由

$$ \begin{aligned} tr(AB) =& \sum_{i=1}^{m} ({AB})_{ii} \\ =& \sum_{i=1}^{m}\sum_{j=1}^{n} a_{ij} b{ji} \\ \end{aligned} $$

可得:

$$ \frac{\partial tr({AB})}{\partial a_{ij}} = b_{ji} $$

因此:

$$ \frac{\partial tr({AB})}{\partial A} = B^T $$

定理四: $\frac{\partial tr({A^TB})}{\partial A}=\frac{\partial tr({BA^T})}{\partial A}=B$

证明： 证明过程与定理三相同。

定理五: $tr(A) = tr(A^T)$

证明很简单，忽略。

定理六: $tr(a) = a$

证明很简单，忽略。

定理七: $\frac{\partial tr({ABA^TC})}{\partial A}=CAB + C^TAB^T$

证明：根据变量多次出现的求导法则:

$$ \begin{aligned} \frac{\partial tr({ABA^TC})}{\partial A} =& \frac{\partial tr({ABA_c^TC})}{\partial A} + \frac{\partial tr({A_cBA^TC})}{\partial A} \\ =&(BA_c^TC)^T + CA_cB \\ =& C^TAB^T + CAB \\ \end{aligned} $$

参考链接

https://www.jianshu.com/p/eda...
https://blog.csdn.net/sddfsAv...
https://www.coursera.org/lear...

---

个人网站地址: http://www.banbeichadexiaojiu...