Inequality Constrained Kalman Filtering for the Localization and Registration of a Surgical Robot

We present a novel method for enforcing nonlinear inequality constraints in the estimation of a high degree of freedom robotic system within a Kalman filter. Our constrained Kalman filtering technique is based on a new concept, which we call uncertainty projection, that projects the portion of the uncertainty ellipsoid that does not satisfy the constraint onto the constraint surface. A new PDF is then generated with an efficient update procedure that is guaranteed to reduce the uncertainty of the system. The application we have targeted for this work is the localization and automatic registration of a robotic surgical probe relative to preoperative images during image-guided surgery. We demonstrate the feasibility of our constrained filtering approach with data collected from an experiment involving a surgical robot navigating on the epicardial surface of a porcine heart.