A smoothly constrained kaiman filter

This paper presents the Smoothly Constrained Kaiman Filter (SCKF) for nonlinear constraints. A constraint is any relation that exists between the state variables. Constraints can be treated as perfect observations. But, linearization errors can prevent the estimate from converging to the true value. Therefore, the SCKF iteratively applies nonlinear constraints as nearly perfect observations, or, equivalently, weakened constraints. Integration of new measurements is interlaced with these iterations, which reduces linearization errors and, hence, improves convergence compared to other iterative methods. The weakening is achieved by artificially increasing the variance of the nonlinear constraint. The paper explains how to choose the weakening values, and when to start and stop the iterative application of the constraint.