Kalman Filtering with Nonlinear State Constraints

In [Simon and Chia, 2002], an analytic method was developed to incorporate linear state equality constraints into the Kalman filter. When the state constraint is nonlinear, linearization was employed to obtain an approximately linear constraint around the current state estimate. This linearized constrained Kalman filter is subject to approximation errors and may suffer from a lack of convergence. In this paper, we present a method that allows exact use of second-order nonlinear state constraints. It is based on a computational algorithm that iteratively finds the Lagrangian multiplier for the nonlinear constraints. The method therefore provides better approximation when higher order nonlinearities are encountered. Computer simulation results are presented to illustrate the algorithm