On Kalman Filtering With Nonlinear Equality Constraints

The state space description of some physical systems possess nonlinear equality constraints between some state variables. In this paper, we consider the problem of applying a Kalman filter-type estimator in the presence of such constraints. We categorize previous approaches into pseudo-observation and projection methods and identify two types of constraints-those that act on the entire distribution and those that act on the mean of the distribution. We argue that the pseudo-observation approach enforces neither type of constraint and that the projection method enforces the first type of constraint only. We propose a new method that utilizes the projection method twice-once to constrain the entire distribution and once to constrain the statistics of the distribution. We illustrate these algorithms in a tracking system that uses unit quaternions to encode orientation