A nonlinear complementary filter for attitude estimation with dynamics compensation of MARG sensor

摘要

A novel complementary filter for three-dimensional (3D) attitude estimation is proposed. In order to deals with the nonlinearity of 3D attitude, the conventional complementary filter integrated the measured angular velocity corrected by the vector measurement, but it ignored the sensor dynamics which causes the lag. Authors' previous work compensated the sensor dynamics based on the idea that it can be divided into the linear transfer function and the nonlinear coordinate transformation. The properness and stability of the total transfer function are guaranteed by the designed filter inserted between the linear and nonlinear part. However, the disturbance strongly affects the accuracy of the azimuth angle due to the coordinate transformation. This paper aims to improve the estimation accuracy by combining the idea of correcting the measured angular velocity by the vector measurement and the sensor dynamics compensation only with the proper and stable transfer function. The characteristics of the proposed method are the following two points: 1) the relaxation of the complementary condition in order to make the non-proper and unstable function proper and stable, and 2) the filtering of the estimated vector by the sensor dynamics instead of the filtering of the measured vector by the inverse dynamics in order to avoid the magnification of the high frequency noise. Through the experiment for the fast and irregular motion under 5[Hz], the validity of the proposed method is verified.