Error-State Kalman Filter理解与公式推导
目录
- 直接法与间接法滤波
- ESKF公式推导
- VINS中对于ESKF的使用
直接法与间接法滤波
卡尔曼滤波作为一种贝叶斯滤波的具体实现被广泛应用于状态估计问题中，其优势与特点这里就不再赘述了，而众所周知的是，卡尔曼滤波只能应用于线性系统中，而实际中大部分系统无法满足这个约束，于是诞生了扩展卡尔曼滤波（Extended Kalman Filter，EKF）和误差状态卡尔曼滤波（Error State Kalman Filter，ESKF）等一系列应用于非线性系统的滤波方法，这些方法各有特点，这里主要讨论EKF和ESKF。
EKF和ESKF分别作为直接法滤波和间接法滤波的一种实现，首先需要弄清楚直接法滤波和间接法滤波的区别，这一点秦永元的书1上有详细说明，截图如下：
个人理解其主要区别在于：
1、从理论上来说，精度上直接法应该优于间接法，因为间接法的误差系统方程实际上是推导出来的，并且过程中作了一定近似（例如去掉了所有的二次微分项），虽然完成了系统线性化，但损失了精度；而直接法的系统方程是没有损失的，其精度损失在于在局部点上采用了泰勒一阶近似对系统线性化造成的精度损失，这取决于迭代速度、系统份非线性化程度等多个因素，因此实际工程实践中精度还是取决于使用情况和具体实现。
2、直接法例如EKF在每次迭代都需要计算雅克比矩阵，对于算力要求较高，而间接法在将误差方程线性化后，其雅克比矩阵是固定的，只与当前状态相关。
3、间接法由于每次估计的是状态误差项，其数值差异小，而直接法估计的是系统状态，不同状态项数值差异大，肯能在实际计算中带来误差。
ESKF公式推导
ESKF的核心在于将状态分解为两个部分组成：
X t = X + δ X (1) X_t=X+\delta X\tag1 Xt=X+δX(1)
其中 X t X_t Xt为系统状态真值， X X X为Nominal state， δ X \delta X δX为Error state，以下所有公式中均符合这个定义，系统递推方程为：
X k + 1 = f ( X k ) X t , k + 1 = f ( X t , k ) + w k (2) X_{k+1}=f(X_{k})\\[3mm] X_{t,k+1}=f(X_{t,k})+w_k\tag2 Xk+1=f(Xk)Xt,k+1=f(Xt,k)+wk(2)
上式展开为：
X k + 1 + δ X k + 1 = f ( X k + δ X k ) + w k (3) X_{k+1}+\delta X_{k+1}=f(X_{k}+\delta X_{k})+w_k\tag3 Xk+1+δXk+1=f(Xk+δXk)+wk(3)
上式中Nominal state为固定值，由于 δ X \delta X δX一般很小，可以认为其在0值附近波动，因此将上式 δ X \delta X δX在0值附近进行一阶泰勒展开：
X k + 1 + δ X k + 1 = f ( X k + 0 ) + d f d δ X k ∣ X k , 0 ( δ X k − 0 ) + w k (4) X_{k+1}+\delta X_{k+1}=f(X_{k}+0)+\cfrac{df}{d\delta X_k|_{X_k,0}}(\delta X_k-0)+w_k\tag4 Xk+1+δXk+1=f(Xk+0)+dδXk∣Xk,0df(δXk−0)+wk(4)
令 F k = d f d δ X k ∣ X k , 0 F_k=\cfrac{df}{d\delta X_k|_{X_k,0}} Fk=dδXk∣Xk,0df，并将（2）中第一个式子代入，则上式为：
δ X k + 1 = F k δ X k + w k (5) \delta X_{k+1}=F_k\delta X_k+w_k\tag5 δXk+1=FkδXk+wk(5)
以上是根据EKF的思路求得系统的误差传递方程的过程，之后就是测量方程的处理：
Y k + 1 = h ( X t , k + 1 ) + v k + 1 (6) Y_{k+1}=h(X_{t,k+1})+v_{k+1}\tag6 Yk+1=h(Xt,k+1)+vk+1(6)
同样的展开为：
Y k + 1 = h ( X k + 1 + δ X k + 1 ) + v k + 1 (7) Y_{k+1}=h(X_{k+1}+\delta X_{k+1})+v_{k+1}\tag7 Yk+1=h(Xk+1+δXk+1)+vk+1(7)
需要得到关于Error state的观测，同样将上式 δ X \delta X δX在0值附近进行一阶泰勒展开：
Y k + 1 = h ( X k + 1 + 0 ) + δ h δ δ X k + 1 ∣ X k + 1 , 0 ( δ X k + 1 − 0 ) + v k + 1 (8) Y_{k+1}=h(X_{k+1}+0)+\cfrac{\delta h}{\delta \delta X_{k+1}|_{X_{k+1},0}}(\delta X_{k+1}-0)+v_{k+1}\tag8 Yk+1=h(Xk+1+0)+δδXk+1∣Xk+1,0δh(δXk+1−0)+vk+1(8)
令 H k + 1 = δ h δ δ X k + 1 ∣ X k + 1 , 0 H_{k+1}=\cfrac{\delta h}{\delta \delta X_{k+1}|_{X_{k+1},0}} Hk+1=δδXk+1∣Xk+1,0δh，则上式为：
Y k + 1 − h ( X k + 1 ) = H k + 1 δ X k + 1 + v k + 1 (9) Y_{k+1}-h(X_{k+1})=H_{k+1}\delta X_{k+1}+v_{k+1}\tag9 Yk+1−h(Xk+1)=Hk+1δXk+1+vk+1(9)
剩下的部分则与正常的卡尔曼滤波完全一致，由于：
δ X k ∼ N ( δ X ^ k , P k ) \delta X_k \sim N(\delta \hat{X}_k,P_k) δXk∼N(δX^k,Pk)
则：
K k + 1 = P k H k + 1 T ( H k + 1 P k H k + 1 T + V k + 1 ) − 1 δ X ^ k + 1 = δ X ^ k + K k + 1 ( Y k + 1 − h ( X k + 1 ) − H k + 1 δ X k + 1 ) P k + 1 = ( I − K k + 1 H k + 1 ) P k (10) K_{k+1}=P_kH_{k+1}^T(H_{k+1}P_kH_{k+1}^T+V_{k+1})^{-1}\tag{10}\\[3mm] \delta \hat{X}_{k+1}= \delta \hat{X}_k+K_{k+1}(Y_{k+1}-h(X_{k+1})-H_{k+1}\delta X_{k+1})\\[3mm] P_{k+1}=(I-K_{k+1}H_{k+1})P_k Kk+1=PkHk+1T(Hk+1PkHk+1T+Vk+1)−1δX^k+1=δX^k+Kk+1(Yk+1−h(Xk+1)−Hk+1δXk+1)Pk+1=(I−Kk+1Hk+1)Pk(10)
由于 δ X \delta X δX一般描述为符合正态分布，且每次测量修正nominal state后都将其置0，因此这里 δ X ^ k = 0 \delta \hat{X}_k=0 δX^k=0，（10）中第二个式子变为：
δ X ^ k + 1 = K k + 1 ( Y k + 1 − h ( X k + 1 ) ) (11) \delta \hat{X}_{k+1}= K_{k+1}(Y_{k+1}-h(X_{k+1}))\tag{11} δX^k+1=Kk+1(Yk+1−h(Xk+1))(11)
之后将观测得到的Error state注入nominal state用于修正误差：
X k + 1 ← X k + 1 + δ X ^ k + 1 (12) X_{k+1}\gets X_{k+1}+\delta \hat{X}_{k+1}\tag{12} Xk+1←Xk+1+δX^k+1(12)
以上就是ESKF离散状态公式推导的过程，其中(2)(3)(4)(5)使用的是使用泰勒一阶展开的方式求出误差传递方程，看起来跟EKF有些类似，但由于 δ X \delta X δX数值很小，因此其近似的精度损失也较小。也可以通过直接推导的方式得到误差传递方程，2，VINS中的使用就是根据中值积分法求解nominal state，并根据motion model推导得到误差传递方程，具体推导过程可以参考VINS-Mono代码分析总结
VINS中对于ESKF的使用
VINS中对于Error state的使用与一般用法有点不同，它的error state主要用于当 δ b a , δ b g \delta b_a,\delta b_g δba,δbg发生较小变化时快速的更新IMU预积分状态，由于在前端VIO优化过程中会进行很多次迭代，而每次迭代会轻微的更新 δ b a , δ b g \delta b_a,\delta b_g δba,δbg，若每次更新 δ b a , δ b g \delta b_a,\delta b_g δba,δbg后都进行重新的预积分，虽然精度会较高，但计算量太大，且 δ b a , δ b g \delta b_a,\delta b_g δba,δbg进行较小的更新时使用系统累计的雅克比矩阵乘以更新后的值再更新系统状态并不会带来太大误差，但计算量会大大减少。
具体解释结合下图解释：
黄色的线代表了IMU预积分构成的约束，对于sliding window中每一个frame，系统都会构建一个intergration_base的对象用以管理它与上一个frame之间IMU预积分的结果以及对应时间段内系统状态对error state的雅克比矩阵，例如在 x k ， x k + 1 x_k，x_{k+1} xk，xk+1之间存在n个IMU测量值 u n u_n un，可以由中值积分得到nominal state：
Δ x k + 1 = f ( u 0 , u 1 . . . u n , b a , b g ) (13) \Delta x_{k+1}=f(u_0,u_1...u_n,b_a,b_g)\tag{13} Δxk+1=f(u0,u1...un,ba,bg)(13)
并且每次在积分的同时结合当时的nominal state更新雅各比矩阵 F t F_t Ft，并将(5)式中雅各比矩阵累乘：
F k + 1 = F n ∗ F n − 1 ∗ . . . F 0 (14) F_{k+1} = F_n*F_{n-1}*...F_0\tag{14} Fk+1=Fn∗Fn−1∗...F0(14)
最终有：
Δ x t , k + 1 = Δ x k + 1 ⊕ δ x k + 1 (15) \Delta x_{t,k+1} = \Delta x_{k+1} \oplus \delta x_{k+1} \tag{15} Δxt,k+1=Δxk+1⊕δxk+1(15)
其中 δ x k + 1 = F k + 1 δ x k \delta x_{k+1}=F_{k+1}\delta x_{k} δxk+1=Fk+1δxk，其中默认 δ x k = 0 \delta x_{k}=0 δxk=0，因此上式变为：
Δ x t , k + 1 = Δ x k + 1 (16) \Delta x_{t,k+1} = \Delta x_{k+1} \tag{16} Δxt,k+1=Δxk+1(16)
若VIO运行优化的迭代过程中，对 δ x k \delta x_{k} δxk进行了更新，使其不再为0，此时可以通过式(15)快速更新 Δ x t , k + 1 \Delta x_{t,k+1} Δxt,k+1，可以大大减小计算量。在优化完成后，再通过全部重新积分的方式更新 Δ x t , k + 1 \Delta x_{t,k+1} Δxt,k+1以保证更高的精度。
秦永元等 ,《卡尔曼滤波与组合导航原理》 ↩︎
Joan Sola ,《Quaternion kinematics for the error-state Kalman filter》 ↩︎