基于不变扩展卡尔曼滤波的无人机姿态估计

UAV Attitude Estimation Based on Invariant Extended Kalman Filter

为解决四旋翼无人机姿态估计问题,提出了一种不变扩展卡尔曼滤波(Invariant Extended Kalman Filter, InEKF)算法,用于同时估计四旋翼的姿态和惯性测量单元(Inertial Measurement Unit, IMU)的陀螺仪偏差。利用李群理论和不变观测器设计方法,将算法表述为一个连续时间随机非线性滤波器,其状态空间由直积矩阵李群SO(3)×R^(3)给出,SO(3)中的估计值由IMU对重力矢量和地球磁场向量测量值加以修正。为在无人机上实现该算法,将状态和协方差传播方程离散化。最后,所提滤波算法相对于现有算法的性能优势通过仿真实验得到了验证。

To solve the attitude estimation problem of quadrotor UA Vs,an invariant extended Kalman filter(In-EKF)algorithm is proposed for simultaneous estimation of quadrotor attitude and gyroscope bias of inertial measurement unit(IMU).Utilizing Lie group theory and invariant observer design methods,the algorithm is formulated as a continuous-time stochastic nonlinear filter,with state space given by the direct product matrix Lie group SO(3)×R^(3),and the estimated values in SO(3)are corrected by IMU measurements of the gravity vector and the Earth's magnetic field vector.To implement the algorithm on UAVs,the state and covariance propagat ion equations are discretizd.Finally,the performance advantage of the proposed filter over existing al-gorithms is verified by simulation experiments.