从零开始学习自动驾驶系统(三)-State Estimation & Localization

前面提到的 Ordinary Least Squares Estimation 和 Weighted Least Squares Estimation 和都假设提前收集好了所有的测量数据。但在实际的应用中，测量数据可能是流式的，比如位置测量系统以 100HZ 的频率不间断的对车辆的位置进行测量。在高频的更新频率下，测量数据越来越多，如果每次都把所有的测量结果都按照矩阵解进行计算，计算成本会不断增加，直至计算能力不可承受。所以需要用 Recursive Least Squares Estimation 解决这一问题。

1. Linear Recursive Estimation 测量更新方程

我们假设第 k 次测量目标变量

和输入变量

存在如下关系:

是测量误差项，它们之间相互独立，但是方差不同。

是根据前 k-1 次测量结果计算出的最优估计，第 k 次测量更新最优估计的方程如下:

其中

被称为 Estimator Gain Matrix,

被称为 Correction Term。

2. Linear Recursive Estimation 是无偏估计

如果

，并且

,那么

。这就意味着，如果测量噪声

的均值为 0，并且

，那么

，所以 Linear Recursive Estimation 是无偏估计。

的值如何取

=（使得 k 时刻 Estimation Errors 的方差和最小的那个

）Estimation Errors 的方差和如下:

是 Estimation Error 的协方差。

由于 k-1 时刻的 Estimation Error

与 k 时刻的测试噪声

独立，所以:

因此:

其中:

是

的协方差。

令:

得到:

3.Recursive Least Squares Estimation 的流程

步骤一: Initialize the parameter and covariance estimates:

如果测量之前对

一无所知，

，如果对

有充分的了解，

.

步骤二: 假设

其中

是均值为 0，协方差为

的随机变量,每一时刻 k 的测量噪声

相互独立。

更新 Estimation 的值

和 estimation Error 的值

：

1）Calculate the correction gain

2） Update the parameter estimate

3）Update the covariance estimate

4. Recursive Least Squares 应用举例(一)

假设我们要测量电阻的准确阻值，标准的电阻都会注明电阻的阻值，但是由于生产工艺、使用材料等不同，实际的电阻值与标注的电阻值总会有些差异。测量电阻需要测定电压 V 和电流 I，然后根据根据欧姆定律：

，计算出电阻 R。

假设电压和电流的测量数值如下:

采用线性模型

拟合电压和电流，计算电阻 R。

1）初始化估计值。

假设我们对电阻的具体值非常不确定, R 初值的方差很大。

根据欧姆定律，电压 V 和电流 I 之间应该为线性关系，所以我们应该比较确定，b 应该非常趋于 0。

假设测量设备的测量噪声方差为: 0.0225.

2） 进行迭代。

第一次迭代:

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

5. Recursive Least Squares 应用举例(二)

least_squares_loc_sample.png

仍以测量车辆位置为例，一开始车辆并不确认自己所在的位置，

，测量设备噪声方差为:0.01,测量方程为:

，连续测量的数据如下：

根据测量数据迭代计算车辆位置的过程如下:

1）初始化

2）迭代过程

第一次迭代:

继续进行第 2、3、4、5 次迭代，最后得到:

即测量的车辆位置为 1.837 左右。