search

KumarRobotics / msckf_vio

Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight
Other
1.73k stars 599 forks source link

question about MsckfVio::predictNewState #30

Closed PetWorm closed 6 years ago

PetWorm commented 6 years ago

In '// Some pre-calculation': Why multiply a 'cos(gyro_normdt0.5)' term when gyro_norm is no bigger than 1e-5 ? And any references for these imu propogation methods ? I read the paper and it just said "we apply a 4th order Runge-Kutta numerical integration of Eq. (1) to propagate the estimated IMU state." Thanks a lot.

chutsu commented 6 years ago

They are using the equations found in "Indirect Kalman Filter for 3D Attitude Estimation" Section 1.6.1 Zeroth Order Quaternion Integrator. Here they are using the aforementioned section to integrate the quaternion then convert it to rotation matrices for the Runge Kutta stage.

The equations they implemented are Eq. 101 and 103 (for very small angular velocities, due to numerical instabilities). But I'n not sure the cos(gyro_norm*dt*0.5) was needed?

PetWorm commented 6 years ago

@chutsu Thanks for your reply. I learnt the Eq. 101 and 103 from other references, and i was wondering why using cos(gyro_normdt0.5) but not approximate it to 1 as gyro_norm is very small. And multiply this cos term to the imaginary part doesn't make any sense to me. And btw in function processModel, do you know why the matrix Phi, which is the transition matrix, was modified ? I cant understand this at all. Hope u all the best.

chutsu commented 6 years ago

I don't know the answer to the first part of your question w.r.t. cos(gyro_norm*dt*0.5).

The transition matrix (and actually the measurement jacobian H) were modified (here and here) because they are trying to fix observability issues found in EKF based VINS or in our case the MSCKF formulation. To be specific they applied the OC-EKF (mentioned in the UPenn paper, Section IIIc) described in this document "Observability-constrained Vision-aided Inertial Navigation" by Hesch et al.

ke-sun commented 6 years ago

I agree the cos term can be approximated with 1 when the angle is no greater than 1e-5. However, it just depends on to what level one what to approximate. The only problematic term for the small angle is 1/|w|. Therefore, only the second term is converted using L'Hopital's rule.

PetWorm commented 6 years ago

@ke-sun I got it, thanks for your reply and your brilliant work ! @chutsu Thank you very much for your patient and detailed reply !