Kalman Filter in SLAM (7) ——Error-state Iterated Kalman Filter (EsIKF, 误差状态迭代卡尔曼滤波)

---

实际 EsIKF 就是把 EsKF 和 IEKF 两个结合起来了，并且如果从本质上来说它就是个迭代卡尔曼滤波 IEKF，只不过是基于误差状态的。

1. EsKF 和 IEKF 公式回顾

1.1. EsKF 误差状态卡尔曼滤波方程

预测公式：

$$\begin{array}{c}\delta {\hat{\mathbf{x}}}_k=\mathbf{F}\delta {\check{\mathbf{x}}}_{k-1}\\ {\hat{\mathbf{P}}}_k=\mathbf{F}{\check{\mathbf{P}}}_{k-1}{\mathbf{F}}^T+\mathbf{Q}\end{array}$$

校正公式：

$$\begin{array}{c}{\mathbf{K}}_k=\frac{{\hat{\mathbf{P}}}_k{\mathbf{H}}^T}{\mathbf{H}{\hat{\mathbf{P}}}_k{\mathbf{H}}^T+\mathbf{R}}\\ \delta {\check{\mathbf{x}}}_k=\delta {\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left(({\mathbf{z}}_m-\mathbf{h}({\hat{\mathbf{x}}}_k,0)-{\mathbf{H}}_k\delta {\hat{\mathbf{x}}}_k)-\mathbf{H}\delta {\hat{\mathbf{x}}}_k\right)\\ {\check{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}\right){\hat{\mathbf{P}}}_k\end{array}$$

1.2. IEKF 迭代扩展卡尔曼滤波方程

预测公式：

$$\begin{gathered} 带入上一时刻的后验状态到自变量中，即{\mathbf{x}}_{k-1}\leftarrow {\check{\mathbf{x}}}_{k-1}: \\ {\hat{\mathbf{x}}}_k=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},0)+{\mathbf{F}}_{k-1}({\check{\mathbf{x}}}_{k-1}-{\check{\mathbf{x}}}_{k-1})=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},0) \\ {\hat{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\check{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T+\mathbf{Q} \end{gathered}$$

校正公式：注意为了写成统一的结果，这里把观测方程线性化展开点记为 ${\mathbf{x}}_{op}$，其中下标 $op$ 代表 $optmise$ 优化，然后迭代一次之后的结果变成 ${\check{\mathbf{x}}}_{op}$，这个值再赋值到下一次的线性展开点进行展开。

$$\begin{gathered} 观测方程在展开点{\mathbf{x}}_{op}处展开。带入IMU的先验状态到自变量中，即{\mathbf{x}}_k\leftarrow {\hat{\mathbf{x}}}_k: \\ {\mathbf{z}}_k=\mathbf{h}({\mathbf{x}}_{op},0)+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op}) \\ \begin{array}{c}{\mathbf{K}}_k=\frac{{\hat{\mathbf{P}}}_k{\mathbf{H}}^T}{\mathbf{H}{\hat{\mathbf{P}}}_k{\mathbf{H}}^T+\mathbf{R}}\\ {\check{\mathbf{x}}}_{op}={\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{h}({\mathbf{x}}_{op},0)-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})\right)\\ {\check{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}\right){\hat{\mathbf{P}}}_k\end{array} \end{gathered}$$

2.EsIKF公式推导

2.1. 预测公式

由于 IEKF 中对预测公式没有改变，所以仍然是 EsKF 的结果

$$\begin{array}{c}\delta {\hat{\mathbf{x}}}_k=\mathbf{F}\delta {\check{\mathbf{x}}}_{k-1}\\ {\hat{\mathbf{P}}}_k=\mathbf{F}{\check{\mathbf{P}}}_{k-1}{\mathbf{F}}^T+\mathbf{Q}\end{array}$$

2.2. 校正公式

2.2.1. 误差状态观测方程

首先还是要推导 不同线性展开点 下的 误差观测方程，根据之前的公式，把观测方程在优化点处展开：

$$\begin{gathered} 在先验状态处展开：\mathbf{z}+\delta \mathbf{z}=\mathbf{h}({\hat{\mathbf{x}}}_k,0)+{{\mathbf{H}}_k|}_{{\hat{\mathbf{x}}}_k}({\hat{\mathbf{x}}}_k+\delta {\hat{\mathbf{x}}}_k-{\hat{\mathbf{x}}}_k)+{{\mathbf{V}}_k|}_{{\hat{\mathbf{x}}}_k}{\mathbf{v}}_k \\ 在优化点处展开：\mathbf{z}+\delta \mathbf{z}=\mathbf{h}({\mathbf{x}}_{op},0)+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k+\delta {\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})+{{\mathbf{V}}_k|}_{{\mathbf{x}}_{op}}{\mathbf{v}}_k \end{gathered}$$

