基于重磁双模检测自适应Kalman滤波的姿态估计方法

本申请涉及一种基于重磁双模检测自适应动态Kalman滤波的姿态估计方法。该方法包括：构建重磁双模检测系统的姿态解算状态方程和观测方程；对状态值和先验误差协方差矩阵进行初始化；根据重磁双模检测表征工业工具在运动过程中的噪声动态变化，对测量噪声协方差进行自适应调整，并采用kalman滤波对其进行预测、更新；根据当前时刻自适应调整的测量噪声协方差、状态和先验误差协方差矩阵的初始化值，采用kalman滤波姿态估计算法进行姿态估计。本方法通过构建重力矢量动态检测机制和磁干扰检测机制，协调两种检测机制，自适应调整噪声协方差矩阵，提升姿态估计的精度，对动态噪声特性有较强抑制效果。