andreabelles
commented
3 years ago We've implemented the Lagrange interpolation mentioned in Skog and Händel (2011) to obtain the IMU measurements at the delayed instant of time. Using this interpolated accelerometer measurement, we apply the navigation equations to obtain rIMU (position, velocity, bias and time delay) at k-Td. Then, we update the state vector at k-Td and correct rIMU. Finally, we "propagate" to k-1 (not sure about it yet due to the F matrix used and whether we should be at k-1 or k), and then we predict the next rIMU at k.
jtec
What Skog et al. are proposing, how I understand it, is basically to delay the IMU outputs by as much as the GNSS is delayed, to then perform the update for a state in the past, at the time of GNSS observation.
To implement this IMU delay you can e.g. follow their reference [22] (in the literature folder on Teams), or select a more intuitive approach, such as a FIFO filter with linear interpolation in between samples - to be verified though whether that second approach is theoretically sound.
The current implementation https://github.com/andreabelles/Time-Synch/blob/0e9f025afd9d9a03953aae1ef78ba5003b9919a5/Estimation_Methods/skogEKF.m#L32L33
appears to propagate the state backwards in time to the time of GNSS observation; I see no reason not to do so, but make sure to double-check whether Skog's covariance computation still applies.