Kalman Filter in SLAM (6) ——Error-state Kalman Filter (EsKF, 误差状态卡尔曼滤波)

---

0.前言

这里先说一句：什么误差状态卡尔曼？完全就是在扯淡！

回想上面我们推导的IMU的误差状态空间方程，其实如果后面我们是使用优化的方法，而不是使用滤波的方法，那么推导完IMU的误差状态空间方程就可以了。因为上面我们已经说了，IMU的误差状态空间方程和状态空间方程的协方差矩阵是完全一致的。所以后面如果是使用优化的方法，那么就可以使用误差状态空间方程得到的协方差矩阵作为IMU积分值的置信度了。

但是如果我们是要使用滤波的方法呢？那么就要使用基于误差状态（而非状态）的卡尔曼滤波了，也就是我们这里说的误差状态卡尔曼滤波。

这里可能有一个疑问，既然我们推导的误差状态和状态的协方差矩阵是相等的，那直接用普通的那种基于状态变量的卡尔曼滤波不就可以了吗？实际上确实是可以的，经过后面的公式推导也可以发现，误差状态卡尔曼滤波的公式实际上和普通的卡尔曼滤波没有任何区别，所以上面第一句才说“误差状态卡尔曼完全就是在扯淡”！

但是为什么还要用它呢？实际上只有一个原因，就是 旋转的原因，前面我们说了旋转的状态变量我们选的是 角轴（旋转向量），但是旋转向量存在一个问题，那就是周期性。所以如果我们用的是 状态旋转向量，那么在滤波过程中就存在周期性。但是如果我们选择的是 误差状态旋转向量 呢？它始终在0附近，所以就 不会产生周期性 的问题。因此实际上使用误差状态卡尔曼滤波完全就是因为这个原因，而且现在仔细想想，实际上误差状态卡尔曼滤波我们 不应该把它理解为一种新的滤波算法（比如 KF 和 EKF 就是两种算法，虽然非常像），它只是在卡尔曼滤波的时候把状态变量从原来的 状态 变成了 误差状态，其他并没有任何变化。

1. IMU的误差状态空间方程

见本系列另一篇博客： Kalman Filter in SLAM (4) ——IMU Intergration and State Space Representation (IMU积分和状态空间表示)

2. 误差状态观测方程

用 $\mathbf{z}$ 代表 预测观测值，也就是我们把IMU预测得到的状态（先验状态） ${\hat{\mathbf{x}}}_k$ 带入到观测方程中得到的 计算出来的观测值。现在假设我们的先验状态 ${\hat{\mathbf{x}}}_k$ 的误差是 $\delta {\hat{\mathbf{x}}}_k$。由于它存在误差，导致带入观测方程之后得到的 预测观测值 $\mathbf{z}$ 产生的误差是 $\delta \mathbf{z}$。利用公式（37），我们带入 真实的状态值 ${\mathbf{x}}_k={\hat{\mathbf{x}}}_k+\delta {\hat{\mathbf{x}}}_k$ 到 函数自变量 ${\mathbf{x}}_k$ 中，可以得到真实的状态算出的预测观测值 $\mathbf{z}+\delta \mathbf{z}$ 的表达式为：

$$\mathbf{z}+\delta \mathbf{z}=\mathbf{h}({\hat{\mathbf{x}}}_k,0)+{\mathbf{H}}_k({\hat{\mathbf{x}}}_k+\delta {\hat{\mathbf{x}}}_k-{\hat{\mathbf{x}}}_k)+{\mathbf{V}}_k{\mathbf{v}}_k$$

注意：我们对观测方程进行线性化最主要的目的就是得到系数矩阵 $\mathbf{H}$，但是另外一个同样重要的点就是把状态值带入观测方程计算预测观测值的时候，我们也必须带入 线性化的方程，而不能带入原来的线性化的方程中，这样才能保证我们用的是一个模型。否则就变成了系数矩阵用的线性化模型，而计算的观测值用的是非线性模型。因此公式（39）中我们的自变量只有原来公式（37）中的 ${\mathbf{x}}_k$，误差变量 $\delta {\mathbf{x}}_k$ 并没有带入到函数 $\mathbf{h}({\hat{\mathbf{x}}}_k,0)$ 中，因为它只是一个线性函数的常数项。

再由公式（38）我们可以发现，$\mathbf{z}=\mathbf{h}({\hat{\mathbf{x}}}_k,0)$，把这个结果带入到公式（39）中，我们就可以得到 状态误差 $\delta {\mathbf{x}}_k$ 和 预测观测值的误差 $\delta \mathbf{z}$ 的关系，即 误差状态观测方程：

