A Two-stage Particle Filter for Equality Constrained Systems

This paper is concerned with the particle filtering problem for nonlinear dynamic systems with nonlinear equality constraints. It is well-known from the literature that filters incorporating constraint information can improve the accuracy of state estimation and that any true state should always satisfy these constraints in reality. However, it is difficult to obtain the particles naturally satisfying equality constraints from the importance density function (IDF) in the sampling procedure. To this end, this paper attempts to propose a novel constrained particle filter consisting of two stages. Considering that the dynamic model plays an important part in the sampling, the first stage incorporates the current measurement and constraint information to approximate the true dynamic model uncertainty. In the second stage, to sample the constrained particles, we construct a constrained optimization function from the perspective of IDF in the filtering. The performance of the proposed two-stage particle filter is demonstrated with simulated data in a target tracking application.