关于F、B矩阵的推导问题以及姿态角的EliminateError问题

[zhouzhibo0117]

【一】F、B矩阵的推导问题

请问UpdateErrorState()中的jaccobian矩阵的推导是如何来的？ 源代码中的实现为:

F_.block<3, 3>(INDEX_STATE_POSI, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity();
F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ORI) = F_23;
F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_ORI) = F_33;
F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ACC_BIAS) = pose_.block<3, 3>(0, 0);
F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_GYRO_BIAS) = -pose_.block<3, 3>(0, 0);

B_.block<3, 3>(INDEX_STATE_VEL, 3) = pose_.block<3, 3>(0, 0);
B_.block<3, 3>(INDEX_STATE_ORI, 0) = -pose_.block<3, 3>(0, 0);

但是参考《Quaternion kinematics for the error-state Kalman filter》一文中的推导，实现应该修改如下（忽略地球自转影响）：

F_.block<3, 3>(INDEX_STATE_POSI, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity();
F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ORI) = -pose_.block<3, 3>(0, 0) * BuildSkewSymmetricMatrix(ComputeUnbiasAccel(curr_imu_data.linear_accel));
F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_ORI) = -BuildSkewSymmetricMatrix(ComputeUnbiasGyro(curr_imu_data.angle_velocity));
F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ACC_BIAS) = -pose_.block<3, 3>(0, 0);
F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_GYRO_BIAS) = -Eigen::Matrix3d::Identity();

B_.block<3, 3>(INDEX_STATE_VEL, 3) = Eigen::Matrix3d::Identity();
B_.block<3, 3>(INDEX_STATE_ORI, 0) = Eigen::Matrix3d::Identity();

【二】姿态角的EliminateError问题

同上，EliminateError()源代码相关的实现为：

Eigen::Matrix3d C_nn = SO3Exp(-X_.block<3, 1>(INDEX_STATE_ORI, 0));
pose_.block<3, 3>(0, 0) = C_nn * pose_.block<3, 3>(0, 0);

同样参考《Quaternion kinematics for the error-state Kalman filter》一文中的公式，实现应该修改如下：

pose_.block<3, 3>(0, 0) = pose_.block<3, 3>(0, 0) * SO3Exp(X_.block<3, 1>(INDEX_STATE_ORI, 0));

【三】疑问

1. 以上两处源代码的推导过程是如何来的？是否有错误？
2. 实验有趣地发现，问题一、二均使用保留源代码的实现，或者均使用修改后的实现，都能得到正确一样的结果。请教原因。

[sunbingfeng]

这个我可以解答一下，作者实现里面的X_对应的是全局扰动，而Quaternion kinematics for the error-state Kalman filter中使用的是局部扰动，因此推导的转移矩阵有差异，相应的update部分就对应扰动的左乘/右乘了。

至于B矩阵，我也很好奇为什么会有一个pose_.block<3, 3>(0, 0)，我也怀疑推导有问题。但是，这个无非影响到Noise的大小问题，因此影响没有那么大。

求原作者解答一下