$$\delta \mathbf{z}={\mathbf{H}}_k\delta {\hat{\mathbf{x}}}_k+{\mathbf{V}}_k{\mathbf{v}}_k$$

可见这个公式的结果跟我们推导的IMU误差状态空间方程的结果也是非常相似的，因为 误差观测方程 的系数矩阵和我们的 状态观测方程 的系数矩阵是完全相等的。

3. 误差状态卡尔曼滤波

我们先给出之前已经推导过的IMU的离散 误差状态空间方程，误差状态空间方程两边正好把线性化出来的固定函数值抵消了。之前我们也已经证明了，误差状态空间方程和状态空间方程的系数矩阵是完全相等的，如下所示：

$$\delta {\hat{\mathbf{x}}}_k={\mathbf{F}}_{k-1}\delta {\check{\mathbf{x}}}_{k-1}+{\mathbf{W}}_{k-1}{\mathbf{w}}_{k-1}$$

然后是刚才推导过的 误差状态观测方程：

$$\delta \mathbf{z}={\mathbf{H}}_k\delta {\hat{\mathbf{x}}}_k+{\mathbf{V}}_k{\mathbf{v}}_k$$

把这两个方程带入卡尔曼滤波的五大公式中，我们可以得到以下公式，即为误差状态卡尔曼滤波方程。

预测公式（注意输入项 $\mathbf{B}{\mathbf{u}}_{k-1}$ 是我们准确知道的，所以真实状态减名义状态就消去了）：

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

校正公式（注意其中预测观测值 $\delta \mathbf{z}$ 带入的时候，把噪声 ${\mathbf{V}}_k{\mathbf{v}}_k$ 简化为0了）：

