1. 无论是ESKF, 还是优化VIO 中,都需要得到 误差状态递推方程。因为都需要进行协方差传播更新。
2. 深蓝VIO课程中给了两种计算误差递推方程的方法:
1) 一阶泰勒展开
2) 误差递推方程的微分形式
一阶泰勒展开:已知离散形式的 真实方程和标称方程,得到误差方程。
误差递推方程的微分形式:先根据真实方程和标称方程的微分形式得到连续时间的误差递推方程的微分形式。如何根据误差递推微分方程得到误差递推方程?
该书的附录B给出了闭式解---- 即 非近似解。
就是对 e^AT 进行近似。
附录C进行近似:
C.1 系统层面的近似
C.2 分块近似
公式 401 和 公式402
滤波时,多采用 n=1 即一阶近似;即 公式 401; 相比于 402, 会损失一部分。
公式 402 也不等于 这个啊: 根据260 欧拉积分 离散方程,进行一阶泰勒展开求解误差递推方程。
因此我认为: 标称状态方程离散时采用 欧拉 中值 RK 与 误差递推方程近似时采用C.2 中的 一阶 or 2阶 没有联系。
根据C.2 获得 误差递推方程,方程中使用到的状态量从 标称离散方程中获得。 至于你是使用欧拉,中值 或者 RK 哪一种方法获得 状态量,不关心。
综上:可以分为两部:
1. 标称方程的离散:中值,欧拉,RK 参考附录A
2. 误差递推方程的形式:使用分块近似的 一阶 or 2 阶;(一般使用一阶形式,这样方便,二阶形式需要自己先写出闭式解,然后再做近似)
如果是使用一阶泰勒展开计算误差递推方程:
1. 先写出欧拉离散的标称方程
2. 一阶泰勒展开获得误差递推方程
如果想换用 中值 or RK 方式进行离散化,则 只需要改进第一个步骤即可,误差状态递推方程不需要改变(因为误差递推方程就是负责协方差的计算,用欧拉 或者 中值 还是 RK4 根本不影响)。参考 京胖RK4 预积分。京胖代码中协方差的传播就是 根据 欧拉积分 得到的 状态方程,一阶泰勒展开得到的 误差状态方程 来传播的。预积分离散化部分使用的是 RK4. 当然京胖也给出了 RK4 下的协方差传播形式。
注意: 即使误差微分递推方程选用 分块近似的 2 阶形式,和 一阶泰勒展开的计算结果也不同。
================ 下面关注噪声的协方差 =====