自動駕駛系統定位與狀態估計- Weighted Least Square Method

最小二乘法(Least Squares Method)能夠在存在測量噪聲的狀況下，能夠最大限度的剝離噪聲的影響，求得最優解。

自動駕駛車輛配備了許多的不一樣的傳感器，它們的測量精度各不相同，有的精度高，有的精度低，而高精度設備的測量結果更加值得信任,如何達到這種效果呢。一個直覺的作法，就是賦予高精度的測量更大的權重，同時下降低質量的測量結果的貢獻。Weighted Least Square Method能夠幫助達到這一目的。

1. Weighted Least Square Method

1.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。

1.2 Weighted Least Square的目標函數

咱們假設目標變量$y$和輸入變量$x$存在以下關係:

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

$\epsilon^{(i)}$是測量偏差項，它們獨立同分布，而且服從標準正態分佈$\epsilon^{(i)} \sim N(0,\sigma^2)$。

即:

$$ R=E(\epsilon_i \epsilon_i^T) =\left[ \begin{matrix} \sigma^2 & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & \sigma^2 \end{matrix} \right] $$

若是假設$\epsilon^{(i)}$獨立，可是有不一樣的方差(variance)。

$$ R=E(\epsilon_i \epsilon_i^T) =\left[ \begin{matrix} \sigma_1^2 & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & \sigma_m^2 \end{matrix} \right] $$

則Weighted Least Squares Method的目標函數能夠定義以下:

$$ \begin{aligned} WLS\_J(\theta_1, \theta_2, ..., \theta_n) =& \frac{1}{2} \sum_{i=1}^{n}{w_i}(\theta^Tx^{(i)} - y^{(i)})^2 \\ =& \frac{1}{2} \sum_{i=1}^{n}\frac{(\theta^Tx^{(i)} - y^{(i)})^2}{\sigma_i^2} \\ =& \frac{1}{2} (X\theta - Y)^TR^{-1}(X\theta - Y) \\ \end{aligned} $$

1.3 Weighted Least Square的矩陣解

令導數爲0，求解極值點:

$$ \begin{aligned} \frac{\partial WLS\_J(\theta_1, \theta_2, ..., \theta_n)}{\partial \theta} =& X^TR^{-1}X\theta - X^TR^{-1}Y \\ =& 0 \end{aligned} $$

可獲得:

$$ \theta = (X^TR^{-1}X)^{-1} X^TR^{-1}Y $$

2. Weighted Least Squares的應用舉例

仍之前一篇文章提到的測量車輛位置爲例，展現Weighted Least Squares的用法。

假設存在m個測量值和n個未知參數:

$$ \left[ \begin{matrix} y_1 \\ y_2 \\ \vdots \\ y_m \\ \end{matrix} \right] =H \left[ \begin{matrix} x_1 \\ x_2 \\ \vdots \\ x_n \\ \end{matrix} \right] + \left[ \begin{matrix} e_1 \\ e_2 \\ \vdots \\ e_m \\ \end{matrix} \right] $$

Weighted Least Squares的目標函數以下:

$$ WLS\_J=e^T R^{-1} e $$

其中:

$$ \left[ \begin{matrix} e_1 \\ e_2 \\ \vdots \\ e_m \\ \end{matrix} \right]=e= \left[ \begin{matrix} y_1 \\ y_2 \\ \vdots \\ y_m \\ \end{matrix} \right] - H \left[ \begin{matrix} x_1 \\ x_2 \\ \vdots \\ x_n \\ \end{matrix} \right] $$

$$ H=\left[ \begin{matrix} 1 \\ 1 \\ \vdots \\ 1 \\ \end{matrix} \right] $$

令:

$$ \frac{\partial (WLS\_J)}{\partial X} = 0 = -Y^TR^{-1}H+X^TH^TR^{-1}H $$

獲得:

$$ X = (H^TR^{-1}H)^{-1} H^TR^{-1}Y $$

假設有激光雷達和衛星同時對自動駕駛車輛進行位置測量，測量結果以下:

測量設備	車輛位置	方差
衛星1	1.80	400
衛星2	1.78	400
衛星3	1.82	400
激光雷達1	1.89	4
激光雷達2	1.90	4

代入上式:

$$ X=\Bigg( \left[ 1, 1, 1,1,1 \right] \left[ \begin{matrix} 400 & & & & \\ & 400 & & & \\ & & 400 & &\\ & & & 4 & \\ & & & & 4 \\ \end{matrix} \right]^{-1} \left[ \begin{matrix} 1 \\ 1 \\ 1 \\ 1 \\ 1 \\ \end{matrix} \right] \Bigg)^{-1} \left[ 1, 1, 1,1,1 \right] \left[ \begin{matrix} 400 & & & & \\ & 400 & & & \\ & & 400 & &\\ & & & 4 & \\ & & & & 4 \\ \end{matrix} \right]^{-1} \left[ \begin{matrix} 1.80 \\ 1.78 \\ 1.82 \\ 1.89 \\ 1.90 \\ \end{matrix} \right] = 1.89 $$

能夠看到，Weighted Least Square的計算結果更接近於方差較小的激光雷達測量結果。

相關文章

自動駕駛定位系統-State Estimation & Localization

自動駕駛定位算法-直方圖濾波定位

自動駕駛高精地圖-概述與分析

Waymo-自動駕駛長尾問題挑戰(2019)

---

我的網站地址: http://www.banbeichadexiaojiu...