自动驾驶系统定位与状态估计- 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} $$

如上所示，持续进行第2、3、4、5次迭代，最后求得:

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