注意到公式中的 $\mathbf{z}$ 是 先验状态预测观测值，即 $\mathbf{z}=\mathbf{h}({\hat{\mathbf{x}}}_k,0)$，带入上面优化点处展开的方程中，可以得到：

$$\begin{array}{rl}\delta \mathbf{z}& =\mathbf{h}({\mathbf{x}}_{op},0)-\mathbf{h}({\hat{\mathbf{x}}}_k,0)+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k+\delta {\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})+{{\mathbf{V}}_k|}_{{\mathbf{x}}_{op}}{\mathbf{v}}_k\\ & =\mathbf{h}({\mathbf{x}}_{op},0)-\mathbf{h}({\hat{\mathbf{x}}}_k,0)+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}\delta {\hat{\mathbf{x}}}_k+{{\mathbf{V}}_k|}_{{\mathbf{x}}_{op}}{\mathbf{v}}_k\end{array}$$

上面的公式中，${\hat{\mathbf{x}}}_k$ 是IMU先验状态值，${\mathbf{x}}_{op}$ 是对观测方程进行线性展开的点（可以是IMU先验状态，也可以是迭代开始后的滤波后验状态），都是已知量，因此公式中的前三项都是已知量，即都是常数。

2.2.2. 传感器观测误差值

下面公式中的 ${\mathbf{z}}_{{\mathbf{x}}_{kt}}$ 是 先验状态等于真实状态 的时候，即 ${\mathbf{x}}_k={\hat{\mathbf{x}}}_k+\delta {\hat{\mathbf{x}}}_k$，我们计算得到的 真实状态预测观测值。同理这里就是为了评估噪声大小，所以真实状态预测观测值的计算中没有噪声项。

$$\begin{array}{rl}\delta {\mathbf{z}}_m& ={\mathbf{z}}_m-{\mathbf{z}}_{{\mathbf{x}}_{kt}}\\ & ={\mathbf{z}}_m-\left(\mathbf{h}({\mathbf{x}}_{op},0)+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\mathbf{x}}_k-{\mathbf{x}}_{op})\right)\\ & ={\mathbf{z}}_m-\left(\mathbf{h}({\mathbf{x}}_{op},0)+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k+\delta {\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})\right)\\ & ={\mathbf{z}}_m-\mathbf{h}({\mathbf{x}}_{op},0)-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k+\delta {\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})\\ & ={\mathbf{z}}_m-\mathbf{h}({\mathbf{x}}_{op},0)-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}\delta {\hat{\mathbf{x}}}_k\end{array}$$

2.2.3. 校正公式

$$\begin{gathered} 观测方程在展开点{\mathbf{x}}_{op}处展开。带入IMU的先验状态到自变量中，即{\mathbf{x}}_k\leftarrow {\hat{\mathbf{x}}}_k: \\ {\mathbf{z}}_k=\mathbf{h}({\mathbf{x}}_{op},0)+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op}) \\ \begin{array}{c}{\mathbf{K}}_k=\frac{{\hat{\mathbf{P}}}_k{\mathbf{H}}^T}{\mathbf{H}{\hat{\mathbf{P}}}_k{\mathbf{H}}^T+\mathbf{R}}\\ \delta {\check{\mathbf{x}}}_{op}=\delta {\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left(\delta {\mathbf{z}}_m-\delta \mathbf{z}\right)\\ {\check{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}\right){\hat{\mathbf{P}}}_k\end{array} \end{gathered}$$

下面带入校正公式中的 $\delta {\mathbf{z}}_m$ 和 $\delta \mathbf{z}$ 项，将公式化简（注意带入公式的时候 $\delta \mathbf{z}$ 的噪声项为 0，因为我们不知道这个噪声是多少，所以计算的时候简化成了 0）：

$$\begin{array}{rl}\delta {\check{\mathbf{x}}}_{op}=& \delta {\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left(\delta {\mathbf{z}}_m-\delta \mathbf{z}\right)\\ =& \delta {\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{h}({\mathbf{x}}_{op},0)-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}\delta {\hat{\mathbf{x}}}_k\right)\\ & -{\mathbf{K}}_k\left(\mathbf{h}({\mathbf{x}}_{op},0)-\mathbf{h}({\hat{\mathbf{x}}}_k,0)+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})+{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}\delta {\hat{\mathbf{x}}}_k\right)\\ =& \delta {\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_m-2\mathbf{h}({\mathbf{x}}_{op},0)+\mathbf{h}({\hat{\mathbf{x}}}_k,0)-2{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})-2{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}\delta {\hat{\mathbf{x}}}_k\right)\\ =& \delta {\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{h}({\mathbf{x}}_{op},0)-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})\right)\\ & +{\mathbf{K}}_k\left(\mathbf{h}({\hat{\mathbf{x}}}_k,0)-\mathbf{h}({\mathbf{x}}_{op},0)-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})-2{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}\delta {\hat{\mathbf{x}}}_k\right)\end{array}$$

