Eq. 11 of the propagation step different from code (?)

[scott81321]

Eq. 11 of the propagation step in Brossard's paper (see attached clip) has:

P_{n+1} = F_n P_n F_n^T + G_n Q_n G_n^T

where F_n and G_n are Jacobian matrices.

However, when you look at the code, propagate_cov in utils_numpy_filter.py (or utils_torch_filter.py), you have instead:

    F = F * dt
    G = G * dt
    F_square = F.dot(F)
    F_cube = F_square.dot(F)
    Phi = self.IdP + F + 1/2*F_square + 1/6*F_cube
    P = Phi.dot(P_prev + G.dot(self.Q).dot(G.T)).dot(Phi.T)

Now the Phi as written is really exp(F_n * dt) expanded to the third term. I have checked that scipy.linalg.expm which exponentiates square matrices is in agreement with this expansion to within at least 16 digits for my current test runs. Thus the code for P in ai-imu-dr means:

P_{n+1} = exp (F_n dt ) ( P_n + G_n Q_n G_n^T) exp(F_n dt )^T

which is not the same thing as eq. 11 (?). Notice that the exponential part is bracketed around G_n Q_n G_n^T. I don't think this is wrong because many test cases work well.

Also according to Gilles Teodori and Hans Neuner

https://doi.org/10.1515/jag-2022-0034

their eq. 18 confirms that exp(F_n dt)( G_nQ_nG_n^T)exp(F_n dt)^T is indeed the "extension of the diagonal matrix" (the initial Q) in the EKF scheme. They quote P. Groves "Principles of GNSS, inertial, and multisensor integrated navigation systems, 2nd ed. Bosten, London: Artech House; 2013."

However, I am a bit confused. Brossard's paper claims that Q is held constant and strictly speaking it is. However the exp(F_n dt) .... exp(F_n *dt) indicates a time development of the Q matrix (?)

On a different note, if Q would be allow to vary, be optimized? what would you use? The best (and perhaps only) thing I have found in the literature is something based on the Recursive Least Squares Technique, which apparently has a connection with Kalman filters. Some references first.

1) "Kalman filtering and Information Fusion" https://link.springer.com/book/10.1007/978-981-15-0806-6

(it's a big book and I've only seen parts of it on Google books.) It does express the problem we face nicely.

2) "Kalman Filter with Recursive Covariance Estimation Revisited with Technical Conditions Reduced" by Hongbin Ma, Liping Yan, Yuanqing Xia and Mengyin Fu

https://link.springer.com/chapter/10.1007/978-981-15-0806-6_4

3) Also " A Method to Estimate the Process Noise Covariance for a Certain Class of Nonlinear Systems" by L.I. Gliga,H. Chafouk, D. Popescua and C. Lupua

Any suggestions? warnings? etc... Mind you, these references concern the EKF not IEKF per se.

[ghs2015]

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.

[scott81321]

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.

[ghs2015]

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

[scott81321]

@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.