l1 Norm Regularization Robust Attitude Smoother Using Inertial and Magnetic Sensors

Abstract

A smoother algorithm to estimate attitude using inertial and magnetic sensors is proposed, where an attitude estimation problem is formulated as an equality constrained convex optimization problem. Existing standard smoothers are local estimators, whose estimation performance critically depends on the initial estimation. This issue is avoided by formulating an attitude problem as a global optimization problem. An l1 regression term is also introduced to make the proposed method robust to sensor outliers (such as temporary magnetic disturbance or external acceleration). The optimization problem is solved through two approximation steps. In the first step, a relaxed solution is computed by relaxing equality constraints to inequality constraints. In the second step, equality constraints are recovered by searching neighborhood of a relaxed solution. Due to the approximation, the proposed algorithm is not guaranteed to provide a globally optimal solution. However, a measure of how close the computed solution is to the globally optimal solution is provided. Through simulation and experiments, the proposed method is shown to be robust to large sensor outliers: the sum of Euler angle rms errors (for 729 different simulations) by the proposed method is 2.974 (rad), while the sum by the standard smoother is 11.52 (rad).