-
Notifications
You must be signed in to change notification settings - Fork 226
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Eq. 11 of the propagation step different from code (?) #81
Comments
Hi Tony, @scott81321 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 In the code, F means F(t), and Phi corresponds to "F_n" in Eq 11. I am still trying to clarify other questions. |
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. |
@scott81321 I guess we can reduce eq. 3.7.10 or eq. 11 to the coded one following |
@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. |
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:
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_n*G_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.
"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.
"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
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.
The text was updated successfully, but these errors were encountered: