前面提到的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] $$
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} $$
继续进行第2、3、4、5次迭代，最后得到:
$$x=1.83665339$$
即测量的车辆位置为1.837左右。
相关文章
自动驾驶系统定位与状态估计- Weighted Least Square Method
自动驾驶定位系统-State Estimation & Localization
个人网站地址: http://www.banbeichadexiaojiu...
**粗体** _斜体_ [链接](http://example.com) `代码` - 列表 > 引用。你还可以使用@来通知其他用户。