自動駕駛系統定位與狀態估計- Recursive Least Squares Estimation

前面提到的Ordinary Least Squares Estimation和 Weighted Least Squares Estimation和都假設提早收集好了全部的測量數據。但在實際的應用中，測量數據多是流式的，好比位置測量系統以100HZ的頻率不間斷的對車輛的位置進行測量。在高頻的更新頻率下，測量數據愈來愈多，若是每次都把全部的測量結果都按照矩陣解進行計算，計算成本會不斷增長，直至計算能力不可承受。因此須要用Recursive Least Squares Estimation解決這一問題。

1. Linear Recursive Estimation測量更新方程

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

$$ y_k = H_k x + v_k $$

$\epsilon_k$是測量偏差項，它們之間相互獨立，可是方差不一樣。

$\hat{x}_{k-1}$是根據前k-1次測量結果計算出的最優估計，第k次測量更新最優估計的方程以下:

$$ \hat{x}_{k} = \hat{x}_{k-1} + K_k(y_k - H_k \hat{x}_{k-1}) $$

其中$K_k$被稱爲Estimator Gain Matrix, $y_k - H_k \hat{x}_{k-1}$被稱爲Correction Term。

2. Linear Recursive Estimation是無偏估計

$$ \begin{aligned} E(\epsilon_{x,k}) =& E(x-\hat{x}_k) \\ =& E(x - \hat{x}_{k-1} - K_k(y_k - H_k \hat{x}_{k-1}) ) \\ =& E(x - \hat{x}_{k-1} - K_k(H_k x + v_k - H_k \hat{x}_{k-1})) \\ =& E(\epsilon_{x, k-1} - K_k(H_k\epsilon_{x, k-1} + v_k)) \\ =& E(\epsilon_{x, k-1} - K_k H_k\epsilon_{x, k-1} + K_kv_k) \\ =& (I-K_k H_k) E(\epsilon_{x, k-1}) - K_k E(v_k) \end{aligned} $$

若是$E(\epsilon_{x, k-1}) = 0$，而且$E(v_k) = 0$,那麼$E(\epsilon_{x, k}) = 0$。這就意味着，若是測量噪聲$v_k$的均值爲0，而且$x_0=E(x)$，那麼$E(\hat{x}_k)=x_k$，因此Linear Recursive Estimation是無偏估計。

$K_k$的值如何取

$K_k$=（使得k時刻Estimation Errors的方差和最小的那個$K_k$）
Estimation Errors的方差和以下:

$$ \begin{aligned} J_k=& E[(x_1 - \hat{x}_1)]^2 + ... + E[(x_n-\hat{x}_n)^2] \\ = & E(\epsilon_{x_1,k}^2 + ... + \epsilon_{x_n,k}^2) \\ =& E(\epsilon_{x,k}^T\epsilon_{x,k}) \\ =& E[tr(\epsilon_{x,k}\epsilon_{x,k}^T)]\\ = & TrP_k \end{aligned} $$

$P_k$是Estimation Error的協方差。

$$ \begin{aligned} P_k &= E(\epsilon_{x,k}\epsilon_{x,k}^T) \\ =& E\{[(I-K_k H_k) \epsilon_{x, k-1} - K_k v_k][...]^T\} \\ =& E\{[(I-K_k H_k)\epsilon_{x, k-1}\epsilon_{x, k-1}^T(I-K_k H_k)^T]-[(I-K_k H_k)\epsilon_{x, k-1}(K_kv_k)^T]-K_kv_k\epsilon_{x, k-1}^T(I-K_k H_k)^T + K_kv_k(K_kv_k)^T\}\\ =& (I-K_k H_k)E(\epsilon_{x, k-1}\epsilon_{x, k-1}^T)(I-K_k H_k)^T-(I-K_k H_k)E(\epsilon_{x, k-1}v_k^T)K_k-K_kE(v_k\epsilon_{x,k-1}^T)(I-K_k H_k)^T + K_kE(v_kv_k^T)K_k^T\\ \end{aligned} $$

