Jinya Su, Wen-Hua Chen, Baibing Li: Nonlinear state estimation with nonlinear equality constraints. CDC 2014: 322-327