jtec
commented
3 years ago Hi guys, I'll have a look at your code as soon as I have the opportunity.
A debugging strategy I have been using in my work when a state converges to a biased value: set all the noise sources to very small values (1e-6 ...) and have a look at the innovations - when a state has converged, it indicates that innovations (or the corresponding elements of the Kalman gain matrix - so make sure to check both) go to zero, which should not be the case if the state is biased.
So removing all noise form the system allows you to debug the deterministic part of the filter, where something seems to be off somewhere.
We implemented a new version of Skog's method in which we followed the procedure as it is described in the paper from 2011 (with no jumps in time so everything is delayed). The only thing we're doing different is the order of the prediction and update, which we do in the "classical" way and not in the way described in Table 1 from the paper. Nonetheless, it should be the same since they are applying the augmented state transition and measurement models to the classical EKF (from the paragraph below Eq. 23 in the paper from 2011). This version is in branch feature/testSkog1D (commit ID a757d315ea4934e6ee8a437784b03d13ecf7f711). With this, we obtain the following results, which aren't that bad but the estimation of the delay does not converge to the true value. We've been trying to tune the initialization parameters but it does not seem to converge anyway. Despite that, whenever the time delay estimation is closer to the actual delay, the RMSE of the Skog estimation is slightly lower than the standard EKF (differences of the order of 0.1 m).