Giters

SpectacularAI / HybVIO

HybVIO visual-inertial odometry and SLAM system

Home Page:https://arxiv.org/abs/2106.11857

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Problem with the dy/dx in the predict function?

Gatsby23 opened this issue · comments

commented

Dear Professor:
Sorry for bothering you again. Are there some docs about the dy/dx and the dy/dq in the prediction function？It seems it is different from the EKF Based algorithm.

commented

https://en.wikipedia.org/wiki/Extended_Kalman_filter#Non-additive_noise_formulation_and_equations In the notation of that Wikipedia article, dydx is $F$ and dydq is $L$.

Thanks for your quick replay. However, follow the equaction. I found the F and the Q should be:
image
This equation is from the Paper"Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight" and it is as the same as "Quaternion kinematics for the error-state Kalman filter".
However, there some differences in the code. Like the:

dydx.block(VEL, BGA, 3, 3) = -dydq.block(VEL, Q_GYRO, 3, 3);

The derivatives of bga about the velocity should be same in the two equation. I don't know where the "minus" come from？Another problem is that why multiply the A matrix twice Like this:

dydq.block(VEL, Q_GYRO, 3, 3) = dydx.block(VEL, ORI, 3, 4) * dydq.block(ORI, Q_GYRO, 4, 3);

I know the unit test proved that the matrix should right. But I don't know how to derivate the analytical equation about the F and Q. Could you please help me and give me some advice？

commented 
dydx.block(VEL, BGA, 3, 3) = -dydq.block(VEL, Q_GYRO, 3, 3);

These blocks have different signs simply because in $\hat{\omega} = \omega_m - b_g + n_g$ the two last terms have different signs. Here $n_g$ represents the gyroscope measurement noise (Q_GYRO). I'm not very familiar with the formalism of the papers you referred to, but there may very well be some sign difference since we handle the orientations differently, without reducing their dimensionality from 4 to 3.

https://github.com/SpectacularAI/HybVIO/blob/main/src/odometry/ekf.cpp#L480-L485
As you must have noticed, the code comments about the possibly duplicated matrix $A = \exp{S}$ — this is not clear to me either. As I understand it, the derivative of the matrix exponential is not simply $\frac{d}{dx} \exp{S} = (\exp{S}) \frac{d}{dx}S$, but that is some kind of approximation. See for example https://en.wikipedia.org/wiki/Derivative_of_the_exponential_map

If I recall correctly, we also tried removing the possibly duplicated matrix A and the derivative check unit test still passed. The VIO performance was the same or slightly worse.

commented 
dydx.block(VEL, BGA, 3, 3) = -dydq.block(VEL, Q_GYRO, 3, 3);

These blocks have different signs simply because in $\hat{\omega} = \omega_m - b_g + n_g$ the two last terms have different signs. Here $n_g$ represents the gyroscope measurement noise (Q_GYRO). I'm not very familiar with the formalism of the papers you referred to, but there may very well be some sign difference since we handle the orientations differently, without reducing their dimensionality from 4 to 3.

https://github.com/SpectacularAI/HybVIO/blob/main/src/odometry/ekf.cpp#L480-L485 As you must have noticed, the code comments about the possibly duplicated matrix $A = \exp{S}$ — this is not clear to me either. As I understand it, the derivative of the matrix exponential is not simply $\frac{d}{dx} \exp{S} = (\exp{S}) \frac{d}{dx}S$, but that is some kind of approximation. See for example https://en.wikipedia.org/wiki/Derivative_of_the_exponential_map

If I recall correctly, we also tried removing the possibly duplicated matrix A and the derivative check unit test still passed. The VIO performance was the same or slightly worse.

Thank you for your quick reply. I'll check it again and make it more clearly to understand.