因爲k-1時刻的Estimation Error $\epsilon_{x,k-1}$與k時刻的測試噪聲$v_k$獨立，因此:

$$ E(v_k\epsilon_{x,k-1}^T) =E(v_k)E(\epsilon_{x,k-1})=0 $$

所以:

$$ \begin{aligned} P_k=&(I-K_k H_k)E(\epsilon_{x, k-1}\epsilon_{x, k-1}^T)(I-K_k H_k)^T+K_kE(v_kv_k^T)K_k^T\\ =&(I-K_k H_k)P_{k-1}(I-K_k H_k)^T+K_kR_kK_k^T \end{aligned} $$

其中: $R_k$是$v_k$的協方差。

令:

$$ \frac{\partial J_k}{\partial K_k}=2(I-K_k H_k)P_{k-1}(-H_k^T)+2K_kR_k=0 $$

獲得:

$$ K_k(R_k + H_kP_{k-1}H_k^T)=P_{k-1}H_k^T $$

$$ K_k=P_{k-1}H_k^T(R_k + H_kP_{k-1}H_k^T)^{-1} $$

3.Recursive Least Squares Estimation的流程

步驟一: Initialize the parameter and covariance estimates:

$$ \hat{x}_0=E(x) $$

$$ P_0 = E[(x-\hat{x}_0)(x-\hat{x}_0)^T] $$

若是測量以前對$x$一無所知，$P_0=\infty I$，若是對$x$有充分的瞭解，$P_0=0$.

步驟二: 假設

$$ y_k=H_kx + v_k $$

其中$v_k$是均值爲0，協方差爲$R_k$的隨機變量,每一時刻k的測量噪聲$v_k$相互獨立。

更新Estimation的值$x$和estimation Error的值$P$：

1）Calculate the correction gain

$$ \begin{aligned} K_k=&P_{k-1}H_k^T(R_k + H_kP_{k-1}H_k^T)^{-1} \\ =&P_k H_k^TP_k^{-1} \end{aligned} $$

2） Update the parameter estimate

$$ \hat{x}_{k} = \hat{x}_{k-1} + K_k(y_k - H_k \hat{x}_{k-1}) $$

3）Update the covariance estimate

$$ \begin{aligned} P_k=&(I-K_k H_k)P_{k-1}(I-K_k H_k)^T+K_kR_kK_k^T\\ =& (I-K_kH_k)P_{k-1} \end{aligned} $$

4. Recursive Least Squares應用舉例(一)

假設咱們要測量電阻的準確阻值，標準的電阻都會註明電阻的阻值，可是因爲生產工藝、使用材料等不一樣，實際的電阻值與標註的電阻值總會有些差別。測量電阻須要測定電壓V和電流I，而後根據根據歐姆定律：$V=IR$，計算出電阻R。

假設電壓和電流的測量數值以下:

Current (A)	Voltage (V)
0.2	1.23
0.3	1.38
0.4	2.06
0.5	2.47
0.6	3.17

採用線性模型$y = Rx + b$擬合電壓和電流，計算電阻R。

1）初始化估計值。

假設咱們對電阻的具體值很是不肯定, R初值的方差很大。

$$\hat{R} \sim \mathcal{N}(4.0, 10.0)$$

根據歐姆定律，電壓V和電流I之間應該爲線性關係，因此咱們應該比較肯定，b應該很是趨於0。

$$\hat{b} \sim \mathcal{N}(0, 0.2)$$

假設測量設備的測量噪聲方差爲: 0.0225.

$$ x_0=\left[ \begin{matrix} 4.0 & 0.0 \end{matrix} \right] $$

$$ P_0=\left[ \begin{matrix} 10.0 & 0.0 \\ 0.0 & 0.2 \\ \end{matrix} \right] $$

2） 進行迭代。

第一次迭代:

$$H_1=[0.2, 1]$$