$$\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?-\delta \mathbf{z}\right)=\delta {\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left(?{\mathbf{z}}_m?-\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}$$

但是带入的时候我们发现了问题，原来的状态卡尔曼滤波我们在校正的时候，$K_k$ 乘以的系数是 观测传感器测量值 $-$ 带入预测状态到观测方程中得到的 预测观测值。这里换成误差状态后，误差的预测观测值我们已经推导出来了，就是 $\mathbf{H}\delta {\hat{\mathbf{x}}}_k$。按理说原本状态卡尔曼滤波中的 传感器测量值${\mathbf{z}}_m$ 也应该替换成 传感器测量误差值，那么测量误差值应该怎么计算呢？

其实仔细思考公式（44），即 $\delta \mathbf{z}={\mathbf{H}}_k\delta {\hat{\mathbf{x}}}_k+{\mathbf{V}}_k{\mathbf{v}}_k$，这个公式我们在计算什么？因为方程中 已经把传感器的测量噪声项的影响 ${\mathbf{V}}_k{\mathbf{v}}_k$ 建模进去了，所以我们计算的是 仅仅由先验状态误差 引起的 观测测量值误差。那么我们上面说的传感器测量值误差应该是什么呢？显然就是 传感器的测量噪声 ${\mathbf{V}}_k{\mathbf{v}}_k$！因为传感器测量误差就是在说，如果我传入的 先验状态就是当前的真实状态，但是带入观测方程中计算得到的观测测量值 仍然 和传感器的真实测量值之间有差距，这个差距就是因为我在带入观测方程中计算的时候由于不知道此时传感器的噪声是多少，所以把它简化成了 0，从而计算结果和真实的传感器测量不一致。明白了这个道理之后，就很容易计算 传感器的测量误差值 了，如下所示：

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

注意：

（1） 同理还是之前那一点，我们带入真实状态计算观测测量值的时候，带入的仍然是线性化之后的观测方程，即 ${\mathbf{z}}_k=\mathbf{h}({\hat{\mathbf{x}}}_k,0)+{\mathbf{H}}_k({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k)+{\mathbf{V}}_k{\mathbf{v}}_k$ 中计算。

（2） 计算真实状态的测量误差值的时候，我们不知道噪声，所以计算公式中没有 ${\mathbf{V}}_k{\mathbf{v}}_k$ 这一项。实际上我们上面的公式就是在求噪声，就是在求 ${\mathbf{V}}_k{\mathbf{v}}_k$ 这一项。

因此，误差卡尔曼滤波中的校正公式为：

$$\begin{array}{rl}\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)\\ & =\delta {\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{h}({\hat{\mathbf{x}}}_k,0)-2{\mathbf{H}}_k\delta {\hat{\mathbf{x}}}_k\right)\end{array}$$

4. 误差状态卡尔曼滤波方程细节问题

1. 状态的更新

每当我们进行一次误差卡尔曼滤波之后，即得到了 后验误差 之后，都需要把它加到 先验的状态值 上，即：

$${\check{\mathbf{x}}}_{\mathbf{k}}={\hat{\mathbf{x}}}_{\mathbf{k}}+\delta {\check{\mathbf{x}}}_k$$

此时我们就得到了经过这次卡尔曼滤波之后，我们得到的 对状态${\mathbf{x}}_k$ 的最优估计，所以说此时我们要把后验误差 $\delta {\check{\mathbf{x}}}_{\mathbf{k}}$ 置为 $0$，表示此时我们得到的状态就是我们 能够给出的最优估计的状态结果。

2. 先验估计误差的值和初始化问题

（1）非初始化状态

根据上面状态更新中的操作可以知道，每一次（假设第$k$次）误差状态卡尔曼滤波结束之后，我们的后验误差$\delta {\check{\mathbf{x}}}_{\mathbf{k}}$都是$0$，表示这次我们给出的状态估计结果是最优的状态估计结果。

那么当下一次（即第$k+1$次）误差迭代卡尔曼滤波来的时候，我们首先使用IMU状态方程计算这一次（第$k+1$次）的先验误差$\delta {\hat{\mathbf{x}}}_{k+1}$如下，注意公式中没有过程噪声${\mathbf{W}}_k{\mathbf{w}}_k$项，因为噪声我们始终都是不知道的。
$$\delta {\hat{\mathbf{x}}}_{k+1}=\mathbf{A}\delta {\check{\mathbf{x}}}_k$$
这里就会发现由于上一次的后验误差是0，所以这一次我们的先验误差计算结果也是0。所以只要不是在第一次进行误差状态卡尔曼滤波，那么卡尔曼校正公式都可以化简为：
$$\delta {\check{\mathbf{x}}}_k={\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{h}({\hat{\mathbf{x}}}_k,0)\right)$$
所以实际上，这个公式才是在误差状态卡尔曼滤波中最常见的校正公式！

（2）初始化状态

那么问题来了，第一次进行误差状态卡尔曼滤波的时候，先验误差${\hat{\mathbf{x}}}_0$如何给定呢？这个问题和卡尔曼滤波问题是一样的，其实卡尔曼滤波问题也存在初始化问题，就是第一次的先验状态给多少？这个感觉就是凭工程经验，靠调参来解决。所以这个也是r2live中存在一段函数进行ESKF初始化的原因。

3. 另一个角度思考卡尔曼滤波和误差卡尔曼滤波的校正公式

经过上面的公式推导，其实我们已经比较清楚为何ESKF和EKF的校正公式为什么看起来相差比较大了，二者校正公式：

EKF：
$${\check{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{H}{\hat{\mathbf{x}}}_k\right)$$

ESKF：

$$\begin{gathered} 初始化状态：\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) \\ 一般状态：\delta {\check{\mathbf{x}}}_k={\mathbf{K}}_k\left({\mathbf{z}}_m-\mathbf{h}({\hat{\mathbf{x}}}_k,0)\right) \end{gathered}$$

但是从数据融合的意义上来想好像又不明白为什么 ESKF 中一般状态下的先验误差为什么是0。其实可以不从数据融合的意义上想，而是从 状态调整的角度 这么想：我们先由状态方程得到一个先验状态，此时我们只有这个先验状态，而且它给出的也是我们 从先验方程中能够得到的最优的状态，所以如果没有观测方程，那么 先验状态的误差就是0，因为这是我们能够给出的最优估计结果。但是现在我们有观测方程，所以我们就想利用观测方程的观测结果，在我们的先验状态的基础上进行一些调整，得到更加准确的后验状态，而调整的大小就是卡尔曼增益所在的这一项。

4. 思考为什么能直接把误差状态方程带入KF中变成ESKF？

从B站DR_CAN推导KF的公式中看，求卡尔曼增益的时候，利用的是对 后验误差的协防差矩阵的迹（就是方差的和）求导为0，即 让后验误差的方差总和达到最小值。那么同理如果现在状态向量变成了误差状态，那推导的时候不就变成对 误差的后验误差的协方差矩阵的迹 求导了吗？这又出来一个 误差的误差？

实际上从应用来说，不需要考虑到推导这一步。因为卡尔曼滤波的方程推导出来五大公式，会根据状态方程和观测方程用就可以了。观察我们上面推导出来了误差的状态方程和误差的观测方程，就简单的认为 误差 是另一个 新的状态变量$\mathbf{y}$ 就可以了，然后它就和原来的状态变量没有任何区别，就可以直接带入到卡尔曼滤波的方程中了。

---