Hybrid Iterative Linear Quadratic Estimation: Optimal Estimation for Hybrid Systems

In this letter we present Hybrid iterative Linear Quadratic Estimation (HiLQE), an optimization based offline state estimation algorithm for hybrid dynamical systems. We utilize the saltation matrix, a first order approximation of the variational update through an event driven hybrid transition, to calculate gradient information through hybrid events in the backward pass of an iterative linear quadratic optimization over state estimates. This enables accurate computation of the value function approximation at each timestep. Additionally, the forward pass in the iterative algorithm is augmented with hybrid dynamics in the rollout. A reference extension method is used to account for varying impact times when comparing states for the feedback gain in noise calculation. The proposed method is demonstrated on an ASLIP hopper system with position measurements. In comparison to the Salted Kalman Filter (SKF), the algorithm presented here achieves a maximum of 63.55% reduction in estimation error magnitude over all state dimensions near impact events.

I. Introduction

Contact is essential for many robots as they must physically interact with their environment to accomplish their goals. For example, legged robots must repeatedly make and break contact with the ground to move around and perform their tasks, whether that is mapping, surveying, search and rescue, or any other task. Similarly, manipulation robots must make contact with the objects they seek to manipulate and their environment in order to push, pull, grasp, etc., those objects. To plan reliable control sequences through contact, robots need an accurate state estimate. However, intermittent contacts cause robots' dynamics to become non-smooth and potentially discontinuous, breaking the assumptions of classical methods of state estimation which assume smoothness [1], [2], [3], [4], [5].