理论五、State Augmentation

感谢：紫薯萝卜 https://zhuanlan.zhihu.com/p/76894345

 

MSCKF的状态向量：

它包括IMU状态和N个相机状态。那究竟维护哪些相机状态呢？与基于关键帧的方法不同的是，MSCKF将每一帧都加入到状态向量中，每来一帧图像后，先根据IMU Propogation对IMU状态进行预测，然后根据相机外参计算当前帧的相机位姿，将最新相机状态加入到状态向量中并扩展协方差矩阵，这就是状态扩增（State Augmentation）。- 理解这个思想

 

1. 状态向量扩增

根据预测的当前IMU位姿 以及相机外参 来计算 当前相机的位姿：

然后将计算的当前相机状态 加入到状态向量中。

 

2. 协方差扩增

协方差扩增

其中 是新增相机状态对原状态向量之间的Jacobian：

推导如下（这里顺便推导了相机状态对外参的Jacobian，目前未将外参加入了状态向量，可以暂时先忽略）：

求导思路：根据IMU位姿转Camera位姿的关系式，对IMU状态中的相关量求导，根据 对位置量进行展开，根据 对旋转量进行展开，等式两边相约得到误差量的关系式，然后根据叉乘的性质进行等式变形，最终得到Jacobian。
注意 的左乘和右乘，这里所有推导用的左乘，而MSCKF的原论文中用的是右乘，所以推导出来的Jacobian会不一样。

 

3. 相机状态维护

相机状态扩增

• 扩增时机：IMU Propagation之后，Measurement Update之前。
• 扩增条件：每一帧新的图像观测都进行相机状态扩增。
• 扩增操作：计算最新相机状态并插入到状态向量中；扩展新状态的自身协方差以及与原状态向量的关联协方差。

相机状态移除

• 移除时机：Measurement Update之后
• 移除条件：当相机状态个数超过最大限制 时移除。正常是移除最老的相机状态，但当相机运动较小时也可以移除最近的相机状态（思路同VINS是一样的）。
• 移除操作：将待移除相机状态从状态向量中删除；删除协方差中对应的行和列。

 

4. 状态扩增的意义

MSCKF中，IMU Propagation只改变IMU状态向量和其对应的协方差，与相机无关；而Measurement Updata的观测模型是残差相对于相机状态的观测模型，与IMU状态没有直接关联。状态扩增就相当于相机和IMU状态之间的桥梁，通过关联协方差 描述相机和IMU状态之间的关系，每一个相机状态都与IMU状态形成关联，这样在观测更新相机状态的同时，会间接更新IMU状态。（理解这个思路）

EKF观测更新计算如下：

如果令IMU与相机之间的关联协方差 ，那么

可以看到，观测更新只会改变相机状态和相机对应的协方差，而不会改变IMU状态及其协方差，EKF的预测 而更新 ，二者完全独立，没有任何融合。只有当 时，在观测更新时， 和对应的协方差 才会同步被更新。