$$ \begin{aligned} K_1 =& P_0H_1^T(H_1P_0H_1^T+R_1)^{-1} \\ =& \left[ \begin{matrix} 10.0 & 0.0 \\ 0.0 & 0.2 \\ \end{matrix} \right] \left[ \begin{matrix} 0.2 \\ 1.0 \\ \end{matrix} \right]( \left[ \begin{matrix} 0.2& 1.0 \\ \end{matrix} \right] \left[ \begin{matrix} 10.0 & 0.0 \\ 0.0 & 0.2 \\ \end{matrix} \right] \left[ \begin{matrix} 0.2 \\ 1.0 \\ \end{matrix} \right] + 0.0225 )^{-1} \\ =& \left[ \begin{matrix} 3.21285141 \\ 0.32128514 \\ \end{matrix} \right] \end{aligned} $$

$$ \begin{aligned} x_1 =& x_0 + K_1(y_1 - H_1x_{0}) \\ =&\left[ \begin{matrix} 4.0 \\ 0.0 \\ \end{matrix} \right] + \left[ \begin{matrix} 3.21285141 \\ 0.32128514 \\ \end{matrix} \right](1.23- \left[ \begin{matrix} 0.2& 1.0 \\ \end{matrix} \right]\left[ \begin{matrix} 4.0 \\ 0.0 \\ \end{matrix} \right]) \\ =& \left[ \begin{matrix} 5.3815261 \\ 0.13815261 \\ \end{matrix} \right] \end{aligned} $$

$$ \begin{aligned} P_1 =& (I - K_1 H_1)P_0 \\ =& (\left[ \begin{matrix} 1.0 & 0.0 \\ 0.0 & 1.0 \end{matrix} \right] - \left[ \begin{matrix} 3.21285141 \\ 0.32128514 \end{matrix} \right] \left[ \begin{matrix} 0.2& 1.0 \\ \end{matrix} \right]) \left[ \begin{matrix} 10.0 & 0.0 \\ 0.0 & 0.2 \end{matrix} \right]\\ =& \left[ \begin{matrix} 3.57429719 & -0.64257028 \\ -0.64257028 & 0.13574297 \end{matrix} \right] \end{aligned} $$

如上所示，持續進行第二、三、四、5次迭代，最後求得:

$$ \left[ \begin{matrix} R & b \end{matrix} \right]= \left[ \begin{matrix} 4.97896278 & 0.06886542 \end{matrix} \right] $$

5. Recursive Least Squares應用舉例(二)

仍以測量車輛位置爲例，一開始車輛並不確認本身所在的位置，$x_0 \sim \mathcal{N}(1.5, 0.5)$，測量設備噪聲方差爲:0.01,測量方程爲:$Y=Hx+V$，連續測量的數據以下：

測量設備	車輛位置
測量設備	1.80
測量設備	1.78
測量設備	1.82
測量設備	1.89
測量設備	1.90

根據測量數據迭代計算車輛位置的過程以下:

1）初始化

$$ \hat{x}_0 = 1.5 $$

$$ P_0 = 0.5 $$

2）迭代過程

$$ H_k=1,\{k=1,2,...\} $$

$$ R_k=0.01,\{k=1,2,...\} $$

第一次迭代:

$$ \begin{aligned} K_1 =& P_0H_1^T(H_1P_0H_1^T+R_1)^{-1} \\ =& P_0 * (P_0 + R_1)^{-1}\\ =& 0.98039216 \end{aligned} $$

$$ \begin{aligned} x_1 =& x_0 + K_1(y_1 - H_1x_{0}) \\ =& 1.5 + 0.98039216 * (1.8 - 1.5) \\ =& 1.79411765 \end{aligned} $$

$$ \begin{aligned} P_1 =& (I - K_1 H_1)P_0 \\ =& (1-0.98039216) * 0.5\\ =& 0.00980392 \end{aligned} $$

繼續進行第二、三、四、5次迭代，最後獲得:

$$x=1.83665339$$

即測量的車輛位置爲1.837左右。

相關文章

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

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

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

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

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

---

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