EKF-SLAM初探（二）预测与更新

参考书：概率机器人、Probabilistic Robotics

EKF定位实现步骤：

预测
修正（包括：测量预测、估计更新）

下面使用里程计测量机器人的运动输入，路标测量（距离、角度、编号），地图由路标点在世界坐标系下的位置构成。

1、预测步骤（Prediction Step）

1) KF模型和EKF模型
KF模型：

\begin{aligned} x_{k} = A_{k} x_{k - 1} + B_{k} u_{k} + w_{k} \\ z_{k} = C_{k} x_{k} + v_{k} \end{aligned}

EKF模型：

\begin{aligned} x_{k} = f (x_{k - 1}, u_{k}) + w_{k} \\ z_{k} = h (x_{k}) + v_{k} \end{aligned}

线性化：

\begin{aligned} f (x_{k - 1}, u_{k}) \approx f ({\hat{x}}_{k - 1}, u_{k}) + {\frac{\partial f (x_{k - 1}, u_{k})}{\partial x_{k - 1}} |}_{{\hat{x}}_{k - 1}} (x_{k - 1} - {\hat{x}}_{x - 1}) \\ h (x_{k}) \approx h ({\tilde{x}}_{k}) + {\frac{\partial h (x_{k})}{\partial x_{k}} |}_{{\tilde{x}}_{k}} (x_{k} - {\tilde{x}}_{k}) \end{aligned}

其中，

F_{k - 1} = {\frac{\partial f (x_{k - 1}, u_{k})}{\partial x_{k - 1}} |}_{{\hat{x}}_{k - 1}}, H_{k} = {\frac{\partial h (x_{k})}{\partial x_{k}} |}_{{\tilde{x}}_{k}}

先验分布中（预测阶段），状态变量的协方差由两部分构成：

初始位置估计的不确定性
运动噪声引起的不确定性

2) k-1时刻状态不确定性转移状态空间
在预测步骤中，通过下式将上一时刻状态不确定性转移到状态空间（协方差的传递律），

{\tilde{P}}_{k} = F_{k - 1} {\hat{P}}_{k - 1} F_{k - 1}^{T}

如果还有附加的运动噪声，还需要加上运动噪声到状态空间的转移，即：

{\tilde{P}}_{k} = F_{k - 1} {\hat{P}}_{k - 1} F_{k - 1}^{T} + B_{k} M_{k} B_{k}^{T}

3) 运动噪声转移到状态空间
对附加运动噪声N(0,M)，将控制空间的噪声协方差转换到状态空间（ approximate mapping between the motion noise in control space to the motion noise in state space.），需要对里程计进行线性化。

由里程计测量得到运动控制 $u_{k} = (v_{k}, w_{k})^{T}$ ，考虑附加白噪声，得真实测量 ${\hat{u}}_{k}$ 满足：

{\hat{u}}_{k} \sim N (u_{k}, M_{k}), M_{k} = [\begin{matrix} α_{1} v_{k}^{2} + α_{2} w_{k}^{2} & 0 \\ 0 & α_{3} v_{k}^{2} + α_{4} w_{k}^{2} \end{matrix}]

使用协方差传递律，通过

B_{k} M_{k} B_{k}^{T}

将运动噪声转移到状态空间，其中

B_{k}

一般通过线性化得到：

B_{k} = {\frac{\partial f (x_{k - 1}, u_{k})}{\partial u_{k}} |}_{u_{k} = (v_{k}, w_{k})^{T}}

如下图所示，设定不同的运动噪声，让机器人运动同样的距离，最后产生的总的不确定性对应的椭圆大小不同。
EKF-SLAM初探（二）预测与更新

2、测量预测（Measurement Prediction ）

这一步对于EKF更新是非必要的，但是存在必有道理，它是为了去除错误匹配（outlier rejection）和未知匹配（unknown correspondences）情况。
对观测方程 $z_{k} = C_{k} x_{k} + v_{k}$ ，其中 $x_{k} \sim N ({\tilde{x}}_{k}, {\tilde{P}}_{k})$ ，类似上面的k-1时刻状态不确定性转移状态空间的步骤，可得（详细推导参见概率机器人p156）：
均值： ${\hat{z}}_{k}^{i} = h ({\tilde{x}}_{k}, c_{k}^{i}, m)$ ，表示预测的测量值
协方差： $S_{k} = H_{k} {\tilde{P}}_{k} H_{k}^{T} + Q_{k}$ ，表示该预测的不确定性度

如下图所示，白色箭头为更新向量（innovation vector）： $z_{k} - {\hat{z}}_{k}$ ，它表示观测与预测的测量之间的差值，它是一个零均值、协方差为 $S_{k}$ 的高斯分布。
EKF-SLAM初探（二）预测与更新

3、估计更新（Estimation Update）

通过卡尔曼增益矩阵 $K_{k}$ 将更新向量按比例映射到状态空间，实现更新。如果观察越确定，即 $Q_{k}$ 越小，卡尔曼增益就越高，因此产生的位置修正就越强大。
如图为EKF的修正效果，减小均值估计并减小位置不确定性的椭圆。
EKF-SLAM初探（二）预测与更新

<完>
@leatherwang