没有卫星的时候可咋办啊！！！——AHRS的妙用（matlab代码）

前言

在卫星质量不好的情况下，可通过AHRS计算载体姿态（低动态情况下），保飞行控制内环。AHRS计算方法中，Mahony算法是一种常用的姿态解算算法，主要用于融合加速度计和陀螺仪的数据，以实现高精度的姿态估计。

一、什么是AHRS算法？

陀螺仪解算得到的姿态具有良好的高频特性，但是会随着时间漂移，而加速度计解算得到的姿态具有良好的低频特性，不会随着时间漂移，但是载体剧烈运动时，往往不能解算出真实的姿态。这时可以将陀螺仪的高频特性和加速度计的低频特性相融合，得到高频、低频特性都很好的算法。这里除了加速度计可以作为姿态量测外，磁力计、飞行速度等矢量同样可以在一些特定环境下作为量测来矫正陀螺仪的时间漂移。这种将陀螺仪和加速度计解算得到的姿态进行滤波计算得到优化姿态的目的，简称互补滤波算法。

二、AHRS算法的应用背景

一般，AHRS算法中，低精度IMU做航姿解算时没有考虑地球自转、牵连角速度等，而是将这些项全部归结到陀螺漂移中。
考虑主要有以下原因：
1）对于低精度IMU，这些项相对飞机真实运动角速度来说比较小，被淹没在陀螺的噪声和误差中；
2）小无人机的运动速度较小，飞行范围不大，飞行时间较小，因此地球自转、牵连角速度可以忽略。
姿态解算原则是通过角速度进行四元数更新（积分过程）。但由于陀螺漂移的存在，长时间积分出现漂移。航姿系统一般会利用外界磁力计、加计等信息对陀螺漂移进行补偿。而这些传感器的共同有点是，误差漂移不积累。