search

KumarRobotics / msckf_vio

Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight
Other
1.66k stars 592 forks source link

Could you give more detail about the related `Phi` in `MsckfVio::processModel`? #114

Closed FanaticsKang closed 3 years ago

FanaticsKang commented 3 years ago

You modify the transition matrix in MsckfVio::processModel

1.

Phi.block<3, 3>(0, 0) =
    quaternionToRotation(imu_state.orientation) * R_kk_1.transpose();

2.

Phi.block<3, 3>(6, 0) = A1 - (A1*u-w1)*s;

3.

Phi.block<3, 3>(12, 0) = A2 - (A2*u-w2)*s;

It seems all related to delta theta and, I guess, you replace Phi.block<3, 3>(0, 0) with the same element in [1]. This operation looks like FEJ-EKF? The remainder elements, Phi.block<3, 3>(6, 0) and Phi.block<3, 3>(12, 0), confuse me.

Could you give more source about these codes? Is these related to observability constraint?

[1] M. Li and A. I. Mourikis, "Improving the accuracy of EKF-based visual-inertial odometry," 2012 IEEE International Conference on Robotics and Automation, 2012, pp. 828-835, doi: 10.1109/ICRA.2012.6225229.

FanaticsKang commented 3 years ago

I got it. Thank you for your code.