目录
三. 结合VINS 和 文献[1]，讨论两种获得误差状态递推方程的方法
一. ESKF的流程
对应正常状态的EKF流程，ESKF流程有如下改变：
1. 误差状态预测：deltaX_k = 0 . 同时，进行正常状态预测（用于步骤4，和 后续的误差状态重置。）
- 因为在上一次滤波结束后，k-1时刻的误差状态被重置为0，因此，k时刻的误差状态预测的结果也是0。
2. 协方差更新：和EKF一样，不变。
公式267 是 误差状态递推方程 ----- 这是我们要求解的；
公式268-269 分别是 误差状态预测；协方差更新 
3. 计算卡尔曼增益：和EKF一样，不变。
4. 利用观测量进行状态更新：如图1 公式275所示
- 完整的公式应该是： deltaX_k = deltaX_k + K(y-h(X))。因为误差状态的预测量 deltaX_k=0，所以省略了。
- 公式275 中的 Xt = X + deltaX。 X 是步骤1之后的，当前时刻的状态预测量；deltaX 是当前时刻的误差状态预测量。
5. 协方差更新：不变。
图1. 步骤3-5- 参考自《Quaternion kinematics for the error-state Kalman filter》
- 关键的H计算。明确一点，H 是 函数 h(Xt) 对误差状态 deltaX的导数。利用链式法则：
图2. H 计算公式
然后分别计算 Hx, X_deltax 即可。 详细的，参考 《Quaternion kinematics for the error-state Kalman filter》
- 此外，还有最重要的更新。将误差状态量补偿到正常状态量上，同时，误差状态量和其协方差进行更新。如图3所示。当状态量中包含旋转R时，P' 并不严格的等于 P。我们近似认为 P' = P 。详细的参考：《Quaternion kinematics for the error-state Kalman filter》P64 6.3 ESKF reset
图3. 协方差更新
二. ESKF的好处
参考文献[1] 5.1 P52
1. 旋转部分可以用李代数表示，避免过参数化；
2. 求解雅阁比时，线性化点一直是 deltaX=0
3. 二阶量视为无穷小，可以被忽略
4. 误差量的变化是缓慢的，因此，观测更新(步骤4)可以比预测(步骤1)慢。即： 观测量的频率可以低于IMU的频率。
三. 结合VINS 和 文献[1]，讨论两种获得误差状态递推方程的方法
（0）基础数学知识
1. 姿态对时间的导数
此外，还有常用的：
2. 伴随性质
参考文献[2]
以及一个变种： R*[w]x*R-1 = [Rw]x
（一）原理分析
为什么要获得误差递推方程？
- 在ESKF中，显然，误差递推方程是必须的，即步骤1；
- 在VIO中，需要计算预积分的协方差，协方差的传播过程，需要知道误差递推方程。
如何获得误差递推方程？有哪些方法？
- 通过微分形式获得（滤波通常采用）
- 一阶泰勒展开获得 （VIO通常采用）
下面分别解析两种方法：
1. 微分形式获得误差递推方程
声明：这里只是简单介绍思路，详细过程参考文献[1]
1) 获得误差状态微分方程
已知 真实状态 = 标称状态+误差状态。可以很容易写出真实状态和标称状态的微分方程，因此获得误差状态微分方程。参考文献[1] 5.3节。
推导过程中有如下技巧： 详见(二)
zm_ekf 中使用的技巧：详见(二)
2）标称状态微分方程离散化，获得标称状态方程
- 具体方法有： 欧拉，中值积分，RK4。参考文献[1]附录A
3）误差微分方程获得误差状态递推方程
要做什么事情呢？已知误差微分方程
最终要获得离散时间下的误差递推方程：
首先，讨论闭式解。参考文献[1]附录B。有一点要注意，如何由公式342 得到 公式343？参考：我的这篇阅读笔记
然后，讨论近似解。参考文献[1]附录C。（至于为啥要采用近似解，文献[1]给出了理由，但是没理解）。结论是：
a） 采用2阶近似的公式402比较好，会考虑加速度的影响。采用一阶近似公式401也没问题，即 公式343中的 fai = I + A * δT。
b）使用一阶泰勒展开的计算结果 与 公式402 形式统一，但是略微有些差别。我认为可以忽略。
重点需要说明： 一阶近似 公式401 也正确。为啥形式和 一阶泰勒展开的结果不统一？因为只做了一阶近似嘛。
c）虽然 附录C.2 Block-wise truncation 中公式402 的近似结果比较好，但是我们需要先写出闭式解，才可以做近似。实际操作时，不方便。因此，通常采用C.1 System-wise truncation， 且进行一阶近似。这也就是滤波中最常见的形式：
Xn+1 = (I + A * deltaT)* Xn
4) 误差微分方程获得误差状态递推方程 ---- 噪声部分
2. 一阶泰勒展开获得误差递推方程
先上一个结论：
我们已知：离散后的状态递推方程，我们的目的是：求解 误差递推方程，即公式 36。
为什么 F ， G 是上图所说的结果呢？（注意： 离散后并不一定线性，这是两个不同的概念）
证明如下：
实际中，如何求解 F G？
很简单啦，就是求导嘛！ 已知 y= f(x) ， 则y' = deltaY/deltaX = f(x+deltaX)-f(x)/deltaX
例如：
a. 计算得到deltaY
b. 计算 deltaY/deltaX
很重要一点：在 P V Q 的方程中，bg ba 并不算输入量，因此矩阵G中，不应该出现PVQ对ba bg的导数。 (ba, bg的导数服从高斯分布)
总结：
- 滤波中：需要根据真实状态微分方程、标称状态微分方程，获得误差状态微分方程(连续时间的) :
deltaX的导数 = A*deltaX+G*n ; n 为 噪声
然后就可以直接写出 误差状态递推方程：
deltaXk = （I + A* deltaT )* deltaXk-1 + G*deltaT * n
- VIO 中： 先写出离散的状态递推方程，然后分别计算 F （Xk 对 Xk-1 的导数） ，G（Xk 对 输入量的导数）, 然后得到误差状态递推方程：
deltaXk = F * deltaXk-1 + G * n
问题： 参考文献[1]，上述总结中，滤波的 I + A* deltaT 并不等于 VIO 的 F 。但是，VIO课程中却说两者相等，why ? 因为 P 中含有二次项 加速度。 如果都是一次关系，那么VIO课程中说的没错。
（二）获得误差状态递推方程的小技巧
以（一）1. 微分形式获得误差递推方程微分 方法为例，总结如下：
首先声明思路：Xt = X + deltaX . Xt 的导数既等于 “先求导，再拆分”， 又等于“先拆分，再求导”
备注：指出上图推导中的一个小错误：hat(w) 是标称方程中的角速度，hat_w = w_m-bg; w_m 是角速度测量值。正确的 ： wt = hat_w+nw+deltaBg。虽然上图写错了，但是不影响结果。
注意： wt 中并不包含 n_bg ； 因为 n_bg 是随机游走噪声。
bg_t 的导数 = n_bg ---- 随机游走噪声的定义
bg_t = bg_hat + delta_bg
bg_hat 对时间的导数 = 0
所以 delta_bg 对时间的导数 = n_bg
1. 在计算deltaP 的微分方程时，省略了高阶无穷小项
2. 上面 deltaR 的微分方程中使用的是左扰动，下面给出右扰动的：
（三）误差状态方程、标称状态方程离散化方法需要保持一致吗？
结论：暂时不知道。不过，可以保证如下的操作是正确的：
1. 滤波中，标称状态采用euler积分进行离散化，误差状态 F ≈ I + A×δT
1. 滤波
a. 先写出 真实微分方程、标称微分方程，然后两者相减，得到误差微分方程 delta_X对时间的导数 = A*delta_X; 注意这都是在连续时间下。
b. 得到误差状态方程 deltaX = F * delta_X-1 ；F ≈ I + A×δT
c. 此外，还需要知道离散标称状态递推方程，因为要进行状态预测。
2. VIO中，先euler积分或者假中值积分 进行标称状态方程离散化，然后一阶泰勒展开计算误差状态方程。
有两个需要离散化的步骤：标称状态方程 ， 误差状态方程
前面（一）中提供了两个方法，从方法1. 微分形式获得误差递推方程 的角度分析，误差状态方程的离散化 和 标称方程的离散化没有关系。
但是，从方法2. 一阶泰勒展开获得误差递推方程的角度分析，误差状态递推方程是根据离散化的标称方程获得的，因此，误差状态方程与标称方程离散化有关系。参考：文献[1] 附录D；张腾ORB_VIO_RK4
备注：张腾代码中，预积分协方差的计算与euler积分时一致，K时刻预积分量对于K-1时刻的预积分量的导数与euler积分不一致，因为是按照RK4离散得到的误差方程计算而来。
四. 一些实际应用中的技巧
（一）多个观测时，每次只使用一个观测进行更新。预测的频率通常要高于观测的频率
（二）观测量有延迟：在观测量所在时刻进行滤波操作，然后传递至最新的预测状态
（三）协方差如何调整？
五. 一些实践小例子
1. IMU+GPS
TODO
1. MSCKF 通读源码+测试
2. zm_ekf 通读源码+测试
参考：
[1] 《Quaternion kinematics for the error-state Kalman filter》
[2] 《Lie Groups for 2D and 3D Transformations》
8221
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
