The Right Invariant Nonlinear Complementary Filter for Low Cost Attitude and Heading Estimation of Platforms

Abstract: This paper presents a novel filter with low computational demand to address the problem of orientation estimation of a robotic platform. This is conventionally addressed by extended Kalman filtering of measurements from a sensor suit which mainly includes accelerometers, gyroscopes, and a digital compass. Low cost robotic platforms demand simpler and computationally more efficient methods to address this filtering problem. Hence nonlinear observers with constant gains have emerged to assume this role. The nonlinear complementary filter is a popular choice in this domain which does not require covariance matrix propagation and associated computational overhead in its filtering algorithm. However, the gain tuning procedure of the complementary filter is not optimal, where it is often hand picked by trial and error. This process is counter intuitive to system noise based tuning capability offered by a stochastic filter like the Kalman filter. This paper proposes the right invariant formulation of the complementary filter, which preserves Kalman like system noise based gain tuning capability for the filter. The resulting filter exhibits efficient operation in elementary embedded hardware, intuitive system noise based gain tuning capability and accurate attitude estimation. The performance of the filter is validated using numerical simulations and by experimentally implementing the filter on an ARDrone 2.0 micro aerial vehicle platform.