An efficient SQP algorithm for Moving Horizon Estimation with Huber penalties and multi-rate measurements
Dimitris Kouzoupis, Rien Quirynen, Fabian Girrbach, Moritz Diehl · 2016
Moving Horizon Estimation (MHE) is a powerful, yet computationally expensive approach for state and parameter estimation that is based on online optimization. In applications with multi-rate measurements that may include outliers, the Huber penalty is often a better candidate for the MHE objective than the commonly used Euclidean norm. Treating this non-smooth objective in Newton-type optimization typically requires the use of slack variables that would in turn increase the problem size significantly. As an alternative, we propose a novel algorithm that combines Sequential Convex Programming (SCP) and Sequential Quadratic Programming (SQP) techniques in an effort to reduce the computational complexity. The proposed implementation is tailored to embedded applications, as it combines state-of-the-art numerical tools and efficient C code. We demonstrate the performance of the algorithm on a real-world state estimation problem where the position and orientation of a single propeller aircraft are estimated using GPS and IMU measurement data.