Risk sensitive nonlinear optimal control with measurement uncertainty

We present an algorithm to synthesize locally-optimal feedback controllers that take into account additive process and measurement noise for nonlinear stochastic optimal control problems. The algorithm is based on an exponential performance criteria which allows to optimize not only the expected value of the cost, but also a linear combination of its higher order moments; thereby, the cost of uncertainty can be taken into account for synthesis of robust or risk-sensitive policies. The method constructs an affine feedback control law, whose gains explicitly depend upon the covariance of the estimation errors and process noise. Despite the fact that controller and observer are designed separately, the measurement noise variance enters the optimal control, therefore generating feedback laws that do not rely on the Certainty Equivalence Principle. The capabilities of the approach are illustrated in simulation on a two degree of freedom (DOF) manipulator, first in a waypoint task and then in a task where the manipulator goes in contact with its environment.