Kalman Filtering with Nonlinear State Constraints

An analytic method was developed by D. Simon and T. L. Chia to incorporate linear state equality constraints into the Kalman filter. When the state constraint was 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. 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. Computer simulation results are presented to illustrate the algorithm.