一种基于mahony改进的姿态解算方法及系统

本发明属于但不限于惯性导航与姿态测量技术领域，公开了一种基于mahony改进的姿态解算方法及系统，初始化第一四元数A和第二四元数B；采集陀螺仪角速度数据及加速度计加速度数据；使用比例增益K和积分增益K生成补偿量，将补偿量加到角速度数据，并更新四元数A；采用未经补偿的角速度数据，更新四元数B；统计连续N帧加速度模长方差，当方差小于阈值T时判定装置静止，计算误差四元数E；构建补偿四元数Dt，将Dt左乘四元数B并归一化；利用四元数B求得横滚角、俯仰角和航向角并输出。本发明在应用中能够保持低噪声和快速误差抑制：动态时不被线加速度干扰，静止时又能迅速贴合姿态基准，实现了高动态稳定性和长时低漂移。