（1） 当 ${\mathbf{x}}_{o}p={\hat{\mathbf{x}}}_k$ 时，即第一次迭代时，校正公式化简为
$$\begin{array}{r}\delta {\check{\mathbf{x}}}_{op}=\delta {\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{h}({\hat{\mathbf{x}}}_k,0)-2{{\mathbf{H}}_k|}_{{\hat{\mathbf{x}}}_k}\delta {\hat{\mathbf{x}}}_k\right)\end{array}$$
可见该公式结果与普通的 基于状态的IEKF 的结果相同。

（2） 当 ${\mathbf{x}}_{o}p\ne {\hat{\mathbf{x}}}_k$ 时，即后面的几次迭代，首先注意到此时先验误差 $\delta {\mathbf{x}}_k=0$。然后对于公式中的 $\mathbf{h}({\hat{\mathbf{x}}}_k,0)-\mathbf{h}({\mathbf{x}}_{op},0)-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})$ 这一项，可以发现其值为下图中所示的 真实的先验预测观测值与线性化的先验预测观测值 的差。

如果这里将其作为 一阶小量忽略，则迭代公式化简为：

$$\delta {\check{\mathbf{x}}}_{op}={\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{h}({\mathbf{x}}_{op},0)-{{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})\right)$$

同理，要注意经过多次迭代后，只有最后一次迭代需要计算后验误差的协方差矩阵，然后将最后一次迭代得到的后验误差补偿到先验状态上去。

2.3. R2Live中的 Es-IKF 的校正公式理解

2.3.1. IMU+Camera

这里只推导迭代过程中的校正公式，初始化时候的预测公式不做推导。

首先明白 ${\hat{\mathbf{x}}}_k$ 始终是IMU状态方程得到的 预测状态值，在 IEKF 的过程中它始终是不变的，同理 先验误差协方差矩阵 也是保持不变的。

在迭代过程中，上一次迭代得到了一个更接近真实状态的点 ${\mathbf{x}}_{op}$，然后这一次迭代就把它作为线性化展开点对观测方程进行线性展开得到系数矩阵${{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}$ 和这个点的观测值 $\mathbf{h}({\mathbf{x}}_{op},0)$，从而 $\mathbf{h}({\mathbf{x}}_{op},0)$ 就知道了，${{\mathbf{H}}_k|}_{{\mathbf{x}}_{op}}({\hat{\mathbf{x}}}_k-{\mathbf{x}}_{op})$ 这一项也可以计算出来了。注意 $\mathbf{h}({\mathbf{x}}_{op},0)$ 就是把 ${\mathbf{x}}_{op}$ 点的相机位姿带入观测方程中，得到路标点的重投影像素坐标，然后 ${\mathbf{z}}_m-\mathbf{h}({\mathbf{x}}_{op},0)$ 就是本次计算的重投影误差。

2.3.2.IMU+LiDAR

这里只推导迭代过程中的校正公式，初始化时候的预测公式不做推导。

首先明白 ${\hat{\mathbf{x}}}_k$ 始终是IMU状态方程得到的 预测状态值，在 IEKF 的过程中它始终是不变的，同理 先验误差协方差矩阵 也是保持不变的。

在迭代过程中，上一次迭代得到了一个更接近真实状态的点 ${\mathbf{x}}_{op}$。那么LiDAR的观测方程是什么呢？我们计算的残差项是世界坐标系中 点到平面的距离，而这个平面是上一次SLAM过程中保存在kdtree中的点组成的，所以 平面是不变 的。而点是从这一帧的LiDAR坐标系转换到世界坐标系，因此点的坐标和这一帧的 LiDAR坐标系的位姿 有关。

那么这样我们计算的测量值应该就是点到平面的距离，那么实际观测的测量值应该是什么？TODO

---