Mathematically equivalent approaches for equality constrained Kalman Filtering

Kalman Filtering problems often have inherent and known constraints in the physical dynamics that are not exploited despite potentially significant gains (e.g., fixed speed of a motor). In this paper, we review existing methods and propose some new ideas for filtering in the presence of equality constraints. We then show that three methods for incorporating state space equality constraints are mathematically equivalent to the more general “Projection” method, which allows different weighting matrices when projecting the estimate. Still, the different approaches have advantages in implementations that may make one better suited than another for a given application.

💡 Research Summary

The paper investigates how to incorporate known equality constraints into the Kalman filtering framework, a topic of practical importance because many physical systems possess inherent relationships—such as fixed link lengths in robotics or constant speed limits in motor drives—that are often ignored in standard implementations. Ignoring these constraints leaves unnecessary degrees of freedom in the state estimate, leading to larger estimation errors and slower convergence.

Three historically used techniques for handling equality constraints are examined: (1) a direct state‑correction method that projects the predicted state onto the constraint manifold after the prediction step; (2) a Lagrange‑multiplier approach that augments the measurement update with additional variables to enforce the constraints; and (3) a re‑parameterisation method that defines a new reduced‑dimension state vector that inherently satisfies the constraints. The authors systematically derive the mathematical form of each technique and demonstrate that all three are special cases of a more general “Projection” method.

The general projection formula can be written as

\