Converted Measurement Kalman filter with nonlinear equality constrains

For nonlinear systems, Converted Measurement Kalman filter as one of various modifications of the Kalman filter can be used to estimate the state with the non-linear measuring equations, effectively. Although the Converted Measurement Kalman filter is powerful tools for nonlinear state estimation, we might have information about a system that the Converted Measurement Kalman filter does not incorporate. For example, we may know that the states satisfy equality or inequality constraints. In this paper we modify the Converted Measurement Kalman filter to exploit this additional information. A target tracking example is presented to illustrate the effectiveness of Converted Measurement Kalman filter with constraints, which gets better filtering performance than the unstrained Converted Measurement Kalman filter provides. Simulation results between first-order and second-order nonlinear state constraints also show that the second-order solution for higher order nonlinearity as present in this paper outperforms the first-order solution.