Stacked iterated posterior linearization filter

The Kalman Filter (KF) is a classical algorithm that was developed for estimating a state that evolves in time based on noisy measurements by assuming linear state transition and measurements models. There exist various KF extensions for non-linear situations, but they are not exact and provide different linearization errors. The Iterated Posterior Linearization Filter (IPLF) does the linearizations iteratively to achieve better linearizations. However, it is possible that some measurements cannot be well linearized using the current knowledge, but their linearization may be better after more measurements are available. Thus, we propose an algorithm that can store the older state elements and measurements when their linearization error is high. The resulting algorithm, the Stacked Iterated Posterior Linearization Filter (S-IPLF), is based on linear dynamic models and uses information from multiple time instances to make the linearization of the measurement function. Results show that the proposed algorithm outperforms traditional KF extensions when some of the measurements cannot be well linearized with the current knowledge, but can be when future information is available.