Linear- and Linear-Matrix-Inequality-Constrained State Estimation for Nonlinear Systems

Abstract

This paper considers nonlinear state estimation subject to inequality constraints in the form of linear and linear-matrix inequalities. Rewriting the standard maximum likelihood objective function used to derive the Kalman filter allows the Kalman gain to be found by solving a constrained optimization problem with a linear objective function subject to a linear-matrix-inequality constraint. Additional constraints, such as weighted-norm- or linear-inequality constraints, that the state estimate must satisfy are easily augmented to the constrained optimization problem. The proposed constrained estimation methodology is applied in the extended Kalman filter (EKF) and sigma point Kalman filter (SPKF) frameworks. Motivated by estimation problems involving a vehicle that can rotate and translate in space, multiplicative versions of the constrained EKF and SPKF formulations are discussed. Simulation results for a ground-based mobile robot operating in a constrained three-dimensional terrain are presented and are compared to results that use the traditional multiplicative EKF and SPKF, as well as filters that enforce inequality constraints by simply projecting the state estimate into the constrained domain along the shortest Euclidean distance.