OpenVins源码阅读图解——协方差矩阵状态克隆和更新

最近看了一下OpenVins的Propagator.cpp源码，觉得作者对状态的协方差矩阵存储结构设计的很巧妙，所以特意记录一下，为了更加清晰地阐述，我绘制了下图：

其中 X i X_i Xi​表示第i图像帧的IMU状态，共15DoF(6DOF位姿，3DoF速度，6DoF bias)， P i P_i Pi​表示IMU的位姿。初始状态协方差矩阵大小为$30\times 30$，除去IMU状态剩下的分别是1DoF的IMU与Camera之间的时间差，6DoF IMU与Camera之间的相对位姿， 8DoF的相机的内参。

在得到第一个状态的协方差矩阵后，进行状态clone。其实就是利用现有的协防差进行了复制。具体操作如下：

将原来的协方差矩阵维度扩展6维（6DoF的位姿），将和位姿有关的块，按上图标号进行移动即可。这样的结构可以保证很容易的进行协防差的propagation。假设线性化后的IMU状态更新方程如下所示：
X i + 1 = F X i + G N i + 1 X_{i+1}=FX_i+GN_{i+1} Xi+1​=FXi​+GNi+1​
其中 N i + 1 N_{i+1} Ni+1​为噪声与 X i X_i Xi​相互独立。假设关于内参的15DoF的状态统称为 Y i Y_i Yi​。那么经过第一次clone后的协方差矩阵形式如下：

根据状态更新方程，分别计算 X 1 X_1 X1​与 Y 0 Y_0 Y0​和 P 0 P_0 P0​的协方差，以及 X 1 X_1 X1​自己的协方差，即可完成协方差矩阵的更新。上图的结构使得这个过程变得十分方便。(其中假设 Y 0 = Y 1 Y_0=Y_1 Y0​=Y1​)

根据协方差矩阵的定义，如果把第一列块从 X 0 X_0 X0​变到 X 1 X_1 X1​。只需要把第一列块提出来，右乘一个 F T F^T FT就行了。以计算更新后的 Y 0 Y_0 Y0​和 X 1 X_1 X1​对应得那协方差矩阵块 V a r Y 0 , X 1 Var_{Y_0,X_1} VarY0​,X1​​：
V a r Y 0 , X 1 = E ( Y 0 X 1 T ) − E ( Y 0 ) E ( X 1 T ) = E ( Y 0 ( X 0 T F T + G N i + 1 ) ) − E ( Y 0 ) E ( X 0 T F T + G N i + 1 ) = E ( Y 0 X 0 T ) F T − E ( Y 0 ) E ( X 0 T ) F T + E ( Y 0 ) E ( G N i + 1 ) − E ( Y 0 ) E ( G N i + 1 ) = ( E ( Y 0 X 0 T ) − E ( Y 0 ) E ( X 0 T ) ) F T = V a r Y 0 , X 0 F T \begin{aligned} Var_{Y_0,X_1}&=E(Y_0X_1^T)-E(Y_0)E(X_1^T)\\ &=E(Y_0(X_0^TF^T+GN_{i+1}))-E(Y_0)E(X_0^TF^T+GN_{i+1})\\ &=E(Y_0X_0^T)F^T-E(Y_0)E(X_0^T)F^T+E(Y_0)E(GN_{i+1})-E(Y_0)E(GN_{i+1})\\ &=(E(Y_0X_0^T)-E(Y_0)E(X_0^T))F^T\\ &=Var_{Y_0,X_0}F^T \end{aligned} VarY0​,X1​​​=E(Y0​X1T​)−E(Y0​)E(X1T​)=E(Y0​(X0T​FT+GNi+1​))−E(Y0​)E(X0T​FT+GNi+1​)=E(Y0​X0T​)FT−E(Y0​)E(X0T​)FT+E(Y0​)E(GNi+1​)−E(Y0​)E(GNi+1​)=(E(Y0​X0T​)−E(Y0​)E(X0T​))FT=VarY0​,X0​​FT​
关于 V a r P 0 , X 1 Var_{P_0,X_1} VarP0​,X1​​与上式类似。如果只把第一行块换成 X 1 X_1 X1​根据协方差矩阵得对称性质，可以通过更新后得列块直接转置得到。最后再根据更新公式调整一如下左上角得那个方块就行了：
V a r X 1 , X 1 = E ( X 1 X 1 T ) − E ( X 1 ) E ( X 1 T ) = F V a r X 0 , X 0 F T + G Q i + 1 G T \begin{aligned} Var_{X_1,X_1}&=E(X_1X_1^T)-E(X_1)E(X_1^T)\\ &=FVar_{X_0,X_0}F^T+GQ_{i+1}G^T\\ \end{aligned} VarX1​,X1​​​=E(X1​X1T​)−E(X1​)E(X1T​)=FVarX0​,X0​​FT+GQi+1​GT​
其中 Q i + 1 Q_{i+1} Qi+1​为噪声 N i + 1 N_{i+1} Ni+1​得协方差。

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！