自動駕駛定位系統-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} $$

流程分析:

一、假設目標變量$y$和輸入變量$x$的關係以下:

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

$\epsilon^{(i)}$是測量偏差項。

二、$\epsilon$服從獨立同分布。

獨立同分布的定義：隨機過程當中，任什麼時候刻取值均爲隨機變量，若是這些隨機變量服從同一分佈，而且相互獨立，那麼這些隨機變量是獨立同分布。

三、假設$\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}}) $$

四、最大似然估計

聯合機率密度函數爲:

$$ 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...