Eq. 11 of the propagation step different from code (?) about ai-imu-dr HOT 4 OPEN

Hi Tony, @scott81321
Thank you for your helpful comments! I am also reading this paper and have a lot of questions.

However, for Equation (11), I guess the author means a discrete covariance propagation. The confusing part is the naming. In Eq. 11, it's actually
P_{n+1} = Phi_n * P_n * Phi^T + Gamma_n * Q_n * Gamma_n^T
which can be found in chapter 4 of Applied Optimal Estimation or other KF books:

In the code, F means F(t), and Phi corresponds to "F_n" in Eq 11.

I am still trying to clarify other questions.

from ai-imu-dr.

Yes @ghs2015 , this eq. 3.7.10 has the same functional form as eq. 11 of Brossard's paper. I am not quibbling about that part of the paper. However, eq. 11 is not what is coded up in his code. That's my issue/question.

from ai-imu-dr.

@scott81321 I guess we can reduce eq. 3.7.10 or eq. 11 to the coded one following

from ai-imu-dr.

@ghs2015 @robocar2018 There is something else about the code which does not match with Brossard's paper. Eqs. 18, 19 and 20 of his paper defined P0, Q and Nn as the SQUARE of diagonal matrices. But according to the code, there is no squaring. E.g. just look at the code definition of Q in set_param_attr. I also checked that N (or R as written in the code) is R = torch.diag(measurement_cov) as defined in routine 'update'. I have verified that this is more ore less diag(cov_lat,cov_up) with some variance. Certainly is not diag(..,...)^2. I think I get it: Brossard used the noise covariances of the paper and the square of a diagonal matrix is a diagonal matrix of the same size BUT with each diagonal entry squared. In other words, he just re-defined his noise covariances.

from ai-imu-dr.