Minimum-energy state estimation for systems with perspective outputs and state constraints

We address the problem of estimating the state of a system with perspective outputs, whose state is known to satisfy a set of quadratic constraints. We construct a dynamical system that produces an estimate of the state that satisfies the constraints and is "most compatible" with the dynamics, in the sense that it requires the least amount of noise energy to explain the measured output. We apply these results to the estimation of position and orientation of a controlled rigid body, using measurements from a charged-coupled-device (CCD) camera attached to the body. The main contribution of this work is the inclusion of state constraints in the minimum-energy formulation. In the context of our application, these constraints allow us to avoid singularities that previous minimum-energy controllers encountered. The results are validated experimentally by using measurements from a CCD camera mounted on a mobile robot to estimate its position and orientation. These estimates are then used to close the loop and control the robot to a desired position, defined with respect to visual landmarks. The use of state constraints in the estimator allow the system to operate even when the trajectories for the robot do not exhibit "excitation" which was needed in previous minimum-energy estimators.